Publish goal point from local planner.
Rip out old planner and clean up.
More cleanup and planning.
autosave
autosave
Publish nearest and goal points.