Merge pull request AtsushiSakai#295 from AtsushiSakai/issue_293

fix AtsushiSakai#293

Author: AtsushiSakai

Mapping/grid_map_lib/grid_map_lib.py

@@ -32,10 +32,8 @@ def __init__(self, width, height, resolution,
         self.center_x = center_x
         self.center_y = center_y
 
-        self.left_lower_x = self.center_x - \
-                            (self.width / 2.0) * self.resolution
-        self.left_lower_y = self.center_y - \
-                            (self.height / 2.0) * self.resolution
+        self.left_lower_x = self.center_x - self.width / 2.0 * self.resolution
+        self.left_lower_y = self.center_y - self.height / 2.0 * self.resolution
 
         self.ndata = self.width * self.height
         self.data = [init_val] * self.ndata
@@ -51,7 +49,7 @@ def get_value_from_xy_index(self, x_ind, y_ind):
 
         grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
 
-        if 0 <= grid_ind <= self.ndata:
+        if 0 <= grid_ind < self.ndata:
             return self.data[grid_ind]
         else:
             return None
@@ -99,7 +97,6 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):
         """
 
         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)
@@ -163,7 +160,7 @@ def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
 
         val = self.get_value_from_xy_index(xind, yind)
 
-        if val >= occupied_val:
+        if val is None or val >= occupied_val:
             return True
         else:
             return False
@@ -200,12 +197,22 @@ def check_inside_polygon(iox, ioy, x, y):
             if not min_x < iox < max_x:
                 continue
 
-            if (y[i1] + (y[i2] - y[i1]) / (x[i2] - x[i1])
-                * (iox - x[i1]) - ioy) > 0.0:
+            tmp1 = (y[i2] - y[i1]) / (x[i2] - x[i1])
+            if (y[i1] + tmp1 * (iox - x[i1]) - ioy) > 0.0:
                 inside = not inside
 
         return inside
 
+    def print_grid_map_info(self):
+        print("width:", self.width)
+        print("height:", self.height)
+        print("resolution:", self.resolution)
+        print("center_x:", self.center_x)
+        print("center_y:", self.center_y)
+        print("left_lower_x:", self.left_lower_x)
+        print("left_lower_y:", self.left_lower_y)
+        print("ndata:", self.ndata)
+
     def plot_grid_map(self, ax=None):
 
         grid_data = np.reshape(np.array(self.data), (self.height, self.width))

PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py

@@ -30,22 +30,24 @@ class MovingDirection(IntEnum):
         RIGHT = 1
         LEFT = -1
 
-    def __init__(self, mdirection, sdirection, xinds_goaly, goaly):
-        self.moving_direction = mdirection
-        self.sweep_direction = sdirection
+    def __init__(self,
+                 moving_direction, sweep_direction, x_inds_goal_y, goal_y):
+        self.moving_direction = moving_direction
+        self.sweep_direction = sweep_direction
         self.turing_window = []
         self.update_turning_window()
-        self.xinds_goaly = xinds_goaly
-        self.goaly = goaly
+        self.xinds_goaly = x_inds_goal_y
+        self.goaly = goal_y
 
     def move_target_grid(self, cxind, cyind, gmap):
         nxind = self.moving_direction + cxind
         nyind = cyind
 
         # found safe grid
-        if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
+        if not gmap.check_occupied_from_xy_index(nxind, nyind,
+                                                 occupied_val=0.5):
             return nxind, nyind
-        else:  # occupided
+        else:  # occupied
             ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
             if (ncxind is None) and (ncyind is None):
                 # moving backward
@@ -56,33 +58,39 @@ def move_target_grid(self, cxind, cyind, gmap):
                     return None, None
             else:
                 # keep moving until end
-                while not gmap.check_occupied_from_xy_index(ncxind + self.moving_direction, ncyind, occupied_val=0.5):
+                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
 
     def find_safe_turning_grid(self, cxind, cyind, gmap):
 
-        for (dxind, dyind) in self.turing_window:
+        for (d_x_ind, d_y_ind) in self.turing_window:
 
-            nxind = dxind + cxind
-            nyind = dyind + cyind
+            next_x_ind = d_x_ind + cxind
+            next_y_ind = d_y_ind + cyind
 
             # found safe grid
-            if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
-                return nxind, nyind
+            if not gmap.check_occupied_from_xy_index(next_x_ind, next_y_ind,
+                                                     occupied_val=0.5):
+                return next_x_ind, next_y_ind
 
         return None, None
 
-    def is_search_done(self, gmap):
+    def is_search_done(self, grid_map):
         for ix in self.xinds_goaly:
-            if not gmap.check_occupied_from_xy_index(ix, self.goaly, occupied_val=0.5):
+            if not grid_map.check_occupied_from_xy_index(ix, self.goaly,
+                                                         occupied_val=0.5):
                 return False
 
         # all lower grid is occupied
         return True
 
     def update_turning_window(self):
+        # turning window definition
+        # robot can move grid based on it.
         self.turing_window = [
             (self.moving_direction, 0.0),
             (self.moving_direction, self.sweep_direction),
@@ -95,22 +103,24 @@ def swap_moving_direction(self):
         self.update_turning_window()
 
     def search_start_grid(self, grid_map):
-        xinds = []
+        x_inds = []
         y_ind = 0
         if self.sweep_direction == self.SweepDirection.DOWN:
-            xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
+            x_inds, y_ind = search_free_grid_index_at_edge_y(
+                grid_map, from_upper=True)
         elif self.sweep_direction == self.SweepDirection.UP:
-            xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
+            x_inds, y_ind = search_free_grid_index_at_edge_y(
+                grid_map, from_upper=False)
 
         if self.moving_direction == self.MovingDirection.RIGHT:
-            return min(xinds), y_ind
+            return min(x_inds), y_ind
         elif self.moving_direction == self.MovingDirection.LEFT:
-            return max(xinds), y_ind
+            return max(x_inds), y_ind
 
         raise ValueError("self.moving direction is invalid ")
 
 
-def find_sweep_direction_and_start_posi(ox, oy):
+def find_sweep_direction_and_start_position(ox, oy):
     # find sweep_direction
     max_dist = 0.0
     vec = [0.0, 0.0]
@@ -182,61 +192,65 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
 def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10):
     width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid
     height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid
-    center_x = np.mean(ox)
-    center_y = np.mean(oy)
+    center_x = (np.max(ox)+np.min(ox))/2.0
+    center_y = (np.max(oy)+np.min(oy))/2.0
 
     grid_map = GridMap(width, height, reso, center_x, center_y)
-
+    grid_map.print_grid_map_info()
     grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
-
     grid_map.expand_grid()
 
-    xinds_goaly = []
-    goaly = 0
+    x_inds_goal_y = []
+    goal_y = 0
     if sweep_direction == SweepSearcher.SweepDirection.UP:
-        xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
+        x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
+                                                              from_upper=True)
     elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
-        xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
+        x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
+                                                              from_upper=False)
 
-    return grid_map, xinds_goaly, goaly
+    return grid_map, x_inds_goal_y, goal_y
 
 
-def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
+def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
     # search start grid
-    cxind, cyind = sweep_searcher.search_start_grid(gmap)
-    if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
+    cxind, cyind = sweep_searcher.search_start_grid(grid_map)
+    if not grid_map.set_value_from_xy_index(cxind, cyind, 0.5):
         print("Cannot find start grid")
         return [], []
 
-    x, y = gmap.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
+    x, y = grid_map.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
     px, py = [x], [y]
 
+    fig, ax = None, None
     if grid_search_animation:
         fig, ax = plt.subplots()
         # for stopping simulation with the esc key.
         fig.canvas.mpl_connect('key_release_event',
-                lambda event: [exit(0) if event.key == 'escape' else None])
+                               lambda event: [
+                                   exit(0) if event.key == 'escape' else None])
 
     while True:
-        cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
+        cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, grid_map)
 
-        if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None):
+        if sweep_searcher.is_search_done(grid_map) or (
+                cxind is None or cyind is None):
             print("Done")
             break
 
-        x, y = gmap.calc_grid_central_xy_position_from_xy_index(
+        x, y = grid_map.calc_grid_central_xy_position_from_xy_index(
             cxind, cyind)
 
         px.append(x)
         py.append(y)
 
-        gmap.set_value_from_xy_index(cxind, cyind, 0.5)
+        grid_map.set_value_from_xy_index(cxind, cyind, 0.5)
 
         if grid_search_animation:
-            gmap.plot_grid_map(ax=ax)
+            grid_map.plot_grid_map(ax=ax)
             plt.pause(1.0)
 
-    gmap.plot_grid_map()
+    grid_map.plot_grid_map()
 
     return px, py
 
@@ -245,13 +259,16 @@ def planning(ox, oy, reso,
              moving_direction=SweepSearcher.MovingDirection.RIGHT,
              sweeping_direction=SweepSearcher.SweepDirection.UP,
              ):
-    sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(ox, oy)
+    sweep_vec, sweep_start_posi = find_sweep_direction_and_start_position(
+        ox, oy)
 
     rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
 
-    gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, sweeping_direction)
+    gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso,
+                                              sweeping_direction)
 
-    sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_goaly, goaly)
+    sweep_searcher = SweepSearcher(moving_direction, sweeping_direction,
+                                   xinds_goaly, goaly)
 
     px, py = sweep_path_search(sweep_searcher, gmap)
 
@@ -270,8 +287,9 @@ def planning_animation(ox, oy, reso):  # pragma: no cover
         for ipx, ipy in zip(px, py):
             plt.cla()
             # 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.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
             plt.plot(ox, oy, "-xb")
             plt.plot(px, py, "-r")
             plt.plot(ipx, ipy, "or")
@@ -285,6 +303,7 @@ def planning_animation(ox, oy, reso):  # pragma: no cover
     plt.axis("equal")
     plt.grid(True)
     plt.pause(0.1)
+    plt.close()
 
 
 def main():  # pragma: no cover
@@ -306,7 +325,6 @@ def main():  # pragma: no cover
     planning_animation(ox, oy, reso)
 
     plt.show()
-
     print("done!!")