diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp index 4b835ecf345..0224509ebe2 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -62,12 +62,13 @@ class CollisionChecker * @param linear_vel linear velocity to forward project * @param angular_vel angular velocity to forward project * @param carrot_dist Distance to the carrot for PP + * @param dist_to_path_end Distance used to regulate min_distance_to_obstacle * @return Whether collision is imminent */ bool isCollisionImminent( const geometry_msgs::msg::PoseStamped &, const double &, const double &, - const double &); + const double &, const double &); /** * @brief checks for collision at projected pose diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index e6626c2244c..e0b918e60bc 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -60,6 +60,7 @@ struct Parameters double max_robot_pose_search_dist; bool interpolate_curvature_after_goal; bool use_collision_detection; + bool use_path_aware_obstacle_distance; double transform_tolerance; bool stateful; }; diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index 8340bb5a1ce..0f434fd8cea 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -48,7 +48,7 @@ CollisionChecker::CollisionChecker( bool CollisionChecker::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const double & linear_vel, const double & angular_vel, - const double & carrot_dist) + const double & carrot_dist, const double & dist_to_path_end) { // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in // odom frame and the carrot_pose is in robot base frame. Just how the data comes to us @@ -91,6 +91,7 @@ bool CollisionChecker::isCollisionImminent( // only forward simulate within time requested double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot; + double simulation_distance_limit = carrot_dist; if (params_->min_distance_to_obstacle > 0.0) { max_allowed_time_to_collision_check = std::max( params_->max_allowed_time_to_collision_up_to_carrot, @@ -98,6 +99,20 @@ bool CollisionChecker::isCollisionImminent( std::abs(linear_vel), params_->min_approach_linear_velocity) ); + if (params_->use_velocity_scaled_lookahead_dist) { + double base_simulation_dist; + if (params_->use_path_aware_obstacle_distance) { + // prevents the checker from looking "through" the goal + // considering the safe distance until the path end + const double effective_min_dist = std::min(params_->min_distance_to_obstacle, + dist_to_path_end); + // the simulation distance should look at least as far as the effective safety distance + base_simulation_dist = std::max(carrot_dist, effective_min_dist); + } else { + base_simulation_dist = std::max(carrot_dist, params_->min_distance_to_obstacle); + } + simulation_distance_limit = std::min(base_simulation_dist, params_->max_lookahead_dist); + } } int i = 1; while (i * projection_time < max_allowed_time_to_collision_check) { @@ -111,8 +126,11 @@ bool CollisionChecker::isCollisionImminent( theta += projection_time * angular_vel; curr_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); - // check if past carrot pose, where no longer a thoughtfully valid command - if (hypot(curr_pose.position.x - robot_xy.x, curr_pose.position.y - robot_xy.y) > carrot_dist) { + // Stop simulating if we've gone past the simulation distance limit + // (max: carrot or min distance) + if (hypot(curr_pose.position.x - robot_xy.x, + curr_pose.position.y - robot_xy.y) > simulation_distance_limit) + { break; } diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 31bc7b7f19d..3f267da9024 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -103,6 +103,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_path_aware_obstacle_distance", + rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); @@ -188,6 +191,30 @@ 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_ + ".use_path_aware_obstacle_distance", + params_.use_path_aware_obstacle_distance); + if (params_.use_path_aware_obstacle_distance && params_.min_distance_to_obstacle <= 0.0) { + RCLCPP_WARN( + logger_, + "For 'use_path_aware_obstacle_distance' to be enabled, 'min_distance_to_obstacle' " + "must be set to a positive value, but it is not. The path-aware logic will be ignored."); + } + if (params_.use_path_aware_obstacle_distance && params_.use_velocity_scaled_lookahead_dist) { + RCLCPP_WARN( + logger_, + "For 'use_path_aware_obstacle_distance' to be enabled, 'use_velocity_scaled_lookahead_dist' " + "must also be true. The path-aware logic will be ignored."); + } node->get_parameter(plugin_name_ + ".stateful", params_.stateful); if (params_.inflation_cost_scaling_factor <= 0.0) { @@ -315,6 +342,8 @@ ParameterHandler::updateParametersCallback( params_.allow_reversing = parameter.as_bool(); } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { params_.interpolate_curvature_after_goal = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_path_aware_obstacle_distance") { + params_.use_path_aware_obstacle_distance = parameter.as_bool(); } } } 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 b348bdf7933..7adfd1310df 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 @@ -274,10 +274,13 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity angular_vel = linear_vel * regulation_curvature; } + const double dist_to_path_end = + nav2_util::geometry_utils::calculate_path_length(transformed_plan); // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); if (params_->use_collision_detection && - collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) + collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist, + dist_to_path_end)) { throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); }