Implemented the waypoints navigation