Finishing

- BUGS with waiting waypoints