Extract path segments (#7)

Jaeyoung-Lim · web-flow · commit c6dedc0cc5e8 · 2025-09-23T13:36:54.000-07:00
* WIP extract path segments

Fix path segment extraction

* Fix path visualizations

* Add dubins path planner example

* Fix parsing velocity states

* Add back loiter path segment
diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h
@@ -69,7 +69,7 @@ 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--) {
+    for (int i = trajectory_segments.segments.size() - 1; i > -1; i--) {
       prependSegment(trajectory_segments.segments[i]);
     }
   }
diff --git a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h
@@ -202,6 +202,10 @@ class TerrainOmplRrt {
     min_altitude_ = min_altitude;
   }
 
+  PathSegment extractPathSegment(ompl::base::State* from, ompl::base::State* to,
+                                 ompl::base::OwenStateSpace::PathType& path, double t_start = 0.0, double t_end = 1.0,
+                                 double dt = 0.01) const;
+
  private:
   // double minimum_turning_radius_{66.67};
   std::shared_ptr<ompl::OmplSetup> problem_setup_;
diff --git a/terrain_planner/src/terrain_ompl_rrt.cpp b/terrain_planner/src/terrain_ompl_rrt.cpp
@@ -333,6 +333,24 @@ bool TerrainOmplRrt::getSolutionPath(std::vector<Eigen::Vector3d>& path) {
 //   return segment_curvature;
 // }
 
+PathSegment TerrainOmplRrt::extractPathSegment(ompl::base::State* from, ompl::base::State* to,
+                                               ompl::base::OwenStateSpace::PathType& path, double t_start, double t_end,
+                                               double dt) const {
+  ompl::base::State* state = problem_setup_->getStateSpace()->allocState();
+  PathSegment trajectory;
+  for (double t = t_start; t <= t_end; t += dt) {
+    // Append to trajectory
+    problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->interpolate(from, to, t, path, state);
+    State segment_state;
+    segment_state.position = dubinsairplanePosition(state);
+    double yaw = dubinsairplaneYaw(state);
+    Eigen::Vector3d velocity = Eigen::Vector3d(std::cos(yaw), std::sin(yaw), 0.0);
+    segment_state.velocity = velocity;
+    trajectory.states.emplace_back(segment_state);
+  }
+  return trajectory;
+}
+
 void TerrainOmplRrt::solutionPathToPath(ompl::geometric::PathGeometric path, Path& trajectory_segments,
                                         double resolution) const {
   trajectory_segments.segments.clear();
@@ -341,72 +359,64 @@ void TerrainOmplRrt::solutionPathToPath(ompl::geometric::PathGeometric path, Pat
   for (size_t idx = 0; idx < state_vector.size() - 1; idx++) {
     auto from = state_vector[idx];    // Start of the segment
     auto to = state_vector[idx + 1];  // End of the segment
-    auto path = problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->getPath(from, to);
-    // ompl::base::OwenStateSpace::SegmentStarts segmentStarts;
-    // problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->calculateSegments(
-    //     from, to, dubins_path, segmentStarts);
-
-    ompl::base::State* segment_start_state = problem_setup_->getStateSpace()->allocState();
-    ompl::base::State* segment_end_state = problem_setup_->getStateSpace()->allocState();
-
-    // const double total_length = dubins_path.length_2D();
-    // const double dt = resolution / dubins_path.length_2D();
-    double progress{0.0};
-    //   for (size_t start_idx = 0; start_idx < segmentStarts.segmentStarts.size(); start_idx++) {
-    //     if (dubins_path.getSegmentLength(start_idx) > 0.0) {
-    //       double segment_progress = dubins_path.getSegmentLength(start_idx) / total_length;
-    //       // Read segment start and end statess
-    //       segmentStart2omplState(segmentStarts.segmentStarts[start_idx], segment_start_state);
-    //       if ((start_idx + 1) > (segmentStarts.segmentStarts.size() - 1)) {
-    //         segment_end_state = to;
-    //       } else if ((start_idx + 1) > (segmentStarts.segmentStarts.size() - 2) &&
-    //                  dubins_path.getSegmentLength(start_idx + 1) == 0.0) {
-    //         segment_end_state = to;
-    //       } else {
-    //         segmentStart2omplState(segmentStarts.segmentStarts[start_idx + 1], segment_end_state);
-    //       }
-
-    //       // Append to trajectory
-    //       PathSegment trajectory;
-    //       trajectory.curvature = getSegmentCurvature(problem_setup_, dubins_path, start_idx);
-    //       ompl::base::State* state = problem_setup_->getStateSpace()->allocState();
-    //       trajectory.flightpath_angle = dubins_path.getGamma();
-    //       double yaw;
-    //       double track_progress{0.0};
-    //       for (double t = progress; t <= progress + segment_progress; t = t + dt) {
-    //         State segment_state;
-    //         problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->interpolate(
-    //             dubins_path, segmentStarts, t, state);
-    //         Eigen::Vector3d position = dubinsairplanePosition(state);
-    //         yaw = dubinsairplaneYaw(state);
-    //         Eigen::Vector3d velocity = Eigen::Vector3d(std::cos(yaw), std::sin(yaw), 0.0);
-    //         segment_state.position = position;
-    //         segment_state.velocity = velocity;
-    //         segment_state.attitude = Eigen::Vector4d(std::cos(yaw / 2.0), 0.0, 0.0, std::sin(yaw / 2.0));
-    //         trajectory.states.emplace_back(segment_state);
-    //         track_progress = t;
-    //       }
-    //       // Append end state
-    //       if (((start_idx + 1) > (segmentStarts.segmentStarts.size() - 1)) ||
-    //           ((start_idx + 1) > (segmentStarts.segmentStarts.size() - 2) &&
-    //            dubins_path.getSegmentLength(start_idx + 1) == 0.0)) {
-    //         // Append segment with last state
-    //         State end_state;
-    //         Eigen::Vector3d end_position = dubinsairplanePosition(segment_end_state);
-    //         double end_yaw = dubinsairplaneYaw(segment_end_state);
-    //         Eigen::Vector3d end_velocity = Eigen::Vector3d(std::cos(end_yaw), std::sin(end_yaw), 0.0);
-    //         end_state.position = end_position;
-    //         end_state.velocity = end_velocity;
-    //         end_state.attitude = Eigen::Vector4d(std::cos(end_yaw / 2.0), 0.0, 0.0, std::sin(end_yaw / 2.0));
-    //         trajectory.states.emplace_back(end_state);
-    //       }
-    //       progress = track_progress;
-    //       // Do not append trajectory if the segment is too short
-    //       if (trajectory.states.size() > 1) {
-    //         trajectory_segments.segments.push_back(trajectory);
-    //       }
-    //     }
-    //   }
+    auto dubins_path = problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->getPath(from, to);
+
+    if (dubins_path->phi_ == 0.) {
+      if (dubins_path->numTurns_ == 0) {
+        // Low path case
+        double t_start{0.0};
+        double t_end{0.0};
+        double length = dubins_path->path_.length();
+        for (int idx = 0; idx < 3; idx++) {
+          t_end = idx == 2 ? 1.0 : dubins_path->path_.length_[idx] / length + t_start;
+          auto trajectory = extractPathSegment(from, to, *dubins_path, t_start, t_end);
+          if (trajectory.states.size() > 1) {
+            trajectory_segments.segments.push_back(trajectory);
+          }
+          t_start = t_end;
+        }
+      } else {
+        // high altitude path
+
+        // Parse Trochoidal periodic paths
+        double lengthPeriodicPath = 2.0 * M_PI * dubins_path->turnRadius_;
+        auto lengthPath = dubins_path->path_.length();
+        auto lengthTotal = lengthPath + lengthPeriodicPath * dubins_path->numTurns_;
+        double t_start, t_end;
+        for (int k = 0; k < dubins_path->numTurns_; k++) {
+          /// TODO: Further divide this into segments
+          t_start = lengthPeriodicPath * k / lengthTotal;
+          t_end = lengthPeriodicPath * (k + 1) / lengthTotal;
+          auto trajectory = extractPathSegment(from, to, *dubins_path, t_start, t_end);
+          if (trajectory.states.size() > 1) {
+            trajectory_segments.segments.push_back(trajectory);
+          }
+        }
+        // Path
+        t_start = lengthPeriodicPath * dubins_path->numTurns_ / lengthTotal;
+        auto trajectory = extractPathSegment(from, to, *dubins_path, t_start, 1.0);
+        if (trajectory.states.size() > 1) {
+          trajectory_segments.segments.push_back(trajectory);
+        }
+      }
+    } else {
+      // medium altitude path
+      auto lengthTurn = std::abs(dubins_path->phi_);
+      auto lengthPath = dubins_path->path_.length();
+      auto lengthTotal = lengthTurn + lengthPath;
+      {
+        auto trajectory = extractPathSegment(from, to, *dubins_path, 0.0, lengthTurn / lengthTotal);
+        if (trajectory.states.size() > 1) {
+          trajectory_segments.segments.push_back(trajectory);
+        }
+      }
+      {
+        auto trajectory = extractPathSegment(from, to, *dubins_path, lengthTurn / lengthTotal, 1.0);
+        if (trajectory.states.size() > 1) {
+          trajectory_segments.segments.push_back(trajectory);
+        }
+      }
+    }
   }
 }
 
diff --git a/terrain_planner_benchmark/launch/config.rviz b/terrain_planner_benchmark/launch/config.rviz
@@ -4,7 +4,9 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /GridMap1
         - /GridMap1/ElevationGridMap1
+        - /GridMap1/AvalancheGridMap1
         - /Planning1
       Splitter Ratio: 0.5
     Tree Height: 930
@@ -145,15 +147,15 @@ Visualization Manager:
             Value: /grid_map
           Use Rainbow: true
           Value: false
-        - Alpha: 0.8999999761581421
+        - Alpha: 0.5
           Autocompute Intensity Bounds: true
           Class: grid_map_rviz_plugin/GridMap
           Color: 200; 200; 200
           Color Layer: elevation
-          Color Transformer: GridMapLayer
-          Enabled: false
+          Color Transformer: IntensityLayer
+          Enabled: true
           Height Layer: elevation
-          Height Transformer: GridMapLayer
+          Height Transformer: Layer
           History Length: 1
           Invert Rainbow: false
           Max Color: 255; 255; 255
@@ -168,9 +170,9 @@ Visualization Manager:
             Filter size: 10
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /avalanche_map
+            Value: /grid_map
           Use Rainbow: true
-          Value: false
+          Value: true
       Enabled: true
       Name: GridMap
     - Class: rviz_common/Group
@@ -239,7 +241,7 @@ Visualization Manager:
           Enabled: true
           Name: PathSegmentsMarkerArray
           Namespaces:
-            {}
+            normals: true
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -263,7 +265,7 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz_default_plugins/Path
           Color: 255; 255; 0
-          Enabled: true
+          Enabled: false
           Head Diameter: 0.30000001192092896
           Head Length: 0.20000000298023224
           Length: 0.30000001192092896
@@ -286,7 +288,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /path
-          Value: true
+          Value: false
       Enabled: true
       Name: Planning
   Enabled: true
@@ -336,25 +338,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 3363.000244140625
+      Distance: 4154.14404296875
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -324.49951171875
-        Y: -279.8844299316406
-        Z: 1438.21044921875
+        X: -205.7038116455078
+        Y: -69.17835235595703
+        Z: 174.4804229736328
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7297971248626709
+      Pitch: 0.45479774475097656
       Target Frame: map
       Value: Orbit (rviz_default_plugins)
-      Yaw: 0.7050155997276306
+      Yaw: 3.7250149250030518
     Saved: ~
 Window Geometry:
   Displays:
@@ -364,7 +366,7 @@ Window Geometry:
   Hide Right Dock: true
   PlanningPanel:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003ddfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000078000003dd000000c700fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0000000263000001f20000014e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000006e00ffffff000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000064000002da000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073a00000037fc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000005de000003dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003ddfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000078000003dd000000c700fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0000000263000001f20000006e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000006e00ffffff000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000064000002da000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073a00000037fc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000005de000003dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Teleop:
@@ -376,5 +378,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1850
-  X: 872
-  Y: 1467
+  X: 70
+  Y: 27
diff --git a/terrain_planner_benchmark/launch/test_ompl_rrt_circle_launch.xml b/terrain_planner_benchmark/launch/test_ompl_rrt_circle_launch.xml
@@ -3,8 +3,10 @@
   <!-- <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/> -->
   <node pkg="terrain_planner_benchmark" exec="test_rrt_circle_goal" name="rrt_planner" output="screen">
     <param name="location" value="sertig"/>
-    <param name="map_path" value="$(find-pkg-share terrain_models)/models/sertig.tif"/>
-    <param name="color_file_path" value="$(find-pkg-share terrain_models)/models/sertig_color.tif"/>
+    <param name="map_path" value="/home/jaeyoung/dev/terrain-loader/downloads/berkeley_elevation.tif"/>
+    <param name="color_file_path" value="/home/jaeyoung/dev/terrain-loader/downloads/berkeley_ortho.tif"/>
+    <!-- <param name="map_path" value="$(find-pkg-share terrain_models)/models/sertig.tif"/>
+    <param name="color_file_path" value="$(find-pkg-share terrain_models)/models/sertig_color.tif"/> -->
     <param name="output_directory" value="$(find-pkg-share terrain_planner_benchmark)/../output"/>
   </node>
   <node pkg="rviz2" exec="rviz2" name="rviz" args="-d $(find-pkg-share terrain_planner_benchmark)/launch/config.rviz" />
diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp

Original file line number	Diff line number	Diff line change
`@@ -69,7 +69,7 @@ class Path {`
`69`	`69`	`void resetSegments() { segments.clear(); };`
`70`	`70`	`void prependSegment(const PathSegment &trajectory) { segments.insert(segments.begin(), trajectory); };`
`71`	`71`	`void prependSegment(const Path &trajectory_segments) {`
`72`		`- for (int i = trajectory_segments.segments.size() -1; i > -1; i--) {`
	`72`	`+ for (int i = trajectory_segments.segments.size() - 1; i > -1; i--) {`
`73`	`73`	`prependSegment(trajectory_segments.segments[i]);`
`74`	`74`	`}`
`75`	`75`	`}`