Added waypoint support