Make sure fixed curvature lookahead distance doesn't overshoot distance to cusp (#5134)

Related issue: #5098

Author: moooeeeep

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -187,6 +187,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
   // Find look ahead distance and point on path and publish
   double lookahead_dist = getLookAheadDistance(speed);
+  double curv_lookahead_dist = params_->curvature_lookahead_dist;
 
   // Check for reverse driving
   if (params_->allow_reversing) {
@@ -197,6 +198,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     if (dist_to_cusp < lookahead_dist) {
       lookahead_dist = dist_to_cusp;
     }
+    if (dist_to_cusp < curv_lookahead_dist) {
+      curv_lookahead_dist = dist_to_cusp;
+    }
   }
 
   // Get the particular point on the path at the lookahead distance
@@ -211,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   double regulation_curvature = lookahead_curvature;
   if (params_->use_fixed_curvature_lookahead) {
     auto curvature_lookahead_pose = getLookAheadPoint(
-      params_->curvature_lookahead_dist,
+      curv_lookahead_dist,
       transformed_plan, params_->interpolate_curvature_after_goal);
     rotate_to_path_carrot_pose = curvature_lookahead_pose;
     regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position);