====== 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 ([[courses:b4m36uir:internal:instructions:lab05|teacher]]) ^
| Implement and verify the functionality of the D* Lite algorithm **(4 points)** |
^ Lab resources  ^
| Lab sripts: {{:courses:b4m36uir:labs:lab05.zip|lab05 resource files}}|

==== D* Lite algorithm (4 points) ====
The pseudocode and visualization of the algorithm is in {{courses:b4m36uir:lectures:b4m36uir-lec04-slides.pdf| 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 {{courses:b4m36uir:lectures:b4m36uir-lec04-slides.pdf| algorithm}}:
<code python>
def compute_shortest_path(grid_map, U, start, goal)
def update_vertex(grid_map, U, u, start, goal)
</code>
and extend the main method to implement map update after obstacle detection
<code python>
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
</code>

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