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 planning(self, times, repeat=10, optimize=False, debug=False):
"""main flow."""
self.debug = debug
past = time.time()
for i in range(times):
x_new = self.sample_free(i, repeat)
x_nearest = self.nearest(x_new) if not optimize else self.least(x_new)
if x_nearest and self.benefit(x_new) and self.collision_free(x_nearest, x_new):
self.attach(x_nearest, x_new)
self.rewire(x_new)
self.x_best = self.best()
self.branch_and_bound()
Debugger().debug_planned_path(self, i, switch=self.debug)
Debugger().debug_planning_hist(self, i, (time.time() - past), switch=True)
def recheck(x):
available, cost = self.collision_free(x_new, x), self.cost(x_new, x)
if available and x.g > x_new.g + cost:
Debugger().debug_rewiring(x, x_new.g + cost, switch=self.debug)
x.g = x_new.g + cost
x.fu, x.fl = x.g + x.hu, x.g + x.hl
x.rematch(x_new)
def is_free(state):
contour = self.transform(self.check_poly, state[0], state[1], state[2])
contour = np.floor(contour / self.grid_res + self.grid_map.shape[0] / 2.).astype(int)
mask = np.zeros_like(self.grid_map, dtype=np.uint8)
cv2.fillPoly(mask, [contour], 255)
result = np.bitwise_and(mask, self.grid_map)
Debugger().breaker('sample free: {}'.format(np.all(result < self.obstacle)), self.debug)
return np.all(result < self.obstacle)
def branch_and_bound(self, space=None):
def out(x):
vertices.remove(x)
x.remove()
vertices = space if space else self.vertices
vs = filter(lambda x: x.fl > self.x_best.fu + self.epsilon, vertices)
map(out, vs)
Debugger().debug_branch_and_bound(vs, switch=self.debug)
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 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 attach(self, x_nearest, x_new): # type: (StateNode, StateNode) -> None
"""add the new state to the tree and complement other values.
And fill the hu and fu properties of x_new.
"""
x_new.match(x_nearest)
available = self.collision_free(x_new, self.gain)
x_new.hu = x_new.hl if available else np.inf
x_new.fu = x_new.g + x_new.hu
x_new.status = 0 if available else 1
self.vertices.append(x_new)
Debugger().debug_attaching(x_nearest, x_new, 1. / self.maximum_curvature, switch=self.debug)
def sample_free(self, n, repeat=10, default=((2., .5), (0., np.pi / 4.), (0, np.pi / 6.))):
"""sample a state from free configuration space."""
def is_free(state):
contour = self.transform(self.check_poly, state[0], state[1], state[2])
contour = np.floor(contour / self.grid_res + self.grid_map.shape[0] / 2.).astype(int)
mask = np.zeros_like(self.grid_map, dtype=np.uint8)
cv2.fillPoly(mask, [contour], 255)
result = np.bitwise_and(mask, self.grid_map)
Debugger().breaker('sample free: {}'.format(np.all(result < self.obstacle)), self.debug)
return np.all(result < self.obstacle)
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 emerge():
if self.heuristic:
i = n % len(self.heuristic)
state, biasing = self.heuristic[int(i)]
rand = [state[0], state[1], state[2]] # [x_o, y_o, a_o]
(x_mu, x_sigma), (y_mu, y_sigma), (a_mu, a_sigma) = biasing
rand[0] += np.random.normal(x_mu, x_sigma)
rand[1] += np.random.normal(y_mu, y_sigma)
rand[2] += np.random.normal(a_mu, a_sigma)
return rand
else:
Debugger().debug_no_heuristic(vertex.state, default, self.debug)
rand = [vertex.state[0], vertex.state[1], vertex.state[2]]
(r_mu, r_sigma), (t_mu, t_sigma), (a_mu, a_sigma) = default
r, theta = np.random.normal(r_mu, r_sigma), np.random.normal(t_mu, t_sigma) + rand[2]
rand[0] += r * np.cos(theta)
rand[1] += r * np.sin(theta)
rand[2] += np.random.normal(a_mu, a_sigma)
return rand
vertex = np.random.choice(self.vertices)
for i in range(repeat):
x_rand = emerge()
Debugger().debug_sampling(x_rand, self.check_poly, switch=self.debug)
if is_free(x_rand):
if not exist(x_rand):
return self.StateNode(tuple(x_rand))
return self.StateNode(tuple(x_rand))
def rewire(self, x_new, gamma=0.2): # type: (StateNode, float) -> None
"""rewiring tree by the new state."""
def recheck(x):
available, cost = self.collision_free(x_new, x), self.cost(x_new, x)
if available and x.g > x_new.g + cost:
Debugger().debug_rewiring(x, x_new.g + cost, switch=self.debug)
x.g = x_new.g + cost
x.fu, x.fl = x.g + x.hu, x.g + x.hl
x.rematch(x_new)
xs = filter(lambda x: x.g > x_new.g + gamma, self.vertices)
Debugger().debug_rewiring_check(xs, x_new, switch=self.debug)
map(recheck, xs)
def collision_free(self, x_from, x_to): # type: (StateNode, StateNode) -> bool
"""check if the path from one state to another state collides with any obstacles or not."""
# making contours of the curve
states = reeds_shepp.path_sample(x_from.state, x_to.state, 1. / self.maximum_curvature, 0.3)
# states.append(tuple(x_to.state)) # include the end point
contours = [self.transform(self.check_poly, s[0], s[1], s[2]) for s in states]
contours = [np.floor(con / self.grid_res + self.grid_map.shape[0] / 2.).astype(int) for con in contours]
# making mask
mask = np.zeros_like(self.grid_map, dtype=np.uint8)
[cv2.fillPoly(mask, [con], 255) for con in contours]
# checking
result = np.bitwise_and(mask, self.grid_map)
Debugger().debug_collision_checking(states, self.check_poly, np.all(result < self.obstacle), switch=self.debug)
return np.all(result < self.obstacle)
def connect_graphs(self, x_new):
if self.root is self.start:
vs = self.g_vertices
else:
vs = self.s_vertices
costs = list(map(lambda x: self.cost(x_new, x), vs))
x_nearest, cost = vs[int(np.argmin(costs))], np.min(costs)
Debugger().debug_connect_graphs(x_nearest.state, x_new.g+cost+x_nearest.g, self.x_best.fu, switch=self.debug)
if x_new.g + cost + x_nearest.g < self.x_best.fu:
if self.collision_free(x_new, x_nearest):
x_nearest.fu = x_new.fu = x_new.g + cost + x_nearest.g
x_new.hu = x_new.fu - x_new.g
x_nearest.hu = x_nearest.fu - x_nearest.g
x_new.neighbor = x_nearest
x_nearest.neighbor = x_new
self.x_best = x_new
def nearest(self, x_rand): # type: (StateNode) -> StateNode
"""find the state in the tree which is nearest to the sampled state.
And fill the g, hl and fl properties of the sampled state.
"""
def replenish(x_n, x_r):
x_r.g, x_r.hl = x_n.g + self.cost(x_n, x_r), self.cost(x_r, self.gain)
x_r.fl = x_r.g + x_r.hl
# quick shot
if self.collision_free(self.root, x_rand):
x_nearest = self.root
else:
costs = list(map(lambda x: self.cost(x, x_rand), self.vertices))
x_nearest = self.vertices[int(np.argmin(costs))]
replenish(x_nearest, x_rand)
Debugger().debug_nearest_searching(x_nearest.state, switch=self.debug)
return x_nearest
def least(self, x_rand): # type: (StateNode) -> StateNode
def replenish(x_n, x_r):
x_r.g, x_r.hl = x_n.g + self.cost(x_n, x_r), self.cost(x_r, self.gain)
x_r.fl = x_r.g + x_r.hl
# quick shot
if self.collision_free(self.root, x_rand):
x_least = self.root
else:
nodes = filter(lambda x: self.collision_free(x, x_rand), self.vertices)
if nodes:
costs = list(map(lambda x: x.g + self.cost(x, x_rand), nodes))
x_least = nodes[int(np.argmin(costs))]
else:
x_least = None
if x_least:
replenish(x_least, x_rand)
Debugger().debug_nearest_searching(x_least.state, switch=self.debug)
return x_least
def emerge():
if self.heuristic:
i = n % len(self.heuristic)
state, biasing = self.heuristic[int(i)]
rand = [state[0], state[1], state[2]] # [x_o, y_o, a_o]
(x_mu, x_sigma), (y_mu, y_sigma), (a_mu, a_sigma) = biasing
rand[0] += np.random.normal(x_mu, x_sigma)
rand[1] += np.random.normal(y_mu, y_sigma)
rand[2] += np.random.normal(a_mu, a_sigma)
return rand
else:
Debugger().debug_no_heuristic(vertex.state, default, self.debug)
rand = [vertex.state[0], vertex.state[1], vertex.state[2]]
(r_mu, r_sigma), (t_mu, t_sigma), (a_mu, a_sigma) = default
r, theta = np.random.normal(r_mu, r_sigma), np.random.normal(t_mu, t_sigma) + rand[2]
rand[0] += r * np.cos(theta)
rand[1] += r * np.sin(theta)
rand[2] += np.random.normal(a_mu, a_sigma)
return rand
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