Search
The main task is to implement a simple grid-based planning approach. You will learn how to plan on the grid such that the robot embodiment is considered.
Planner.py
bresenham.py
utils.py
data
t1c-plan.py
Planner.grow_obstacles()
Planner.plan_path()
Planner.simplify_path()
In Planner.py, implement the following steps of grid-based planning.
grow_obstacles
plan_path
simplify_path
The grow_obstacles function processes the occupancy grid map into a binary map that encompasses areas inaccessible for the robot considering its embodiment. Using obstacles growing by a disk representing the robot shape, we can use such a grid map for grid-based planning with a single-cell robot. The robot shape can be modeled as a disk with $r_\mathrm{robot}$ radius robot_size. The result of the obstacle growing is a binary map with the value 1 for occupied and unknown cells and 0 for traversable cells; thus, we threshold the occupancy grid in addition to obstacle growing. For each cell $m_i$ of the grid_map the new binary value $b_i$ of the grid_map_grow is based on its Bayesian occupancy probability $P(m_i = occupied \vert z) = 0.5$ and distance to the nearby obstacles according to:
robot_size
grid_map
grid_map_grow
We can further denote the mid probability of a cell being occupied as CELL_MID.
CELL_MID
The input parameters of the obstacle-growing function are:
The function returns
The grow_obstacles function in the Planner.py class has the following prescription.
def grow_obstacles(self, grid_map: OccupancyGrid, robot_size) -> OccupancyGrid: """ Method to grow the obstacles to take into account the robot embodiment Args: grid_map: OccupancyGrid - grid map for obstacle growing robot_size: float - the size of the robot Returns: grid_map_grow: OccupancyGrid - grid map with considered robot body embodiment """
Suggested approach (click to view)
Obstacle growing - The robot has non-zero dimensions representing its embodiment, which must be considered when planning. Moreover, the plan execution in real experiments has a limited precision that further enlarges demands on the clearance to the obstacles for the robot to navigate the environment safely. We can grow the borders of the obstacles by the diameter of the robot and plan in the resulting map that ensures sufficient clearance to the obstacles. The obstacle growing can be achieved by one of the following approaches:
scipy.ndimage.distance_transform_edt
Path planning is to be implemented in the plan_path function that provides a path between the given start and goal poses on the grid map considering 8-neighborhood. The function returns a path on the output. If the path is not found, the planner returns None.
None
The input parameters of the plan_path function are:
start
goal
position.x
position.y
The plan_path function in the Planner.py class has the following prescription.
def plan_path(self, grid_map: OccupancyGrid, start: Pose, goal: Pose) -> Path: """ Method to plan the path from the start to the goal pose on the grid Args: grid_map: OccupancyGrid - grid map for obstacle growing start: Pose - robot start pose goal: Pose - robot goal pose Returns: path: Path - path between the start and goal Pose on the map """
| | | |
Considered planning methods
Path simplification can be done by excluding unnecessary cells (locations) from the path. The robot does not have to visit them precisely or will visit them along the navigation to the waypoint, where it needs to change direction. A possible approach to path simplification is to employ the ray-casting technique to connect the neighboring segments one by one using straight-line segments up to the point where the straight-line segment collides with an obstacle (grown obstacle) and then follows with another straight-line segment.
$\qquad\bullet\,$ Bresenham's line algorithm (click to view)
def bresenham_line(self, start: (int, int), goal: (int, int)) -> (int, int): """Bresenham's line algorithm Args: start: (int, int) - start coordinate goal: (int, int) - goal coordinate Returns: Interlining points between the start and goal coordinate """ (x0, y0) = start (x1, y1) = goal line = [] dx = abs(x1 - x0) dy = abs(y1 - y0) x, y = x0, y0 sx = -1 if x0 > x1 else 1 sy = -1 if y0 > y1 else 1 if dx > dy: err = dx / 2.0 while x != x1: line.append((x,y)) err -= dy if err < 0: y += sy err += dx x += sx else: err = dy / 2.0 while y != y1: line.append((x,y)) err -= dx if err < 0: x += sx err += dy y += sy x = goal[0] y = goal[1] return line
bresenham_line
The input parameters of the function are
path
The function returns:
The simplify_path function in the Planner.py class has a following prescription:
def simplify_path(self, grid_map: OccupancyGrid, path: Path) -> Path: """ Method to simplify the found path on the grid Args: grid_map: OccupancyGrid - grid map for obstacle growing path: Path - path to be simplified Returns: path_simple: Path - simplified path """
#add the start position path_simple add path[0] #iterate through the path and simplify the path while not path_simple[end] == path[end]: #until the goal is not reached #find the connected segment previous_pose = path_simple[end] for each pose in the rest of the path: if bresenham_line(path_simple[end], pose) not collide: #there is no collision previous_pose = pose #the goal is reached if pose = path[end]: path_simple append pose break else: #there is a collision path_simple append previous_pose break
The evaluation focuses on the correct behavior of the above-described robot navigation pipeline; see t1c-plan.py for the full evaluation script.
ROBOT_SIZE = 0.5 if __name__=="__main__": planner = Planner() scenarios = pickle.load( open( "data/scenarios.bag", "rb" ) ) for gridmap, start, goal in scenarios: gridmap_processed = planner.grow_obstacles(gridmap, radius) path = planner.plan_path(gridmap_processed, start, goal) path_simple = planner.simplify_path(gridmap_processed, path)