Fix issue with multiple waypoints