Bugfix: intermediate state for straight arcs

ut-amrl · Jan 25, 2023 · 7639201 · 7639201
1 parent 3b33b91
commit 7639201
Showing 1 changed file with 2 additions and 12 deletions.
diff --git a/src/navigation/constant_curvature_arcs.cc b/src/navigation/constant_curvature_arcs.cc
@@ -76,24 +76,14 @@ Pose2Df ConstantCurvatureArc::GetIntermediateState(float f) const {
   }
   if (fabs(curvature) < FLT_MIN) {
     // Straight-line motion
-    return Pose2Df(0, Vector2f(length, 0));
+    return Pose2Df(0, Vector2f(f * length, 0));
   }
   const float r = 1.0 / curvature;
   return Pose2Df(a, r * Vector2f(sin(a), 1.0 - cos(a)));
 }
 
 Pose2Df ConstantCurvatureArc::EndPoint() const {
-  const float a = Sign(curvature) * angular_length;
-  if (length == 0) {
-    // Pure rotational motion.
-    return Pose2Df(a, Vector2f(0, 0));
-  }
-  if (fabs(curvature) < FLT_MIN) {
-    // Straight-line motion
-    return Pose2Df(0, Vector2f(length, 0));
-  }
-  const float r = 1.0 / curvature;
-  return Pose2Df(a, r * Vector2f(sin(a), 1.0 - cos(a)));
+  return GetIntermediateState(1.0);
 }
 
 }  // namespace motion_primitives