Lab05 - Incremental Path Planning

Motivations and Goals
Become familiar with advanced methods of grid based path planning and dynamic replanning
Be able to dynamically replan the motion based on the robot feedback from the environment
Tasks (teacher)
Implement and verify the functionality of the D* Lite algorithm (4 points)
Lab resources
Lab sripts: lab05 resource files

D* Lite algorithm (4 points)

The pseudocode and visualization of the algorithm is in Lecture 4. Grid and Graph-based path planning methods slides

The task is to implement a D* Lite algorithm with 4 neighborhood search within the provided d_star_lite_impelmentation. The visualization shall show the map, the g(s) and rhs(s) values for each s in the grid map for each step. Implement the following functions to cope with the algorithm:

def compute_shortest_path(grid_map, U, start, goal)
def update_vertex(grid_map, U, u, start, goal)

and extend the main method to implement map update after obstacle detection

if not grid_map.passable(start_prime):
    #the path is blocked, it is necessary to update weights
    """
    put your code for weights update here
    """
else:
    #move towards the target
    start = start_prime

Your solution submit to the upload system under the Lab05 assignment for evaluation.