[planning-cpp] Additional methods to manipulate Waypoints
[planning-cpp] Additional methods to manipulate Waypoints

fire-rs-laas/fire-rs-saop,fire-rs-laas/fire-rs-saop,fire-rs-laas/fire-rs-saop,fire-rs-laas/fire-rs-saop