def optimize_path(source, target, grid_map, yips_path, debug=False):
rrt_star = BiRRTStar().set_vehicle(contour(), 0.3, 0.20)
heuristic = yips_path_to_heuristic(yips_path)
ori = carla_transform_to_node(source)
start = center2rear(carla_transform_to_node(source)).gcs2lcs(ori.state)
goal = center2rear(carla_transform_to_node(target)).gcs2lcs(ori.state)
grid_ori, grid_res = deepcopy(ori).gcs2lcs(ori.state), 0.1
if debug:
set_plot(debug)
Debugger.plot_grid(grid_map, grid_res)
Debugger().plot_nodes([start, goal])
plt.gca().add_patch(Polygon(
transform(contour().transpose(), start.state).transpose(), True, color='b', fill=False, lw=2.0))
plt.gca().add_patch(Polygon(
transform(contour().transpose(), goal.state).transpose(), True, color='g', fill=False, lw=2.0))
if heuristic:
Debugger.plot_heuristic(heuristic)
plt.draw()
rrt_star.debug = debug
rrt_star.preset(start, goal, grid_map, grid_res, grid_ori, 255, heuristic).planning(500)
while not rrt_star.x_best.fu < np.inf:
logging.warning('Warning, Hard Problem')
rrt_star.preset(start, goal, grid_map, grid_res, grid_ori, 255, heuristic).planning(500*4)
tj = rrt_star.trajectory(a_cc=3, v_max=10, res=0.1)
plt.plot([t.state[0] for t in tj], [t.state[1] for t in tj])
plt.scatter([t.state[0] for t in tj], [t.state[1] for t in tj], c=[t.k for t in tj], s=50)
# LCS to GCS
tj = [t.lcs2gcs(ori.state) for t in tj]
motion = [(t.state[0], t.state[1], t.state[2], t.k, t.v) for t in tj]
Debugger.breaker('Plotting', switch=debug)
return motion, [tuple(p.state) for p in rrt_star.path()]
def exist(state):
def key(y):
dxy = np.fabs(y.state[:-1] - s[:-1])
da = ((y.state[-1] + np.pi) % (2 * np.pi) - np.pi) - ((s[-1] + np.pi) % (2 * np.pi) - np.pi)
return dxy[0] < self.exist_res and dxy[1] < self.exist_res and da < self.exist_res
s = np.array(state)
result = filter(key, self.vertices)
Debugger.breaker('sample exclusive: {}'.format(result == []), switch=self.debug)
return result
def swap(self, i):
self.branch_and_bound(self.g_vertices)
self.branch_and_bound(self.s_vertices)
if self.root is self.start:
self.root = self.goal
self.gain = self.start
self.vertices = self.g_vertices
n = -((i/2) % len(self.heuristic)) - 1 if self.heuristic else i
Debugger.breaker('swap: goal -> start, {}, {}'.format(i, n), self.debug)
return n
else:
self.root = self.start
self.gain = self.goal
self.vertices = self.s_vertices
n = (i/2) % len(self.heuristic) if self.heuristic else i
Debugger.breaker('swap: start -> goal, {}, {}'.format(i, n), self.debug)
return n
def benefit(self, x_new):
words = 'Benefit: {}/ ({}, {})'.format(x_new.fl <= self.x_best.fu, x_new.fl, self.x_best.fu)
Debugger.breaker(words, switch=self.debug)
return x_new.fl < self.x_best.fu