first release grid based_sweep_planner.py

Author: AtsushiSakai

Mapping/grid_map_lib/grid_map_lib.py

@@ -98,9 +98,13 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):
         :param val: grid value
         """
 
+        if (x_ind is None) or (y_ind is None):
+            print(x_ind, y_ind)
+            return False, False
+
         grid_ind = int(y_ind * self.width + x_ind)
 
-        if 0 <= grid_ind <= self.ndata:
+        if 0 <= grid_ind < self.ndata:
             self.data[grid_ind] = val
             return True  # OK
         else:
@@ -164,6 +168,23 @@ def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
         else:
             return False
 
+    def expand_grid(self):
+        xinds, yinds = [], []
+
+        for ix in range(self.width):
+            for iy in range(self.height):
+                if self.check_occupied_from_xy_index(ix, iy):
+                    xinds.append(ix)
+                    yinds.append(iy)
+
+        for (ix, iy) in zip(xinds, yinds):
+            self.set_value_from_xy_index(ix + 1, iy, val=1.0)
+            self.set_value_from_xy_index(ix, iy + 1, val=1.0)
+            self.set_value_from_xy_index(ix + 1, iy + 1, val=1.0)
+            self.set_value_from_xy_index(ix - 1, iy, val=1.0)
+            self.set_value_from_xy_index(ix, iy - 1, val=1.0)
+            self.set_value_from_xy_index(ix - 1, iy - 1, val=1.0)
+
     @staticmethod
     def check_inside_polygon(iox, ioy, x, y):
 

PathPlanning/AStar/a_star.py

@@ -9,9 +9,10 @@
 
 """
 
-import matplotlib.pyplot as plt
 import math
 
+import matplotlib.pyplot as plt
+
 show_animation = True
 
 
@@ -66,6 +67,10 @@ def planning(self, sx, sy, gx, gy):
         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].cost + self.calc_heuristic(ngoal, open_set[o]))
             current = open_set[c_id]
@@ -89,25 +94,25 @@ def planning(self, sx, sy, gx, gy):
             # Add it to the closed set
             closed_set[c_id] = current
 
-            # expand search grid based on motion model
+            # 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)
                 n_id = self.calc_grid_index(node)
 
-                # If the node is in closed_set, do nothing
-                if n_id in closed_set:
-                    continue
 
                 # If the node is not safe, do nothing
                 if not self.verify_node(node):
                     continue
 
+                if n_id in closed_set:
+                    continue
+
                 if n_id not in open_set:
                     open_set[n_id] = node  # discovered a new node
                 else:
-                    if open_set[n_id].cost >= node.cost:
+                    if open_set[n_id].cost > node.cost:
                         # This path is the best until now. record it
                         open_set[n_id] = node
 

PathPlanning/GridBasedSweepCCP/grid_based_sweep_planner.py

@@ -12,7 +12,12 @@
 import numpy as np
 
 sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/"))
-from grid_map_lib import GridMap
+try:
+    from grid_map_lib import GridMap
+except ImportError:
+    exit()
+
+do_animation = True
 
 
 class SweepSearcher:
@@ -35,14 +40,16 @@ def move_target_grid(self, cxind, cyind, gmap):
             return nxind, nyind
         else:  # occupided
             ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
-            if not ncxind and not ncyind:
+            if (ncxind is None) and (ncyind is None):
                 # moving backward
                 ncxind = -self.moving_direction + cxind
                 ncyind = cyind
                 if gmap.check_occupied_from_xy_index(ncxind, ncyind):
                     # moved backward, but the grid is occupied by obstacle
                     return None, None
             else:
+                while not gmap.check_occupied_from_xy_index(ncxind + self.moving_direction, ncyind, occupied_val=0.5):
+                    ncxind += self.moving_direction
                 self.swap_moving_direction()
             return ncxind, ncyind
 
@@ -55,7 +62,6 @@ def find_safe_turning_grid(self, cxind, cyind, gmap):
 
             # found safe grid
             if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
-                print(dxind, dyind)
                 return nxind, nyind
 
         return None, None
@@ -81,34 +87,22 @@ def swap_moving_direction(self):
         self.update_turning_window()
 
 
-def find_sweep_direction_and_start_posi(ox, oy, start_posi):
+def find_sweep_direction_and_start_posi(ox, oy):
     # find sweep_direction
-    maxd_id = None
     maxd = 0.0
     vec = [0.0, 0.0]
+    sweep_start_pos = [0.0, 0.0]
     for i in range(len(ox) - 1):
         dx = ox[i + 1] - ox[i]
         dy = oy[i + 1] - oy[i]
         d = np.sqrt(dx ** 2 + dy ** 2)
 
         if d > maxd:
-            maxd_id = i
             maxd = d
             vec = [dx, dy]
+            sweep_start_pos = [ox[i], oy[i]]
 
-    # find sweep start posi
-    d1 = np.sqrt((ox[maxd_id] - start_posi[0]) ** 2 +
-                 (oy[maxd_id] - start_posi[1]) ** 2)
-    d2 = np.sqrt((ox[maxd_id + 1] - start_posi[0]) ** 2 +
-                 (oy[maxd_id + 1] - start_posi[1]) ** 2)
-
-    if d2 >= d1:  # first point is near
-        sweep_start_posi = [ox[maxd_id], oy[maxd_id]]
-    else:
-        sweep_start_posi = [ox[maxd_id + 1], oy[maxd_id + 1]]
-        vec = [-1.0 * iv for iv in vec]  # reverse direction
-
-    return vec, sweep_start_posi
+    return vec, sweep_start_pos
 
 
 def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi):
