Search
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.
HexapodExplorer.py
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.
plan_path_incremental
The input parameters of the function are:
grid_map
start
goal
The function returns:
None
position.x
position.y
rhs
float[]
g
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
HexapodExplorer
OccupancyGrid
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)
As it is shown above in the prescription of the function, the incremental planning can be achieved by simply replaning each time the map changes and the currently planned path is blocked. However, in this task we want to reuse what has been already calculated and only adjust the current plan to deal with the new observations.
The D* lite stores values of one-step lookahead objective function $rhs$ and objective function $g$ for individual map cells between individual runs of the plan_path_incremental method. Therefore it is suggested to create the storage variables for these values either as static variables of the method, or during the first run time of the function in the similar way shown above, or during the initialization of the class as:
... class HexapodExplorer: def __init__(self): self.gridmap = None self.rhs = None self.g = None ...
1
The next step cope with the pseudocode described in Lecture 3. Path planning. As a suggestion, prepare the following helper functions and then fill them gradually with code. Note, that inside the HexapodExplorer class we are using simple coordinate tuples to index the gridmap, $rhs$ and $g$ arrays.
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)
You will also need a priority queue. A possible implementation of the priority queue follows.
#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).
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)