The main task is to implement an incremental grid-based path planning approach. You will learn how the pre-calculated planning data could be effectively reused to update the path plan in case there are unexpected changes in the map, hence, it is not required to start with the planning from scratch.

In class HexapodExplorer.py implement the plan_path_incremental function. The purpose of the function is to find the collision free path between the start (robot position) and goal position considering 8-neigborhood using D* lite algorithm, according to the pseudocode described in Lecture 3. Path planning.

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!
2. rhs - float[] values of one-step lookahead objective function $rhs$ for individual map cells in row-major order
3. g - float[] values of objective function $g$ for individual map cells in row-major order

The plan_path_incremental function in the HexapodExplorer.py class has a following prescription with the minimum working code:

    def plan_path_incremental(self, grid_map, start, goal):
        """ Method for incremental path planning 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
            rhs: float[] - one-step lookahead objective function in row-major order
            g: float[] - objective function value in row-major order
        """
        if not hasattr(self, 'rhs'): #first run of the function
            self.rhs = np.full((grid_map.height, grid_map.width), np.inf)
            self.g = np.full((grid_map.height, grid_map.width), np.inf)
 
        return self.plan_path(grid_map, start, goal), rhs.flatten(), g.flatten()

The implementation requirements

1. For implementation of D* lite planner, the class HexapodExplorer has to keep its own representation of the environment in a form of map that is initialized to proper dimensions during the first call of the plan_path_incremental method based on the dimensions of the passed OccupancyGrid map. The dimensions of the map are fixed and won't change.
2. The plan_path_incremental function returns as a debugging output the array of $rhs$ and $g$ values used for proper annotation of the visualized grid map
3. The plan_path_incremental method is called whenever the path is obstructed, which forces an update of the occupancy grid map that is passed to the plan_path_incremental method as the grid_map parameter. The planner has to figure out what has been changed in the map and replan accordingly, i.e., it has to figure out the coordinates of the updated cells, update all the affected $rhs$ and $g$ values in its neighborhoods (8-neighborhood is used for the algorithm) and recompute the shortest path

Follow the pseudocode for the D* lite algorithm described in Lecture 3. Path planning, also to be read in the related paper Koenig, S. and Likhachev, M. (2005): Fast Replanning for Navigation in Unknown Terrain. T-RO.

Detailed description, guidance, and suggestions (Click to view)

...
class HexapodExplorer:
    def __init__(self):
        self.gridmap = None
        self.rhs = None
        self.g = None
...

    def calculate_key(self, coord, goal):
        """method to calculate the priority queue key
        Args:  coord: (int, int) - cell to calculate key for
               goal: (int, int) - goal location
        Returns:  (float, float) - major and minor key
        """
        #think about different heuristics and how they influence the steering of the algorithm
        #heuristics = 0
        #heuristics = L1 distance
        #heuristics = L2 distance
 
        return 0
 
    def compute_shortest_path(self, start, goal):
        """Function to compute the shortest path
        Args:  start: (int, int) - start position
               goal: (int, int) - goal position
        """
        #while not finished
            #fetch coordinates u from priority queue
 
            #if g value at u > rhs value at u:
                #node is over consistent    
            #else:
                # node is under consistent
 
    def update_vertex(self, u, start, goal):
        """ Function for map vertex updating
        Args:  u: (int, int) - currently processed position
               start: (int, int) - start position
               goal: (int, int) - goal position
        """
        pass
 
    def reconstruct_path(self, start, goal):
        """Function to reconstruct the path
        Args:  start: (int, int) - start position
               goal: (int, int) - goal position
        Returns:  Path - the path
        """
        return None
 
    def neighbors8(self, coord):
        """Returns coordinates of passable neighbors of the given cell in 8-neighborhood
        Args:  coord : (int, int) - map coordinate
        Returns:  list (int,int) - list of neighbor coordinates 
        """
        return (0,0)

    #priority queue class
    class PriorityQueue:
        def __init__(self):
            self.elements = []
 
        def empty(self):
            return len(self.elements) == 0
 
        def put(self, item, priority):
            heapq.heappush(self.elements, (priority, item))
 
        def get(self):
            return heapq.heappop(self.elements)[1]
 
        def top(self):
            u = self.elements[0]
            return u[1]
 
        def contains(self, element):
            ret = False
            for item in self.elements:
                if element == item[1]:
                    ret = True
                    break
            return ret
 
        def print_elements(self):
            print(self.elements)
            return self.elements
 
        def remove(self, element):
            i = 0
            for item in self.elements:
                if element == item[1]:
                    self.elements[i] = self.elements[-1]
                    self.elements.pop()
                    heapq.heapify(self.elements)
                    break
                i += 1

The following videos show the example behavior of the D* lite planner with steering heuristics $h=0$ on the example scenarios.

The code can be evaluated using the following script (also attached as t1e-expl.py).

#!/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 *
 
 
def plot_path(ax, path, 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)
 
