Bugfix: initialize fpl for straight arcs

ut-amrl · Jan 25, 2023 · 3b33b91 · 3b33b91
1 parent 3dd41cb
commit 3b33b91
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/src/navigation/ackermann_motion_primitives.cc b/src/navigation/ackermann_motion_primitives.cc
@@ -58,6 +58,7 @@ void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
   if (fabs(path.curvature) < kEpsilon) {
     path.length = min(nav_params.max_free_path_length, local_target.x());
+    path.fpl = path.length;
     return;
   } 
   const float turn_radius = 1.0f / path.curvature;