Warning

## t1c-plan - Grid-based path planning

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
Implement
• Planner.grow_obstacles() in Planner.py,
• Planner.plan_path() in Planner.py,
• Planner.simplify_path() in Planner.py.

#### Assignment

In Planner.py, implement the following steps of grid-based planning.

1. Implement the obstacle growing (method grow_obstacles) to account for the robot embodiment.
2. Implement a grid-based planning method (method plan_path) to find a path between the start and goal position (considering planning with 8-neighborhood).
3. Implement the trajectory smoothing method (method simplify_path) that simplifies the path as a sequence of waypoints

# Obstacle Growing

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:

1. Filtering all obstacles - if $P(m_i = occupied \vert z) > 0.5$, then the cell is considered occupied $b_i$ = 1;
2. Filtering all unknown areas - if $P(m_i = occupied \vert z) = 0.5$, then the cell is considered occupied $b_i$ = 1;
3. Filtering cells close to the obstacles - if $P(m_i = occupied \vert z) < 0.5$ and the approximated Euclidean distance on the grid of the cell $m_i$ and the closest obstacle is shorter than $r_\mathrm{robot}$, then the cell is considered occupied $b_i$ = 1;
4. Otherwise the cell is considered free $b_i=0$.

We can further denote the mid probability of a cell being occupied as CELL_MID.

The input parameters of the obstacle-growing function are:

1. grid_map - OccupancyGrid - grid map representation;
2. robot_size - the robot radius in meters.

The function returns

1. OccupancyGrid with the processed grid map - binary map with filtered obstacles, unknown and inaccessible areas.

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:

• Original map;
• Map grown for robot diameter 0.3 m;
• Map grown for robot diameter 0.5 m.

Suggested approach (click to view)

# Path planning

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:

1. grid_map - OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);
2. start - Pose - start robot pose;
3. goal - Pose - goal robot pose.

The function returns

1. Path with the found path or 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:

• Original map;
• Map grown for robot diameter 0.3 m;
• Map grown for robot diameter 0.5 m;

| | | |

Note that the task concerns planning in robotics and not implementing an A* planner. Hence, with a proper citation of the source in the source files, you are encouraged to use any publicly available implementation of the planner you want. Rather, get the intuition behind looking at the resulting paths and recognizing sub-optimal paths, wrongly set heuristics, and so on.

Suggested approach (click to view)

# Path Simplification

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)

You can use the provided bresenham_line function from the bresenham.py.

The input parameters of the function are

1. grid_map - OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);
2. path - Path - path to be simplified.

The function returns:

1. the Path simplified path with the processed path. The path is returned in the world coordinates!

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

• Original map;
• Map grown for robot diameter 0.3 m;
• Map grown for robot diameter 0.5 m.

Suggested approach (click to view)

#### Evaluation

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:
path_simple = planner.simplify_path(gridmap_processed, path)