From 6f0d9bfd5c6cd74bbd65891570568652c0160690 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 24 Feb 2020 16:15:17 +0900 Subject: [PATCH] clean up voronoi road map --- .../VoronoiRoadMap/dijkstra_search.py | 106 +++++++ PathPlanning/VoronoiRoadMap/kdtree.py | 49 +++ .../VoronoiRoadMap/voronoi_road_map.py | 294 +++++------------- 3 files changed, 239 insertions(+), 210 deletions(-) create mode 100644 PathPlanning/VoronoiRoadMap/dijkstra_search.py create mode 100644 PathPlanning/VoronoiRoadMap/kdtree.py diff --git a/PathPlanning/VoronoiRoadMap/dijkstra_search.py b/PathPlanning/VoronoiRoadMap/dijkstra_search.py new file mode 100644 index 00000000..4d294ee5 --- /dev/null +++ b/PathPlanning/VoronoiRoadMap/dijkstra_search.py @@ -0,0 +1,106 @@ +""" + +Dijkstra Search library + +author: Atsushi Sakai (@Atsushi_twi) + +""" + +import matplotlib.pyplot as plt +import math + + +class DijkstraSearch: + + class Node: + """ + Node class for dijkstra search + """ + + def __init__(self, x, y, cost, parent): + self.x = x + self.y = y + self.cost = cost + self.parent = parent + + def __str__(self): + return str(self.x) + "," + str(self.y) + "," + str( + self.cost) + "," + str(self.parent) + + def __init__(self, show_animation): + self.show_animation = show_animation + + def search(self, sx, sy, gx, gy, road_map, sample_x, sample_y): + """ + gx: goal x position [m] + gx: goal x position [m] + ox: x position list of Obstacles [m] + oy: y position list of Obstacles [m] + reso: grid resolution [m] + rr: robot radius[m] + """ + + start_node = self.Node(sx, sy, 0.0, -1) + goal_node = self.Node(gx, gy, 0.0, -1) + + open_set, close_set = dict(), dict() + open_set[len(road_map) - 2] = start_node + + while True: + if not open_set: + print("Cannot find path") + break + + c_id = min(open_set, key=lambda o: open_set[o].cost) + current = open_set[c_id] + + # show graph + if self.show_animation and len( + close_set.keys()) % 2 == 0: # pragma: no cover + plt.plot(current.x, current.y, "xg") + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.pause(0.001) + + if c_id == (len(road_map) - 1): + print("goal is found!") + goal_node.parent = current.parent + goal_node.cost = current.cost + break + + # Remove the item from the open set + del open_set[c_id] + # Add it to the closed set + close_set[c_id] = current + + # expand search grid based on motion model + for i in range(len(road_map[c_id])): + n_id = road_map[c_id][i] + dx = sample_x[n_id] - current.x + dy = sample_y[n_id] - current.y + d = math.hypot(dx, dy) + node = self.Node(sample_x[n_id], sample_y[n_id], + current.cost + d, c_id) + + if n_id in close_set: + continue + # Otherwise if it is already in the open set + if n_id in open_set: + if open_set[n_id].cost > node.cost: + open_set[n_id].cost = node.cost + open_set[n_id].parent = c_id + else: + open_set[n_id] = node + + # generate final course + rx, ry = [goal_node.x], [goal_node.y] + parent = goal_node.parent + while parent != -1: + n = close_set[parent] + rx.append(n.x) + ry.append(n.y) + parent = n.parent + + return rx, ry diff --git a/PathPlanning/VoronoiRoadMap/kdtree.py b/PathPlanning/VoronoiRoadMap/kdtree.py new file mode 100644 index 00000000..d3dc01b1 --- /dev/null +++ b/PathPlanning/VoronoiRoadMap/kdtree.py @@ -0,0 +1,49 @@ +""" + +Kd tree Search library + +author: Atsushi Sakai (@Atsushi_twi) + +""" + +import scipy.spatial + + +class KDTree: + """ + Nearest neighbor search class with KDTree + """ + + def __init__(self, data): + # store kd-tree + self.tree = scipy.spatial.cKDTree(data) + + def search(self, inp, k=1): + """ + Search NN + + inp: input data, single frame or multi frame + + """ + + if len(inp.shape) >= 2: # multi input + index = [] + dist = [] + + for i in inp.T: + idist, iindex = self.tree.query(i, k=k) + index.append(iindex) + dist.append(idist) + + return index, dist + + dist, index = self.tree.query(inp, k=k) + return index, dist + + def search_in_distance(self, inp, r): + """ + find points with in a distance r + """ + + index = self.tree.query_ball_point(inp, r) + return index diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 6db4bb26..dfac7258 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -10,252 +10,124 @@ import math import numpy as np import scipy.spatial import matplotlib.pyplot as plt - -# parameter -N_KNN = 10 # number of edge from one sampled point -MAX_EDGE_LEN = 30.0 # [m] Maximum edge length +from dijkstra_search import DijkstraSearch +from kdtree import KDTree show_animation = True -class Node: - """ - Node class for dijkstra search - """ +class VoronoiRoadMapPlanner: - def __init__(self, x, y, cost, pind): - self.x = x - self.y = y - self.cost = cost - self.pind = pind + def __init__(self): + # parameter + self.N_KNN = 10 # number of edge from one sampled point + self.MAX_EDGE_LEN = 30.0 # [m] Maximum edge length - def __str__(self): - return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) + def planning(self, sx, sy, gx, gy, ox, oy, rr): + obstacle_tree = KDTree(np.vstack((ox, oy)).T) + sample_x, sample_y = self.voronoi_sampling(sx, sy, gx, gy, ox, oy) + if show_animation: # pragma: no cover + plt.plot(sample_x, sample_y, ".b") -class KDTree: - """ - Nearest neighbor search class with KDTree - """ + road_map = self.generate_road_map(sample_x, sample_y, rr, obstacle_tree) - def __init__(self, data): - # store kd-tree - self.tree = scipy.spatial.cKDTree(data) + rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy, road_map, + sample_x, sample_y) - def search(self, inp, k=1): - """ - Search NN + return rx, ry - inp: input data, single frame or multi frame + def is_collision(self, sx, sy, gx, gy, rr, obstacle_kdtree): + x = sx + y = sy + dx = gx - sx + dy = gy - sy + yaw = math.atan2(gy - sy, gx - sx) + d = math.hypot(dx, dy) - """ + if d >= self.MAX_EDGE_LEN: + return True - if len(inp.shape) >= 2: # multi input - index = [] - dist = [] + D = rr + n_step = round(d / D) - for i in inp.T: - idist, iindex = self.tree.query(i, k=k) - index.append(iindex) - dist.append(idist) + for i in range(n_step): + ids, dist = obstacle_kdtree.search(np.array([x, y]).reshape(2, 1)) + if dist[0] <= rr: + return True # collision + x += D * math.cos(yaw) + y += D * math.sin(yaw) - return index, dist - - dist, index = self.tree.query(inp, k=k) - return index, dist - - def search_in_distance(self, inp, r): - """ - find points with in a distance r - """ - - index = self.tree.query_ball_point(inp, r) - return index - - -def VRM_planning(sx, sy, gx, gy, ox, oy, rr): - - obkdtree = KDTree(np.vstack((ox, oy)).T) - - sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree) - if show_animation: # pragma: no cover - plt.plot(sample_x, sample_y, ".b") - - road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree) - - rx, ry = dijkstra_planning( - sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y) - - return rx, ry - - -def is_collision(sx, sy, gx, gy, rr, okdtree): - x = sx - y = sy - dx = gx - sx - dy = gy - sy - yaw = math.atan2(gy - sy, gx - sx) - d = math.hypot(dx, dy) - - if d >= MAX_EDGE_LEN: - return True - - D = rr - nstep = round(d / D) - - for i in range(nstep): - idxs, dist = okdtree.search(np.array([x, y]).reshape(2, 1)) + # goal point check + ids, dist = obstacle_kdtree.search(np.array([gx, gy]).reshape(2, 1)) if dist[0] <= rr: return True # collision - x += D * math.cos(yaw) - y += D * math.sin(yaw) - # goal point check - idxs, dist = okdtree.search(np.array([gx, gy]).reshape(2, 1)) - if dist[0] <= rr: - return True # collision + return False # OK - return False # OK + def generate_road_map(self, node_x, node_y, rr, obstacle_tree): + """ + Road map generation + sample_x: [m] x positions of sampled points + sample_y: [m] y positions of sampled points + rr: Robot Radius[m] + obstacle_tree: KDTree object of obstacles + """ -def generate_roadmap(sample_x, sample_y, rr, obkdtree): - """ - Road map generation + road_map = [] + n_sample = len(node_x) + node_tree = KDTree(np.vstack((node_x, node_y)).T) - sample_x: [m] x positions of sampled points - sample_y: [m] y positions of sampled points - rr: Robot Radius[m] - obkdtree: KDTree object of obstacles - """ + for (i, ix, iy) in zip(range(n_sample), node_x, node_y): - road_map = [] - nsample = len(sample_x) - skdtree = KDTree(np.vstack((sample_x, sample_y)).T) + index, dists = node_tree.search( + np.array([ix, iy]).reshape(2, 1), k=n_sample) - for (i, ix, iy) in zip(range(nsample), sample_x, sample_y): + inds = index[0] + edge_id = [] - index, dists = skdtree.search( - np.array([ix, iy]).reshape(2, 1), k=nsample) + for ii in range(1, len(inds)): + nx = node_x[inds[ii]] + ny = node_y[inds[ii]] - inds = index[0] - edge_id = [] - # print(index) + if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree): + edge_id.append(inds[ii]) - for ii in range(1, len(inds)): - nx = sample_x[inds[ii]] - ny = sample_y[inds[ii]] + if len(edge_id) >= self.N_KNN: + break - if not is_collision(ix, iy, nx, ny, rr, obkdtree): - edge_id.append(inds[ii]) + road_map.append(edge_id) - if len(edge_id) >= N_KNN: - break + # plot_road_map(road_map, sample_x, sample_y) - road_map.append(edge_id) + return road_map - # plot_road_map(road_map, sample_x, sample_y) + @staticmethod + def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover - return road_map + for i, _ in enumerate(road_map): + for ii in range(len(road_map[i])): + ind = road_map[i][ii] + plt.plot([sample_x[i], sample_x[ind]], + [sample_y[i], sample_y[ind]], "-k") -def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): - """ - gx: goal x position [m] - gx: goal x position [m] - ox: x position list of Obstacles [m] - oy: y position list of Obstacles [m] - reso: grid resolution [m] - rr: robot radius[m] - """ + @staticmethod + def voronoi_sampling(sx, sy, gx, gy, ox, oy): + oxy = np.vstack((ox, oy)).T - nstart = Node(sx, sy, 0.0, -1) - ngoal = Node(gx, gy, 0.0, -1) + # generate voronoi point + vor = scipy.spatial.Voronoi(oxy) + sample_x = [ix for [ix, _] in vor.vertices] + sample_y = [iy for [_, iy] in vor.vertices] - openset, closedset = dict(), dict() - openset[len(road_map) - 2] = nstart + sample_x.append(sx) + sample_y.append(sy) + sample_x.append(gx) + sample_y.append(gy) - while True: - if not openset: - print("Cannot find path") - break - - c_id = min(openset, key=lambda o: openset[o].cost) - current = openset[c_id] - - # show graph - if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover - plt.plot(current.x, current.y, "xg") - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - plt.pause(0.001) - - if c_id == (len(road_map) - 1): - print("goal is found!") - ngoal.pind = current.pind - ngoal.cost = current.cost - break - - # Remove the item from the open set - del openset[c_id] - # Add it to the closed set - closedset[c_id] = current - - # expand search grid based on motion model - for i in range(len(road_map[c_id])): - n_id = road_map[c_id][i] - dx = sample_x[n_id] - current.x - dy = sample_y[n_id] - current.y - d = math.hypot(dx, dy) - node = Node(sample_x[n_id], sample_y[n_id], - current.cost + d, c_id) - - if n_id in closedset: - continue - # Otherwise if it is already in the open set - if n_id in openset: - if openset[n_id].cost > node.cost: - openset[n_id].cost = node.cost - openset[n_id].pind = c_id - else: - openset[n_id] = node - - # generate final course - rx, ry = [ngoal.x], [ngoal.y] - pind = ngoal.pind - while pind != -1: - n = closedset[pind] - rx.append(n.x) - ry.append(n.y) - pind = n.pind - - return rx, ry - - -def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover - - for i, _ in enumerate(road_map): - for ii in range(len(road_map[i])): - ind = road_map[i][ii] - - plt.plot([sample_x[i], sample_x[ind]], - [sample_y[i], sample_y[ind]], "-k") - - -def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree): - oxy = np.vstack((ox, oy)).T - - # generate voronoi point - vor = scipy.spatial.Voronoi(oxy) - sample_x = [ix for [ix, iy] in vor.vertices] - sample_y = [iy for [ix, iy] in vor.vertices] - - sample_x.append(sx) - sample_y.append(sy) - sample_x.append(gx) - sample_y.append(gy) - - return sample_x, sample_y + return sample_x, sample_y def main(): @@ -297,12 +169,14 @@ def main(): plt.grid(True) plt.axis("equal") - rx, ry = VRM_planning(sx, sy, gx, gy, ox, oy, robot_size) + rx, ry = VoronoiRoadMapPlanner().planning(sx, sy, gx, gy, ox, oy, + robot_size) assert rx, 'Cannot found path' if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") + plt.pause(0.1) plt.show()