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.
Files | Planner.py - File to be implemented bresenham.py - Implementation of the Bresenham's line algorithm utils.py - Helpers for evaluation, drawing, and saving figures data - Scenarios and reference solutions in bag files t1c-plan.py - Evaluation script |
Resources | redcp.zip, t1c-plan.zip - Initial files and evaluation script |
Planner.grow_obstacles()
in Planner.py
,Planner.plan_path()
in Planner.py
,Planner.simplify_path()
in Planner.py
.
In Planner.py
, implement the following steps of grid-based planning.
grow_obstacles
) to account for the robot embodiment.
plan_path
) to find a path between the start and goal position (considering planning with 8-neighborhood).
simplify_path
) that simplifies the path as a sequence of waypoints
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:
We can further denote the mid probability of a cell being occupied as CELL_MID
.
The input parameters of the obstacle-growing function are:
grid_map
- OccupancyGrid - grid map representation;robot_size
- the robot radius in meters.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 """The following figure depicts an example of the obstacle growing for the first evaluation scenario. From left to right:
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
.
The input parameters of the plan_path
function are:
grid_map
- OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);start
- Pose - start robot pose;goal
- Pose - goal robot pose.The function returns
None
when the path is not found. It is supposed that the path contains only 2D coordinates and not the heading. Only position.x
and position.y
entries of individual poses in the path are filled in. The path is returned in the world coordinates!
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 """An example of the planned paths with the A* algorithm for different robot sizes is visualized in the following figure. From left to right, the planning for:
Suggested approach (click to view)
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)
bresenham_line
function from the bresenham.py
.
The input parameters of the function are
grid_map
- OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);
path
- Path - path to be simplified.
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 """The result of path simplification may vary, but an example of the planned and simplified path using the suggested approach is visualized in the following figure. From left to right, the planning and simplification for
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)