diff --git a/Mapping/grid_map_lib/grid_map_lib.py b/Mapping/grid_map_lib/grid_map_lib.py index fd016a28..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 @@ -51,7 +49,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 @@ -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) @@ -163,7 +160,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 @@ -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 0d2239a9..8d9ccd2e 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -30,22 +30,24 @@ 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,33 +58,39 @@ 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 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 @@ 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] @@ -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!!")