Added waypoint navigation