SYSTEM FOR WAYPOINT SELECTION AND METHOD OF USING

A waypoint selection system includes a non-transitory computer readable medium configured to store instructions thereon; and a processor connected to the non-transitory computer readable medium. The processor is configured to execute the instructions for obtaining information related to a current pose of a vehicle; obtaining a plurality of waypoints between the current pose and a destination pose; and receiving obstacle information. The processor is configured to execute the instructions for determining a first waypoint cost for a first waypoint of the plurality of waypoints. The processor is configured to execute the instructions for selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The processor is configured to execute the instructions for outputting the selected first waypoint and all waypoints of the plurality of waypoints between the first waypoint and the current pose to a controller in response to selecting of the first waypoint.