From 39d12558e99b14e79bb5333b9ad54b24d2a8fca5 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 10 Apr 2020 15:07:54 +0200 Subject: [PATCH 01/10] add Breadth-First Search --- PathPlanning/BreadthFirstSearch/bfs.py | 270 +++++++++++++++++++++++++ 1 file changed, 270 insertions(+) create mode 100644 PathPlanning/BreadthFirstSearch/bfs.py diff --git a/PathPlanning/BreadthFirstSearch/bfs.py b/PathPlanning/BreadthFirstSearch/bfs.py new file mode 100644 index 00000000..d20c6080 --- /dev/null +++ b/PathPlanning/BreadthFirstSearch/bfs.py @@ -0,0 +1,270 @@ +""" + +Breadth-First grid planning + +author: Erwin Lejeune (@spida_rwin) + +See Wikipedia article (https://en.wikipedia.org/wiki/Breadth-First_search) + +""" + +import math + +import matplotlib.pyplot as plt + +import random + +show_animation = True + + +class BFSPlanner: + + def __init__(self, ox, oy, reso, rr): + """ + Initialize grid map for bfs 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): + """ + Breadth 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 = min( + 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 + break + + # Remove the item from the open set + del open_set[c_id] + + # 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: + closed_set[n_id] = node + 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") + + bfs = BFSPlanner(ox, oy, grid_size, robot_radius) + rx, ry = bfs.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 01596296c41b0a76775b64378734fa0bd25c5662 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 13:45:22 +0200 Subject: [PATCH 02/10] change breadth first search file name --- .../BreadthFirstSearch/{bfs.py => breadth_first_search.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename PathPlanning/BreadthFirstSearch/{bfs.py => breadth_first_search.py} (100%) diff --git a/PathPlanning/BreadthFirstSearch/bfs.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py similarity index 100% rename from PathPlanning/BreadthFirstSearch/bfs.py rename to PathPlanning/BreadthFirstSearch/breadth_first_search.py From f2c651ff5514109e5d26bce8dff234b798048d8e Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 13:56:57 +0200 Subject: [PATCH 03/10] Changes following review --- .../breadth_first_search.py | 20 ++++++------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index d20c6080..3f5c95de 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -17,7 +17,7 @@ import random show_animation = True -class BFSPlanner: +class BreadthFirstSearchPlanner: def __init__(self, ox, oy, reso, rr): """ @@ -48,7 +48,7 @@ class BFSPlanner: def planning(self, sx, sy, gx, gy): """ - Breadth First search + Breadth First search based planning input: sx: start x position [m] @@ -101,12 +101,10 @@ class BFSPlanner: del open_set[c_id] # expand_grid search grid based on motion model - successors = [self.Node(current.x + self.motion[i][0], + 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) for i, _ in enumerate(self.motion)] - - random.shuffle(successors) - for node in successors: + 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 @@ -132,12 +130,6 @@ class BFSPlanner: 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 @@ -257,7 +249,7 @@ def main(): plt.grid(True) plt.axis("equal") - bfs = BFSPlanner(ox, oy, grid_size, robot_radius) + bfs = BreadthFirstSearchPlanner(ox, oy, grid_size, robot_radius) rx, ry = bfs.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover From 68c4c78e089d9a41eeeb50dad50fc9481649138c Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 14:02:07 +0200 Subject: [PATCH 04/10] Fix path not connecting to start node --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index 3f5c95de..e67a4788 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -128,6 +128,9 @@ class BreadthFirstSearchPlanner: ry.append(self.calc_grid_position(n.y, self.miny)) n = n.parent + rx.append(self.calc_grid_position(n.x, self.minx)) + ry.append(self.calc_grid_position(n.y, self.miny)) + return rx, ry def calc_grid_position(self, index, minp): From 71daa7898d88f590ee4e65935e239115cb431fb5 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Mon, 13 Apr 2020 14:34:47 +0200 Subject: [PATCH 05/10] easier path retrieving and shuffle children node --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index e67a4788..f4c0fcd0 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -100,6 +100,8 @@ class BreadthFirstSearchPlanner: # Remove the item from the open set del open_set[c_id] + 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], @@ -123,14 +125,11 @@ class BreadthFirstSearchPlanner: 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 - rx.append(self.calc_grid_position(n.x, self.minx)) - ry.append(self.calc_grid_position(n.y, self.miny)) - return rx, ry def calc_grid_position(self, index, minp): From 81622428ef03cf27123cf9c79dede051cf8d1b89 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Tue, 14 Apr 2020 15:27:33 +0200 Subject: [PATCH 06/10] not adding parent if node in closed set --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index f4c0fcd0..f2580243 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -106,7 +106,7 @@ class BreadthFirstSearchPlanner: 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+1, None) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing @@ -114,6 +114,7 @@ class BreadthFirstSearchPlanner: continue if n_id not in closed_set: + node.parent = current closed_set[n_id] = node open_set[n_id] = node From fa5ca3cd7c88736d28ec96868328637c3772b803 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 16:01:19 +0200 Subject: [PATCH 07/10] Fix typo in url, variable name --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index f2580243..5a380355 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -4,7 +4,7 @@ Breadth-First grid planning author: Erwin Lejeune (@spida_rwin) -See Wikipedia article (https://en.wikipedia.org/wiki/Breadth-First_search) +See Wikipedia article (https://en.wikipedia.org/wiki/Breadth-first_search) """ @@ -186,8 +186,8 @@ class BreadthFirstSearchPlanner: 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 7993d7ca8b594fdaa9e8b8071f540cab079670e2 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 18:11:47 +0200 Subject: [PATCH 08/10] use pop instead of min+del --- .../breadth_first_search.py | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index 5a380355..d9e2aa5b 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -74,11 +74,11 @@ class BreadthFirstSearchPlanner: print("Open set is empty..") break - c_id = min( - open_set, - key=lambda o: open_set[o].pind) + current = open_set.pop(list(open_set.keys())[0]) - current = open_set[c_id] + c_id = self.calc_grid_index(current) + + closed_set[c_id] = current # show graph if show_animation: # pragma: no cover @@ -97,25 +97,19 @@ class BreadthFirstSearchPlanner: ngoal.cost = current.cost break - # Remove the item from the open set - del open_set[c_id] - - 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, None) + current.cost + self.motion[i][2], c_id, None) 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: + if (n_id not in closed_set) and (n_id not in open_set): node.parent = current - closed_set[n_id] = node open_set[n_id] = node rx, ry = self.calc_final_path(ngoal, closed_set) @@ -186,8 +180,8 @@ class BreadthFirstSearchPlanner: print("ywidth:", self.ywidth) # obstacle map generation - self.obmap = [[False for _ in range(self.ywidth)] - for _ in range(self.xwidth)] + 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): From 97978ce51fcffeffd141bc4fae48e653b5339fc3 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 16 Apr 2020 18:39:35 +0200 Subject: [PATCH 09/10] remove useless import --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index d9e2aa5b..b4a5c38a 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -12,8 +12,6 @@ import math import matplotlib.pyplot as plt -import random - show_animation = True From 5ba82c6ced66004d1bf8a8dc0b91672394c5af9b Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Sat, 18 Apr 2020 15:27:59 +0200 Subject: [PATCH 10/10] replace meaningless variable name with _ --- PathPlanning/BreadthFirstSearch/breadth_first_search.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index b4a5c38a..dbdeff47 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -178,8 +178,8 @@ class BreadthFirstSearchPlanner: 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):