Implemented addWaypoint (not so much working yet).