Add prepend path api

F

Author: Jaeyoung-Lim

terrain_navigation/include/terrain_navigation/path.h

@@ -68,6 +68,11 @@ class Path {
   }
   void resetSegments() { segments.clear(); };
   void prependSegment(const PathSegment &trajectory) { segments.insert(segments.begin(), trajectory); };
+  void prependSegment(const Path &trajectory_segments) {
+    for (int i = trajectory_segments.segments.size() -1; i > -1; i--) {
+      prependSegment(trajectory_segments.segments[i]);
+    }
+  }
   void appendSegment(const PathSegment &trajectory) { segments.push_back(trajectory); };
   void appendSegment(const Path &trajectory_segments) {
     for (const auto &trajectory : trajectory_segments.segments) {