def plot_dstar_map(ax, grid_map, rhs_=None, g_=None):
    """method to plot the gridmap with rhs and g values
    """
    #plot the gridmap
    gridmap.plot(ax)
    plt.grid(True, which='major')
 
    rhs = rhs_.reshape(grid_map.height, grid_map.width)
    g = g_.reshape(grid_map.height, grid_map.width)
    #annotate the gridmap graph with g and rhs values
    if g is not None and rhs is not None:
        for i in range(0, gridmap.width):
            for j in range(0, gridmap.height):
                ax.annotate("%.2f" % g[i,j], xy=(i+0.1, j+0.2), color="red")
                ax.annotate("%.2f" % rhs[i,j], xy=(i+0.1, j+0.6), color="blue")
 
def check_collision(gridmap, pose, scenario):
    """ method to check whether the robot is in collision with newly discovered obstacle
    """
    if to_grid(gridmap, pose) in scenario:
        return True
    else:
        return False
 
def to_grid(grid_map, pose):
    """method to transform world coordinates to grid coordinates
    """
    cell = ((np.asarray((pose.position.x, pose.position.y)) - (grid_map.origin.position.x, grid_map.origin.position.y)) / grid_map.resolution)
    return  (int(cell[0]), int(cell[1]))
 
 
if __name__=="__main__":
 
    #define planning problems:
    scenarios = [([(1, 0), (1, 1), (1, 2), (1, 3), (2, 3), (3, 3), (4, 3), (4, 2), (4, 1), (5, 1), (6, 1), (6, 2), (6, 3), (6, 4), (6, 5), (6, 6), (5, 6), (4, 6), (3, 6), (2, 6), (1, 6), (7, 1), (8, 1), (8, 2), (8, 3), (8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (7, 8), (6, 8), (5, 8), (4, 8), (3, 8), (2, 8), (1, 8)]),
                 ([(1, 0), (1, 1), (1, 2), (1, 3), (1, 4), (1, 5), (1, 6), (1, 7), (1, 8), (3, 9), (3, 8), (3, 7), (3, 6), (3, 5), (3, 4), (3, 3), (3, 2), (3, 1), (5, 0), (5, 1), (5, 2), (5, 3), (5, 4), (5, 5), (5, 6), (5, 7), (5, 8), (7, 9), (7, 8), (7, 7), (7, 6), (7, 5), (7, 4), (7, 3), (7, 2), (7, 1), (8, 1)]),
                 ([(1, 1), (2, 1), (3, 1), (4, 1), (5, 1), (7, 1), (8, 1), (6, 1), (8, 2), (8, 3), (8, 4), (7, 4), (7, 5), (7, 6), (9, 6), (8, 6), (8, 7), (8, 8), (7, 8), (6, 8), (5, 8), (4, 8), (3, 8), (2, 8), (1, 8), (1, 7), (1, 6), (1, 4), (1, 3), (1, 2)])]
 
    start = Pose(Vector3(4.5, 4.5, 0),Quaternion(0,0,0,1))
    goal = Pose(Vector3(9.5, 5.5, 0),Quaternion(0,0,0,1))
 
    #prepare plot
    fig, ax = plt.subplots(figsize=(10,10))
    plt.ion()
 
    cnt = 0
    #fetch individual scenarios
    for scenario in scenarios:
        robot_odometry = Odometry()
        robot_odometry.pose = start
 
        robot_path = Path()
        robot_path.poses.append(start)
 
        #instantiate the explorer robot
        explor = explorer.HexapodExplorer()
 
        #instantiate the map
        gridmap = OccupancyGrid()
        gridmap.resolution = 1
        gridmap.width = 10
        gridmap.height = 10
        gridmap.origin = Pose(Vector3(0,0,0), Quaternion(0,0,0,1))
        gridmap.data = np.zeros((gridmap.height*gridmap.width))
 
        while not (robot_odometry.pose.position.x == goal.position.x and
               robot_odometry.pose.position.y == goal.position.y):
            #plan the route from start to goal
            path, rhs, g = explor.plan_path_incremental(gridmap, robot_odometry.pose, goal)
            if path == None:
                print("Destination unreachable")
                break
            if len(path.poses) < 1:
                print("There is nowhere to go")
                break
 
            #check for possibility of the move
            if check_collision(gridmap, path.poses[1], scenario):
                #add the obstacle into the gridmap 
                cell = to_grid(gridmap, path.poses[1])
                data = gridmap.data.reshape(gridmap.height, gridmap.width)
                data[cell[1],cell[0]] = 1
                gridmap.data = data.flatten()
            else:
                #move the robot
                robot_odometry.pose = path.poses[1]
 
            robot_path.poses.append(robot_odometry.pose)
 
            #plot it
            plt.cla()
            plot_dstar_map(ax, gridmap, rhs, g)
            plot_path(ax, path, 'r')
            plot_path(ax, robot_path, 'b')
            robot_odometry.pose.plot(ax)
            plt.xlabel('x[m]')
            plt.ylabel('y[m]')
            plt.axis('square')
 
            #optional save of the image for debugging purposes
            #fig.savefig("result/%04d.png" % cnt, bbox_inches='tight')
            cnt += 1
            plt.show()
            plt.pause(0.1)