Use periodic information for getLength (#55)

ethz-asl · Mar 5, 2024 · 0d03b5e · 0d03b5e
1 parent 27b19f4
commit 0d03b5e
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h
@@ -164,7 +164,7 @@ class PathSegment {
     return theta;
   }
 
-  double getLength(double epsilon = 0.001) const {
+  double getLength() const {
     double length{0.0};
     const Eigen::Vector3d segment_start = states.front().position;
     const Eigen::Vector3d segment_end = states.back().position;
@@ -177,7 +177,7 @@ class PathSegment {
       // Compute closest point on a Arc segment
       Eigen::Vector2d segment_start_2d = segment_start.head(2);
       Eigen::Vector2d segment_end_2d = segment_end.head(2);
-      if ((segment_start_2d - segment_end_2d).norm() < epsilon) {
+      if (is_periodic) {
         // Return full circle length
         length = 2 * M_PI * (1 / std::abs(curvature));
       } else {

diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp
@@ -200,7 +200,7 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) {
       double next_segment_curvature = reference_primitive_.segments[current_segment_idx + 1].curvature;
 
       /// Blend current curvature with next curvature when close to the end
-      double segment_length = current_segment.getLength(1.0);
+      double segment_length = current_segment.getLength();
       double cut_off_distance = 10.0;
       double portion = std::min(
           1.0, std::max((path_progress * segment_length - segment_length + cut_off_distance) / cut_off_distance, 0.0));