Starting to implement waypoint navigation