Implemented following of waypoints