ros2: revert value of epsilon used in getClosestPoint (#49)

Signed-off-by: Rhys Mainwaring <[email protected]>
ethz-asl · Feb 24, 2024 · f521515 · f521515
1 parent bb971d2
commit f521515
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) {
+  double getLength(double epsilon = 1.0E-3) {
     double length{0.0};
     Eigen::Vector3d segment_start = states.front().position;
     Eigen::Vector3d segment_end = states.back().position;
@@ -198,7 +198,7 @@ class PathSegment {
   }
 
   double getClosestPoint(const Eigen::Vector3d &position, Eigen::Vector3d &closest_point, Eigen::Vector3d &tangent,
-                         double &segment_curvature, double epsilon = 0.001) {
+                         double &segment_curvature, double epsilon = 1.0E-4) {
     double theta{-std::numeric_limits<double>::infinity()};
     segment_curvature = curvature;
     Eigen::Vector3d segment_start = states.front().position;

diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp
@@ -247,7 +247,7 @@ void TerrainPlanner::cmdloopCallback() {
     double reference_curvature{0.0};
     auto current_segment = reference_primitive_.getCurrentSegment(vehicle_position_);
     double path_progress = current_segment.getClosestPoint(vehicle_position_, reference_position, reference_tangent,
-                                                           reference_curvature, 1.0);
+                                                           reference_curvature, 1.0E-4);
     // Publish global position setpoints in the global frame
     ESPG map_coordinate;
     Eigen::Vector3d map_origin;