diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index edcd6db5583..8567dd5a7a3 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/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);