Fix length computation (#63)

ethz-asl · Apr 16, 2024 · 70883ff · 70883ff
1 parent 4f2eecc
commit 70883ff
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h
@@ -188,8 +188,9 @@ class PathSegment {
         Eigen::Vector2d arc_center_2d = getArcCenter(segment_start_2d, segment_start_tangent_2d, curvature);
         Eigen::Vector2d start_vector = (segment_start_2d - arc_center_2d).normalized();
         Eigen::Vector2d end_vector = (segment_end_2d - arc_center_2d).normalized();
-
-        double psi = std::atan2(end_vector(1), end_vector(0)) - std::atan2(start_vector(1), start_vector(0));
+        double psi = curvature > 0
+                         ? std::atan2(end_vector(1), end_vector(0)) - std::atan2(start_vector(1), start_vector(0))
+                         : std::atan2(start_vector(1), start_vector(0)) - std::atan2(end_vector(1), end_vector(0));
         wrap_2pi(psi);
         length = (1 / std::abs(curvature)) * psi;
       }