T1x-dstar - Incremental path planning (D* Lite)

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.

Due date January 11, 2025, 23:59 PST
Deadline January 11, 2025, 23:59 PST
Points 5 (Bonus points)
Label in BRUTE t1x-dstar
Files to submit archive with HexapodExplorer.py file
Resources B4M36UIR_t1_resource_pack

Assignment

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

Approach

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)


Example behavior

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


Evaluation

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)

courses/uir/hw/t1x-dstar.txt · Last modified: 2024/09/16 20:13 by pragrmi1