Add a getter on the internal graph of nav_graph

Needed to build path planning algorithm
Add a field t to edge structure
Initial implementation of dstar lite algorithm
Document a bit dstar