The main task is to implement the D* lite dynamic replanning algorithm.
Deadline | 3. November 2018, 23:59 PST |
Points | 5 |
Label in BRUTE | Task04 |
Files to submit | archive with robot ,gridmap and gridplanner directory |
Minimal content of the archive: robot/Robot.py , gridmap/GridMapy.py ,gridplanner/GridPlanner.py , gridplanner/DStarLitePlanner.py |
|
Resources | Task04 resource package |
Implement the D* lite incremental path planning algorithm according to the description and pseudocode presented in the Lecture 4. Grid and Graph-based path planning methods.
In file DStarLitePlanner.py
implement the 4-neighborhood D * lite algorithm. You may use the provided code and implement only the functions calculate_key
, compute_shortest_path
and update_vertex
and complete the plan
function according to the pseudocode described in Lecture 4. Grid and Graph-based path planning methods.
The implementation requirements are as follows:
DStarLitePlanner
class is inherited from the GridPlanner
class and overrides the plan(gridmap, start, goal)
method.
plan
method based on the dimensions of the passed gridmap
. The dimensions of the map are fixed and won't change.
DStarLitePlanner
class provides access to its environment representation through the getRHSvalue
and getGvalue
methods, that provide the respective values as an array that are used for proper annotation of the visualized grid map (if called without parameters).
plan
method is called whenever the path is obstructed, which forces an update of the occupancy grid map that is passed to the plan
method as the gridmap
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 (4-neighborhood is used for the algorithm) and recompute the shortest path
calculate_key
function uses manhattan distance as the heuristics
The simplified evaluation script for sole testing of the DStarLitePLanner.py
implementation is following
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import math import time import numpy as np import matplotlib.pyplot as plt from collections import deque sys.path.append('gridmap') sys.path.append('gridplanner') import GridMap as gmap import DStarLitePlanner as dplanner def plot_d_star_map(gridmap, rhs, g): """ method to plot the annotated d star lite map with rhs and g values """ plt.clf() gmap.plot_map(gridmap) ax = plt.gca() for i in range(0, gridmap.width): for j in range(0, gridmap.height): ax.annotate(g[i][j], xy=(i-0.2, j-0.1), color="red") ax.annotate(rhs[i][j], xy=(i-0.2, j+0.3), color="blue") 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 = (4, 4) goal = (9, 5) plt.ion() #fetch individual scenarios for scenario in scenarios: robot_position = start #instantiate the map gridmap = gmap.GridMap(10, 10, 1) #instantiate the d star lite planner planner = dplanner.DStarLitePlanner() #plan the route from start to goal path = planner.plan(gridmap, gridmap.world_to_map(robot_position), gridmap.world_to_map(goal)) if path == None: print("Destination unreachable") break else: path = deque(path) #plot the resulting map and the path plot_d_star_map(gridmap, planner.getRHSvalues(), planner.getGvalues()) gmap.plot_path(path) plt.pause(0.2) #locomote while robot_position != goal: #execute the step and update the map next_step = path.popleft() if next_step in scenario: #there is an obstacle in the way #update the gridmap - the cell is occupied gridmap.set_cell_p(next_step, 0.95) #replan path = planner.plan(gridmap, gridmap.world_to_map(robot_position), gridmap.world_to_map(goal)) if path == None: print("Destination unreachable") break else: path = deque(path) else: #if not, take the next step #update the gridmap - the cell is free gridmap.set_cell_p(next_step, 0.05) #take the next step robot_position = next_step #plot the resulting map and the path plot_d_star_map(gridmap, planner.getRHSvalues(), planner.getGvalues()) gmap.plot_path(path) plt.pause(0.2)