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.

In class HexapodExplorer.py implement the following basic steps of the grid-based navigation pipeline:

1. Implement the obstacle growing (method grow_obstacles) to take into account 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) which purpose is to provide a smoother path for the robot

The detailed description and guidelines on individual steps follow.

The purpose of the grow_obstacles function is to pre-process the grid map into a binary map by rejecting obstacles, unknown and inaccessible areas for the robot given its embodiment. The robot is considered to be a circle with $r_\mathrm{robot}$ radius robot_size. The result of the obstacle growing is a binary map with the used binary values 1 (for occupied, inaccessible and unknown cells) and 0 (for traversable cells). For each grid_map cell $m_i$ 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 the following rules:

1. Filtering all obstacles - if $P(m_i = occupied \vert z) > 0.5$ the cell is considered occupied $b_i$ = 1
2. Filtering all unknown areas - if $P(m_i = occupied \vert z) = 0.5$ 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 (considering precise euclidean distance on the grid is not practical, therefore approximations using mathematical morphology or distance transform are used) of the cell $m_i$ and the closest obstacle is lower then $r_\mathrm{robot}$, then the cell is considered occupied $b_i$ = 1
4. Otherwise the cell is considered free $b_i=0$

The input parameters of the function are:

1. grid_map - OccupancyGrid message - grid map representation
2. robot_size - the robot radius in metres

The function returns:

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

The grow_obstacles function in the HexapodExplorer.py class has a following prescription:

      def grow_obstacles(self, grid_map, robot_size):
        """ Method to grow the obstacles to take into account the robot embodiment
        Args:
            grid_map: OccupancyGrid - gridmap for obstacle growing
            robot_size: float - size of the robot
        Returns:
            grid_map_grow: OccupancyGrid - gridmap with considered robot body embodiment
        """

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

Suggested approach (click to view)

The purpose of the plan_path function is to plan the path between the start and goal poses on the pre-processed grid map considering planning with 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 function are:

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

The function returns:

1. the Path message with the found path or None when the path cannot be found. It is supposed that the path contains only 2D coordinates and not the heading, i.e., 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 HexapodExplorer.py class has a following prescription:

    def plan_path(self, grid_map, start, goal):
        """ Method to plan the path from start to the goal pose on the grid
        Args:
            grid_map: OccupancyGrid - gridmap 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
        """

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

Suggested approach (click to view)

Path simplification is usually done by excluding navigation points from the path, that are not necessary in the sense that the robot does not have to visit them precisely. A typical approach to trajectory smoothing is to connect the neighboring segments one by one using straight-line segments (using Bresenham line algorithm) up to the point where the straight-line segment collide with an obstacle (grown obstacle) and then follow with another straight-line segment.

The input parameters of the function are:

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

The function returns:

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

The simplify_path function in the HexapodExplorer.py class has a following prescription:

      def simplify_path(self, grid_map, path):
        """ Method to simplify the found path on the grid
        Args:
            grid_map: OccupancyGrid - gridmap for obstacle growing
            path: Path - path to be simplified
        Returns:
            path_simple: Path - simplified path
        """

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

Suggested approach (click to view)

The evaluation focus on correct behavior of the above described robot navigation pipeline.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
 
import matplotlib.pyplot as plt
 
import sys
import os
import math
import numpy as np
 
sys.path.append('hexapod_robot')
sys.path.append('hexapod_explorer')
 
#import hexapod robot and explorer
import HexapodRobot as hexapod
import HexapodExplorer as explorer
 
#import communication messages
from messages import *
 
#pickle
import pickle
 
ROBOT_SIZE = 0.3
 
 
if __name__=="__main__":
 
    #instantiate the explorer robot
    explor = explorer.HexapodExplorer()
 
    #load scenarios
    scenarios =  pickle.load( open( "scenarios.bag", "rb" ) )
 
    #evaluate individual scenarios
    for gridmap, start, goal in scenarios:
        #obstacle growing
        gridmap_processed = explor.grow_obstacles(gridmap, ROBOT_SIZE)
 
        #path planning
        path = explor.plan_path(gridmap_processed, start, goal)
 
        #path simplification
        path_simple = explor.simplify_path(gridmap_processed, path)
 
        #plot the result
        def plot_path(path, ax, clr):
            """ simple method to draw the path
            """
            if path is not None:
                poses = np.asarray([(pose.position.x,pose.position.y) for pose in path.poses])
                ax.plot(poses[:,0], poses[:,1], '.',color=clr)
                ax.plot(poses[:,0], poses[:,1], '-',color=clr)
 
        fig, ax = plt.subplots()
        gridmap.plot(ax)
        plot_path(path, ax, 'r')
        plot_path(path_simple, ax, 'b')
 
        plt.xlabel('x[m]')
        plt.ylabel('y[m]')
        plt.axis('square')
        plt.show()