Pathfinder added

A working D* Lite without dynamic replanning.