""" Grid based sweep planner author: Atsushi Sakai """ import math from enum import IntEnum import numpy as np import matplotlib.pyplot as plt import sys import pathlib sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) from utils.angle import rot_mat_2d from Mapping.grid_map_lib.grid_map_lib import GridMap, FloatGrid do_animation = True class SweepSearcher: class SweepDirection(IntEnum): UP = 1 DOWN = -1 class MovingDirection(IntEnum): RIGHT = 1 LEFT = -1 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.x_indexes_goal_y = x_inds_goal_y self.goal_y = goal_y def move_target_grid(self, c_x_index, c_y_index, grid_map): n_x_index = self.moving_direction + c_x_index n_y_index = c_y_index # found safe grid if not self.check_occupied(n_x_index, n_y_index, grid_map): return n_x_index, n_y_index else: # occupied next_c_x_index, next_c_y_index = self.find_safe_turning_grid( c_x_index, c_y_index, grid_map) if (next_c_x_index is None) and (next_c_y_index is None): # moving backward next_c_x_index = -self.moving_direction + c_x_index next_c_y_index = c_y_index if self.check_occupied(next_c_x_index, next_c_y_index, grid_map): # moved backward, but the grid is occupied by obstacle return None, None else: # keep moving until end while not self.check_occupied(next_c_x_index + self.moving_direction, next_c_y_index, grid_map): next_c_x_index += self.moving_direction self.swap_moving_direction() return next_c_x_index, next_c_y_index @staticmethod def check_occupied(c_x_index, c_y_index, grid_map): return grid_map.check_occupied_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5)) def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map): for (d_x_ind, d_y_ind) in self.turing_window: next_x_ind = d_x_ind + c_x_index next_y_ind = d_y_ind + c_y_index # found safe grid if not self.check_occupied(next_x_ind, next_y_ind, grid_map): return next_x_ind, next_y_ind return None, None def is_search_done(self, grid_map): for ix in self.x_indexes_goal_y: if not self.check_occupied(ix, self.goal_y, grid_map): 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), (0, self.sweep_direction), (-self.moving_direction, self.sweep_direction), ] def swap_moving_direction(self): self.moving_direction *= -1 self.update_turning_window() def search_start_grid(self, grid_map): x_inds = [] y_ind = 0 if self.sweep_direction == self.SweepDirection.DOWN: x_inds, y_ind = search_free_grid_index_at_edge_y( grid_map, from_upper=True) elif self.sweep_direction == self.SweepDirection.UP: 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(x_inds), y_ind elif self.moving_direction == self.MovingDirection.LEFT: return max(x_inds), y_ind raise ValueError("self.moving direction is invalid ") def find_sweep_direction_and_start_position(ox, oy): # find sweep_direction 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.hypot(dx, dy) if d > max_dist: max_dist = d vec = [dx, dy] sweep_start_pos = [ox[i], oy[i]] return vec, sweep_start_pos def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position): tx = [ix - sweep_start_position[0] for ix in ox] ty = [iy - sweep_start_position[1] for iy in oy] th = math.atan2(sweep_vec[1], sweep_vec[0]) converted_xy = np.stack([tx, ty]).T @ rot_mat_2d(th) return converted_xy[:, 0], converted_xy[:, 1] def convert_global_coordinate(x, y, sweep_vec, sweep_start_position): th = math.atan2(sweep_vec[1], sweep_vec[0]) converted_xy = np.stack([x, y]).T @ rot_mat_2d(-th) rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]] ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]] return rx, ry def search_free_grid_index_at_edge_y(grid_map, from_upper=False): y_index = None x_indexes = [] if from_upper: x_range = range(grid_map.height)[::-1] y_range = range(grid_map.width)[::-1] else: x_range = range(grid_map.height) y_range = range(grid_map.width) for iy in x_range: for ix in y_range: if not SweepSearcher.check_occupied(ix, iy, grid_map): y_index = iy x_indexes.append(ix) if y_index: break return x_indexes, y_index def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10): width = math.ceil((max(ox) - min(ox)) / resolution) + offset_grid height = math.ceil((max(oy) - min(oy)) / resolution) + offset_grid 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, resolution, center_x, center_y) grid_map.print_grid_map_info() grid_map.set_value_from_polygon(ox, oy, FloatGrid(1.0), inside=False) grid_map.expand_grid() x_inds_goal_y = [] goal_y = 0 if sweep_direction == SweepSearcher.SweepDirection.UP: x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( grid_map, from_upper=True) elif sweep_direction == SweepSearcher.SweepDirection.DOWN: x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( grid_map, from_upper=False) return grid_map, x_inds_goal_y, goal_y def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False): # search start grid c_x_index, c_y_index = sweep_searcher.search_start_grid(grid_map) if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5)): print("Cannot find start grid") return [], [] x, y = grid_map.calc_grid_central_xy_position_from_xy_index(c_x_index, c_y_index) 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]) while True: c_x_index, c_y_index = sweep_searcher.move_target_grid(c_x_index, c_y_index, grid_map) if sweep_searcher.is_search_done(grid_map) or ( c_x_index is None or c_y_index is None): print("Done") break x, y = grid_map.calc_grid_central_xy_position_from_xy_index( c_x_index, c_y_index) px.append(x) py.append(y) grid_map.set_value_from_xy_index(c_x_index, c_y_index, FloatGrid(0.5)) if grid_search_animation: grid_map.plot_grid_map(ax=ax) plt.pause(1.0) return px, py def planning(ox, oy, resolution, moving_direction=SweepSearcher.MovingDirection.RIGHT, sweeping_direction=SweepSearcher.SweepDirection.UP, ): sweep_vec, sweep_start_position = find_sweep_direction_and_start_position( ox, oy) rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position) grid_map, x_inds_goal_y, goal_y = setup_grid_map(rox, roy, resolution, sweeping_direction) sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, x_inds_goal_y, goal_y) px, py = sweep_path_search(sweep_searcher, grid_map) rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_position) print("Path length:", len(rx)) return rx, ry def planning_animation(ox, oy, resolution): # pragma: no cover px, py = planning(ox, oy, resolution) # animation if do_animation: 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.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") plt.grid(True) plt.pause(0.1) plt.close() def main(): # pragma: no cover 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] resolution = 5.0 planning_animation(ox, oy, resolution) ox = [0.0, 50.0, 50.0, 0.0, 0.0] oy = [0.0, 0.0, 30.0, 30.0, 0.0] resolution = 1.3 planning_animation(ox, oy, resolution) 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] resolution = 5.0 planning_animation(ox, oy, resolution) if do_animation: plt.show() print("done!!") if __name__ == '__main__': main()