## 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.

 Deadline 15. November 2020, 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 [Updated 12.10.2020] T1x_dstar evaluation 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), int(cell))

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))
#start = Pose(Vector3(1.5, 1.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()
plt.ion()

#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, scenario):
#add the obstacle into the gridmap
cell = to_grid(gridmap, path.poses)
data = gridmap.data.reshape(gridmap.height, gridmap.width)
data[cell,cell] = 1
gridmap.data = data.flatten()
else:
#move the robot
robot_odometry.pose = path.poses

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')
plt.show()
plt.pause(1) 