From 41d744c67f71de511d22179f2b8999ac41e789e6 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 22 Feb 2020 21:36:51 +0900 Subject: [PATCH 1/2] add error handling. But there is still bug. --- Mapping/grid_map_lib/grid_map_lib.py | 4 +- .../grid_based_sweep_coverage_path_planner.py | 82 ++++++++++++------- 2 files changed, 54 insertions(+), 32 deletions(-) diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py index fd016a28..4787238f 100644 --- a/Mapping/grid_map_lib/grid_map_lib.py +++ b/Mapping/grid_map_lib/grid_map_lib.py @@ -51,7 +51,7 @@ class GridMap: 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 @@ -163,7 +163,7 @@ class GridMap: 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 diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 0d2239a9..c51e9393 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -30,22 +30,23 @@ class SweepSearcher: 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,27 +57,31 @@ class SweepSearcher: 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 @@ -95,22 +100,24 @@ class SweepSearcher: 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] @@ -194,9 +201,11 @@ def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10): 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) + 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) + xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, + from_upper=False) return grid_map, xinds_goaly, goaly @@ -211,16 +220,19 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False): x, y = gmap.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) - if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None): + if sweep_searcher.is_search_done(gmap) or ( + cxind is None or cyind is None): print("Done") break @@ -245,13 +257,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 +285,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,11 +301,17 @@ 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 print("start!!") + ox = [0.0, 50.0, 50.0, 0.0, 0.0] + oy = [0.0, 0.0, 50.0, 50.0, 0.0] + reso = 0.4 + planning_animation(ox, oy, reso) + 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 From 7c8a940ebfd44217ae325470c48b47d9ed35f0f3 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 22 Feb 2020 23:17:25 +0900 Subject: [PATCH 2/2] fixed the bug and code clean up. --- Mapping/grid_map_lib/grid_map_lib.py | 21 +++++--- .../grid_based_sweep_coverage_path_planner.py | 48 +++++++++---------- 2 files changed, 36 insertions(+), 33 deletions(-) diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py index 4787238f..10651cc6 100644 --- a/Mapping/grid_map_lib/grid_map_lib.py +++ b/Mapping/grid_map_lib/grid_map_lib.py @@ -32,10 +32,8 @@ class GridMap: 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 @@ -99,7 +97,6 @@ class GridMap: """ 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) @@ -200,12 +197,22 @@ class GridMap: 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)) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index c51e9393..8d9ccd2e 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -30,7 +30,8 @@ class SweepSearcher: RIGHT = 1 LEFT = -1 - def __init__(self, moving_direction, sweep_direction, x_inds_goal_y, goal_y): + 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 = [] @@ -88,6 +89,8 @@ class SweepSearcher: 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), @@ -189,35 +192,34 @@ 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, + 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, + 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 @@ -229,26 +231,26 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False): 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 ( + 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 @@ -307,11 +309,6 @@ def planning_animation(ox, oy, reso): # pragma: no cover def main(): # pragma: no cover print("start!!") - ox = [0.0, 50.0, 50.0, 0.0, 0.0] - oy = [0.0, 0.0, 50.0, 50.0, 0.0] - reso = 0.4 - planning_animation(ox, oy, reso) - 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 @@ -328,7 +325,6 @@ def main(): # pragma: no cover planning_animation(ox, oy, reso) plt.show() - print("done!!")