fix global planner to abort gracefully and restart waypoints