release grid based_sweep_coverage_path_planner.py

Author: AtsushiSakai

PathPlanning/GridBasedSweepCCP/grid_based_sweep_planner.py renamed to PathPlanning/GridBasedSweepCCP/grid_based_sweep_coverage_path_planner.py

@@ -7,6 +7,7 @@
 import math
 import os
 import sys
+from enum import IntEnum
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -21,14 +22,21 @@
 
 
 class SweepSearcher:
+    class SweepDirection(IntEnum):
+        UP = 1
+        DOWN = -1
 
-    def __init__(self, mdirection, sdirection, xinds_miny, miny):
-        self.moving_direction = mdirection  # +1 or -1
-        self.sweep_direction = sdirection  # +1 or -1
+    class MovingDirection(IntEnum):
+        RIGHT = 1
+        LEFT = -1
+
+    def __init__(self, mdirection, sdirection, xinds_goaly, goaly):
+        self.moving_direction = mdirection
+        self.sweep_direction = sdirection
         self.turing_window = []
         self.update_turning_window()
-        self.xinds_miny = xinds_miny
-        self.miny = miny
+        self.xinds_goaly = xinds_goaly
+        self.goaly = goaly
 
     def move_target_grid(self, cxind, cyind, gmap):
 
@@ -48,6 +56,7 @@ def move_target_grid(self, cxind, cyind, gmap):
                     # moved backward, but the grid is occupied by obstacle
                     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):
                     ncxind += self.moving_direction
                 self.swap_moving_direction()
@@ -67,8 +76,8 @@ def find_safe_turning_grid(self, cxind, cyind, gmap):
         return None, None
 
     def is_search_done(self, gmap):
-        for ix in self.xinds_miny:
-            if not gmap.check_occupied_from_xy_index(ix, self.miny, occupied_val=0.5):
+        for ix in self.xinds_goaly:
+            if not gmap.check_occupied_from_xy_index(ix, self.goaly, occupied_val=0.5):
                 return False
 
         # all lower grid is occupied
@@ -86,19 +95,33 @@ def swap_moving_direction(self):
         self.moving_direction *= -1
         self.update_turning_window()
 
+    def search_start_grid(self, grid_map):
+        xinds = [], 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)
+        elif self.sweep_direction == self.SweepDirection.UP:
+            xinds, 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
+        elif self.moving_direction == self.MovingDirection.LEFT:
+            return max(xinds), y_ind
+
+        raise ValueError("self.moving direction is invalid ")
+
 
 def find_sweep_direction_and_start_posi(ox, oy):
     # find sweep_direction
-    maxd = 0.0
+    max_dist = 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 = d
+        if d > max_dist:
+            max_dist = d
             vec = [dx, dy]
             sweep_start_pos = [ox[i], oy[i]]
 
@@ -156,9 +179,7 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
     return xinds, yind
 
 
-def setup_grid_map(ox, oy, reso):
-    offset_grid = 10
-
+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)
@@ -170,27 +191,28 @@ def setup_grid_map(ox, oy, reso):
 
     grid_map.expand_grid()
 
-    xinds, miny = search_free_grid_index_at_edge_y(grid_map)
-
-    return grid_map, xinds, miny
-
-
-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
+    xinds_goaly = [], goaly = 0
+    if sweep_direction == SweepSearcher.SweepDirection.UP:
+        xinds_goaly, goaly = 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)
 
+    return grid_map, xinds_goaly, goaly
 
-def sweep_path_search(sweep_searcher, gmap):
-    px, py = [], []
 
+def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
     # search start grid
-    cxind, cyind = search_start_grid(gmap)
+    cxind, cyind = sweep_searcher.search_start_grid(gmap)
     if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
         print("Cannot find start grid")
-        return px, py
+        return [], []
+
+    x, y = gmap.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
+    px, py = [x], [y]
+
+    if grid_search_animation:
+        fig, ax = plt.subplots()
 
-    # fig, ax = plt.subplots()
     while True:
         cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
 
@@ -206,44 +228,39 @@ def sweep_path_search(sweep_searcher, gmap):
 
         gmap.set_value_from_xy_index(cxind, cyind, 0.5)
 
-        # gmap.plot_grid_map(ax=ax)
-        # plt.pause(1.0)
+        if grid_search_animation:
+            gmap.plot_grid_map(ax=ax)
+            plt.pause(1.0)
 
     gmap.plot_grid_map()
 
     return px, py
 
 
-def planning(ox, oy, reso):
+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)
 
     rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
 
-    moving_direction = 1
-    sweeping_direction = -1
+    gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, sweeping_direction)
 
-    gmap, xinds_miny, miny = setup_grid_map(rox, roy, reso)
-
-    sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_miny, miny)
+    sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_goaly, goaly)
 
     px, py = sweep_path_search(sweep_searcher, gmap)
 
     rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
 
-    return rx, ry
+    print("Path length:", len(rx))
 
+    return rx, ry
 
-def main():
-    print("start!!")
-
-    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
 
+def planning_animation(ox, oy, reso):
     px, py = planning(ox, oy, reso)
 
-    plt.subplots()
-
     # animation
     if do_animation:
         for ipx, ipy in zip(px, py):
@@ -260,6 +277,27 @@ def main():
     plt.plot(px, py, "-r")
     plt.axis("equal")
     plt.grid(True)
+    plt.pause(0.1)
+
+
+def main():
+    print("start!!")
+
+    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
+    planning_animation(ox, oy, reso)
+
+    ox = [0.0, 50.0, 50.0, 0.0, 0.0]
+    oy = [0.0, 0.0, 30.0, 30.0, 0.0]
+    reso = 1.3
+    planning_animation(ox, oy, reso)
+
+    ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
+    oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
+    reso = 5.0
+    planning_animation(ox, oy, reso)
+
     plt.show()
 
     print("done!!")

PathPlanning/GridBasedSweepCCP/test_grid_based_sweep_coverage_path_planner.py

@@ -0,0 +1,92 @@
+from unittest import TestCase
+
+import grid_based_sweep_coverage_path_planner
+
+
+class TestPlanning(TestCase):
+
+    def test_planning1(self):
+        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 = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+    def test_planning2(self):
+        ox = [0.0, 50.0, 50.0, 0.0, 0.0]
+        oy = [0.0, 0.0, 30.0, 30.0, 0.0]
+        reso = 1.3
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+    def test_planning3(self):
+        ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
+        oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
+        reso = 5.1
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
+                                                                 )
+        self.assertTrue(len(px) >= 5)
+
+        px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
+                                                                 moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
+                                                                 sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
+                                                                 )
+        self.assertTrue(len(px) >= 5)