From d0d4f1116e70987b8ab251a137ae3188bd464c9d Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 10 Apr 2020 15:07:21 +0200 Subject: [PATCH 1/7] add Depth-First Search --- PathPlanning/DepthFirstSearch/dfs.py | 273 +++++++++++++++++++++++++++ 1 file changed, 273 insertions(+) create mode 100644 PathPlanning/DepthFirstSearch/dfs.py diff --git a/PathPlanning/DepthFirstSearch/dfs.py b/PathPlanning/DepthFirstSearch/dfs.py new file mode 100644 index 00000000..4ceb0e2b --- /dev/null +++ b/PathPlanning/DepthFirstSearch/dfs.py @@ -0,0 +1,273 @@ +""" + +Depth-First grid planning + +author: Erwin Lejeune (@spida_rwin) + +See Wikipedia article (https://en.wikipedia.org/wiki/depth-First_search) + +""" + +import math + +import matplotlib.pyplot as plt + +import random + +show_animation = True + + +class DFSPlanner: + + def __init__(self, ox, oy, reso, rr): + """ + Initialize grid map for DFS planning + + ox: x position list of Obstacles [m] + oy: y position list of Obstacles [m] + reso: grid resolution [m] + rr: robot radius[m] + """ + + self.reso = reso + self.rr = rr + self.calc_obstacle_map(ox, oy) + self.motion = self.get_motion_model() + + class Node: + def __init__(self, x, y, cost, pind, parent): + self.x = x # index of grid + self.y = y # index of grid + self.cost = cost + self.pind = pind + self.parent = parent + + def __str__(self): + return str(self.x) + "," + str(self.y) + "," + str( + self.cost) + "," + str(self.pind) + + def planning(self, sx, sy, gx, gy): + """ + Depth First search + + input: + sx: start x position [m] + sy: start y position [m] + gx: goal x position [m] + gy: goal y position [m] + + output: + rx: x position list of the final path + ry: y position list of the final path + """ + + nstart = self.Node(self.calc_xyindex(sx, self.minx), + self.calc_xyindex(sy, self.miny), 0.0, -1, None) + ngoal = self.Node(self.calc_xyindex(gx, self.minx), + self.calc_xyindex(gy, self.miny), 0.0, -1, None) + + open_set, closed_set = dict(), dict() + open_set[self.calc_grid_index(nstart)] = nstart + + while 1: + if len(open_set) == 0: + print("Open set is empty..") + break + + c_id = max( + open_set, + key=lambda o: open_set[o].pind) + + current = open_set[c_id] + + # show graph + if show_animation: # pragma: no cover + plt.plot(self.calc_grid_position(current.x, self.minx), + self.calc_grid_position(current.y, self.miny), "xc") + # 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]) + if len(closed_set.keys()) % 10 == 0: + plt.pause(0.001) + + if current.x == ngoal.x and current.y == ngoal.y: + print("Find goal") + ngoal.pind = current.pind + ngoal.cost = current.cost + closed_set[current.pind] = current + break + + # Remove the item from the open set + del open_set[c_id] + + # Add it to the closed set + closed_set[c_id] = current + + # expand_grid search grid based on motion model + successors = [self.Node(current.x + self.motion[i][0], + current.y + self.motion[i][1], + current.cost + self.motion[i][2], c_id+1, current) for i, _ in enumerate(self.motion)] + + random.shuffle(successors) + for node in successors: + n_id = self.calc_grid_index(node) + + # If the node is not safe, do nothing + if not self.verify_node(node): + continue + + if n_id not in closed_set: + open_set[n_id] = node + + rx, ry = self.calc_final_path(ngoal, closed_set) + return rx, ry + + def calc_final_path(self, ngoal, closedset): + # generate final course + rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ + self.calc_grid_position(ngoal.y, self.miny)] + n = closedset[ngoal.pind] + while n.parent is not None: + rx.append(self.calc_grid_position(n.x, self.minx)) + ry.append(self.calc_grid_position(n.y, self.miny)) + n = n.parent + + return rx, ry + + @staticmethod + def calc_heuristic(n1, n2): + w = 1.0 # weight of heuristic + d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) + return d + + def calc_grid_position(self, index, minp): + """ + calc grid position + + :param index: + :param minp: + :return: + """ + pos = index * self.reso + minp + return pos + + def calc_xyindex(self, position, min_pos): + return round((position - min_pos) / self.reso) + + def calc_grid_index(self, node): + return (node.y - self.miny) * self.xwidth + (node.x - self.minx) + + def verify_node(self, node): + px = self.calc_grid_position(node.x, self.minx) + py = self.calc_grid_position(node.y, self.miny) + + if px < self.minx: + return False + elif py < self.miny: + return False + elif px >= self.maxx: + return False + elif py >= self.maxy: + return False + + # collision check + if self.obmap[node.x][node.y]: + return False + + return True + + def calc_obstacle_map(self, ox, oy): + + self.minx = round(min(ox)) + self.miny = round(min(oy)) + self.maxx = round(max(ox)) + self.maxy = round(max(oy)) + print("minx:", self.minx) + print("miny:", self.miny) + print("maxx:", self.maxx) + print("maxy:", self.maxy) + + self.xwidth = round((self.maxx - self.minx) / self.reso) + self.ywidth = round((self.maxy - self.miny) / self.reso) + print("xwidth:", self.xwidth) + print("ywidth:", self.ywidth) + + # obstacle map generation + self.obmap = [[False for i in range(self.ywidth)] + for i in range(self.xwidth)] + for ix in range(self.xwidth): + x = self.calc_grid_position(ix, self.minx) + for iy in range(self.ywidth): + y = self.calc_grid_position(iy, self.miny) + for iox, ioy in zip(ox, oy): + d = math.hypot(iox - x, ioy - y) + if d <= self.rr: + self.obmap[ix][iy] = True + break + + @staticmethod + def get_motion_model(): + # dx, dy, cost + motion = [[1, 0, 1], + [0, 1, 1], + [-1, 0, 1], + [0, -1, 1], + [-1, -1, math.sqrt(2)], + [-1, 1, math.sqrt(2)], + [1, -1, math.sqrt(2)], + [1, 1, math.sqrt(2)]] + + return motion + + +def main(): + print(__file__ + " start!!") + + # start and goal position + sx = 10.0 # [m] + sy = 10.0 # [m] + gx = 50.0 # [m] + gy = 50.0 # [m] + grid_size = 2.0 # [m] + robot_radius = 1.0 # [m] + + # set obstable positions + ox, oy = [], [] + for i in range(-10, 60): + ox.append(i) + oy.append(-10.0) + for i in range(-10, 60): + ox.append(60.0) + oy.append(i) + for i in range(-10, 61): + ox.append(i) + oy.append(60.0) + for i in range(-10, 61): + ox.append(-10.0) + oy.append(i) + for i in range(-10, 40): + ox.append(20.0) + oy.append(i) + for i in range(0, 40): + ox.append(40.0) + oy.append(60.0 - i) + + if show_animation: # pragma: no cover + plt.plot(ox, oy, ".k") + plt.plot(sx, sy, "og") + plt.plot(gx, gy, "xb") + plt.grid(True) + plt.axis("equal") + + DFS = DFSPlanner(ox, oy, grid_size, robot_radius) + rx, ry = DFS.planning(sx, sy, gx, gy) + + if show_animation: # pragma: no cover + plt.plot(rx, ry, "-r") + plt.pause(0.01) + plt.show() + + +if __name__ == '__main__': + main() From 6d4ea8bd1da771ea1315a73f0759c5f732fe1b99 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 14:24:16 +0200 Subject: [PATCH 2/7] change file name --- PathPlanning/DepthFirstSearch/{dfs.py => depth_first_search.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename PathPlanning/DepthFirstSearch/{dfs.py => depth_first_search.py} (100%) diff --git a/PathPlanning/DepthFirstSearch/dfs.py b/PathPlanning/DepthFirstSearch/depth_first_search.py similarity index 100% rename from PathPlanning/DepthFirstSearch/dfs.py rename to PathPlanning/DepthFirstSearch/depth_first_search.py From 18b73652f6767aef8a32da970f4f82387182d99b Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 14:25:35 +0200 Subject: [PATCH 3/7] remove heuristic method --- PathPlanning/DepthFirstSearch/depth_first_search.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index 4ceb0e2b..466aa164 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -17,7 +17,7 @@ import random show_animation = True -class DFSPlanner: +class DepthFirstSearchPlanner: def __init__(self, ox, oy, reso, rr): """ @@ -135,12 +135,6 @@ class DFSPlanner: return rx, ry - @staticmethod - def calc_heuristic(n1, n2): - w = 1.0 # weight of heuristic - d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) - return d - def calc_grid_position(self, index, minp): """ calc grid position @@ -260,8 +254,8 @@ def main(): plt.grid(True) plt.axis("equal") - DFS = DFSPlanner(ox, oy, grid_size, robot_radius) - rx, ry = DFS.planning(sx, sy, gx, gy) + dfs = DepthFirstSearchPlanner(ox, oy, grid_size, robot_radius) + rx, ry = dfs.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") From 952cff6b34e6abf9c52469952c20373c8d842f40 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 14:29:12 +0200 Subject: [PATCH 4/7] fix path not connecting to start node --- .../DepthFirstSearch/depth_first_search.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index 466aa164..bb080807 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -104,13 +104,13 @@ class DepthFirstSearchPlanner: # Add it to the closed set closed_set[c_id] = current - # expand_grid search grid based on motion model - successors = [self.Node(current.x + self.motion[i][0], - current.y + self.motion[i][1], - current.cost + self.motion[i][2], c_id+1, current) for i, _ in enumerate(self.motion)] + random.shuffle(self.motion) - random.shuffle(successors) - for node in successors: + # expand_grid search grid based on motion model + for i, _ in enumerate(self.motion): + node = self.Node(current.x + self.motion[i][0], + current.y + self.motion[i][1], + current.cost + self.motion[i][2], c_id+1, current) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing @@ -128,7 +128,7 @@ class DepthFirstSearchPlanner: rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] n = closedset[ngoal.pind] - while n.parent is not None: + while n is not None: rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) n = n.parent From ab05ac600aeb9bc5d80947e5a2ba5694a07e0a66 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 16:00:11 +0200 Subject: [PATCH 5/7] Fix typo in url and variable names --- PathPlanning/DepthFirstSearch/depth_first_search.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index bb080807..d2f41347 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -4,7 +4,7 @@ Depth-First grid planning author: Erwin Lejeune (@spida_rwin) -See Wikipedia article (https://en.wikipedia.org/wiki/depth-First_search) +See Wikipedia article (https://en.wikipedia.org/wiki/Depth-first_search) """ @@ -188,8 +188,8 @@ class DepthFirstSearchPlanner: print("ywidth:", self.ywidth) # obstacle map generation - self.obmap = [[False for i in range(self.ywidth)] - for i in range(self.xwidth)] + self.obmap = [[False for _ in range(self.ywidth)] + for _ in range(self.xwidth)] for ix in range(self.xwidth): x = self.calc_grid_position(ix, self.minx) for iy in range(self.ywidth): From 0dca0e328ddda954f149459073a944fc80a3452d Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 18:25:00 +0200 Subject: [PATCH 6/7] Use pop instead of min/del --- .../DepthFirstSearch/depth_first_search.py | 23 +++++-------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index d2f41347..54bbc48f 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -74,11 +74,8 @@ class DepthFirstSearchPlanner: print("Open set is empty..") break - c_id = max( - open_set, - key=lambda o: open_set[o].pind) - - current = open_set[c_id] + current = open_set.pop(list(open_set.keys())[-1]) + c_id = self.calc_grid_index(current) # show graph if show_animation: # pragma: no cover @@ -88,29 +85,19 @@ class DepthFirstSearchPlanner: plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit( 0) if event.key == 'escape' else None]) - if len(closed_set.keys()) % 10 == 0: - plt.pause(0.001) + plt.pause(0.01) if current.x == ngoal.x and current.y == ngoal.y: print("Find goal") ngoal.pind = current.pind ngoal.cost = current.cost - closed_set[current.pind] = current break - # Remove the item from the open set - del open_set[c_id] - - # Add it to the closed set - closed_set[c_id] = current - - random.shuffle(self.motion) - # expand_grid search grid based on motion model for i, _ in enumerate(self.motion): node = self.Node(current.x + self.motion[i][0], current.y + self.motion[i][1], - current.cost + self.motion[i][2], c_id+1, current) + current.cost + self.motion[i][2], c_id, None) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing @@ -119,6 +106,8 @@ class DepthFirstSearchPlanner: if n_id not in closed_set: open_set[n_id] = node + closed_set[n_id] = node + node.parent = current rx, ry = self.calc_final_path(ngoal, closed_set) return rx, ry From 6c977850c6e8e31fd136b265a515d84709bfe610 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 18:39:01 +0200 Subject: [PATCH 7/7] remove useless import --- PathPlanning/DepthFirstSearch/depth_first_search.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index 54bbc48f..d42aa5f1 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -12,8 +12,6 @@ import math import matplotlib.pyplot as plt -import random - show_animation = True @@ -21,7 +19,7 @@ class DepthFirstSearchPlanner: def __init__(self, ox, oy, reso, rr): """ - Initialize grid map for DFS planning + Initialize grid map for Depth-First planning ox: x position list of Obstacles [m] oy: y position list of Obstacles [m]