@@ -140,19 +134,26 @@ def convert_global_coordinate(x, y, sweep_vec, sweep_start_posi):
     return rx, ry
 
 
-def search_free_lower_y_grid_index(grid_map):
-    miny = None
+def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
+    yind = None
     xinds = []
 
-    for iy in range(grid_map.height):
-        for ix in range(grid_map.width):
+    if from_upper:
+        xrange = range(grid_map.height)[::-1]
+        yrange = range(grid_map.width)[::-1]
+    else:
+        xrange = range(grid_map.height)
+        yrange = range(grid_map.width)
+
+    for iy in xrange:
+        for ix in yrange:
             if not grid_map.check_occupied_from_xy_index(ix, iy):
-                miny = iy
+                yind = iy
                 xinds.append(ix)
-        if miny:
+        if yind:
             break
 
-    return xinds, miny
+    return xinds, yind
 
 
 def setup_grid_map(ox, oy, reso):
@@ -167,47 +168,33 @@ def setup_grid_map(ox, oy, reso):
 
     grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
 
-    # fill grid
-    # for i in range(len(ox) - 1):
-    #     grid_map.set_value_from_xy_pos(ox[i], oy[i], 1.0)
-    #
-    #     x, y = ox[i], oy[i]
-    #     th = math.atan2(oy[i + 1] - oy[i], ox[i + 1] - ox[i])
-    #     d = np.sqrt((x - ox[i + 1])**2 + (y - oy[i + 1])**2)
-    #
-    #     while d > reso:
-    #         x += np.cos(th) * reso
-    #         y += np.sin(th) * reso
-    #         d = np.sqrt((x - ox[i + 1])**2 + (y - oy[i + 1])**2)
-    #
-    #     grid_map.set_value_from_xy_pos(x, y, 1.0)
-
-    xinds, miny = search_free_lower_y_grid_index(grid_map)
-
-    # grid_map.plot_gridmap()
+    grid_map.expand_grid()
+
+    xinds, miny = search_free_grid_index_at_edge_y(grid_map)
 
     return grid_map, xinds, miny
 
 
-def sweep_path_search(sweep_searcher, gmap, start_posi):
-    sx, sy = start_posi[0], start_posi[1]
-    # print(sx, sy)
+def search_start_grid(grid_map):
+    xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
+
+    return min(xinds), y_ind
 
-    # search start grid
-    cxind, cyind = gmap.get_xy_index_from_xy_pos(sx, sy)
-    if gmap.check_occupied_from_xy_index(cxind, cyind):
-        cxind, cyind = sweep_searcher.find_safe_turning_grid(cxind, cyind, gmap)
-    gmap.set_value_from_xy_index(cxind, cyind, 0.5)
 
+def sweep_path_search(sweep_searcher, gmap):
     px, py = [], []
 
-    # fig, ax = plt.subplots()
+    # search start grid
+    cxind, cyind = search_start_grid(gmap)
+    if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
+        print("Cannot find start grid")
+        return px, py
 
+    # fig, ax = plt.subplots()
     while True:
-
         cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
 
-        if sweep_searcher.is_search_done(gmap) or (not cxind and not cyind):
+        if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None):
             print("Done")
             break
 
@@ -220,16 +207,15 @@ def sweep_path_search(sweep_searcher, gmap, start_posi):
         gmap.set_value_from_xy_index(cxind, cyind, 0.5)
 
         # gmap.plot_grid_map(ax=ax)
-        # plt.pause(0.1)
+        # plt.pause(1.0)
 
     gmap.plot_grid_map()
 
     return px, py
 
 
-def planning(ox, oy, reso, start_posi):
-    sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(
-        ox, oy, start_posi)
+def planning(ox, oy, reso):
+    sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(ox, oy)
 
     rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
 
@@ -240,7 +226,7 @@ def planning(ox, oy, reso, start_posi):
 
     sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_miny, miny)
 
-    px, py = sweep_path_search(sweep_searcher, gmap, start_posi)
+    px, py = sweep_path_search(sweep_searcher, gmap)
 
     rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
 
@@ -250,17 +236,26 @@ def planning(ox, oy, reso, start_posi):
 def main():
     print("start!!")
 
-    start_posi = [0.0, 0.0]
-
     ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
     oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
     reso = 5.0
 
-    px, py = planning(ox, oy, reso, start_posi)
+    px, py = planning(ox, oy, reso)
 
     plt.subplots()
 
-    plt.plot(start_posi[0], start_posi[1], "or")
+    # animation
+    if do_animation:
+        for ipx, ipy in zip(px, py):
+            plt.cla()
+            plt.plot(ox, oy, "-xb")
+            plt.plot(px, py, "-r")
+            plt.plot(ipx, ipy, "or")
+            plt.axis("equal")
+            plt.grid(True)
+            plt.pause(0.1)
+
+    plt.cla()
     plt.plot(ox, oy, "-xb")
     plt.plot(px, py, "-r")
     plt.axis("equal")