diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index 24d65a6a24..dfbd7187b6 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -98,7 +98,8 @@ bool CollisionChecker::isCollisionImminent( params_->min_distance_to_obstacle / std::max(std::abs(linear_vel), params_->min_approach_linear_velocity) ); - simulation_distance_limit = std::max(carrot_dist, params_->min_distance_to_obstacle); + simulation_distance_limit = std::min(std::max(carrot_dist, params_->min_distance_to_obstacle), + params_->max_lookahead_dist); } int i = 1; while (i * projection_time < max_allowed_time_to_collision_check) { diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 5bd1f2a7ee..ba497e01fc 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -189,6 +189,15 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); + if (params_.use_collision_detection && + params_.min_distance_to_obstacle > params_.max_lookahead_dist) + { + RCLCPP_WARN( + logger_, + "min_distance_to_obstacle (%.02f) is greater than max_lookahead_dist (%.02f). " + "The collision check distance will be capped by max_lookahead_dist.", + params_.min_distance_to_obstacle, params_.max_lookahead_dist); + } node->get_parameter(plugin_name_ + ".stateful", params_.stateful); if (params_.inflation_cost_scaling_factor <= 0.0) {