Path planner now converts A* solution to a set of commands