takes waypoints one at a time, better planner