A* working, issues with finding nearest waypoint to robot's current pose unresolved.