diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 839c0b0925..379e2a56f2 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -61,7 +61,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `transform_tolerance` | The TF transform tolerance | | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | -| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | @@ -111,6 +111,7 @@ controller_server: use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 use_approach_linear_velocity_scaling: true + approach_velocity_scaling_dist: 1.0 max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 9224921de2..01a41df0ac 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -208,19 +208,27 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double costAtPose(const double & x, const double & y); + double approachVelocityScalingFactor( + const nav_msgs::msg::Path & path + ) const; + + void applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel + ) const; + /** * @brief apply regulation constraints to the system * @param linear_vel robot command linear velocity input - * @param dist_error error in the carrot distance and lookahead distance * @param lookahead_dist optimal lookahead distance * @param curvature curvature of path * @param speed Speed of robot * @param pose_cost cost at this pose */ void applyConstraints( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & speed, - const double & pose_cost, double & linear_vel, double & sign); + const double & pose_cost, const nav_msgs::msg::Path & path, + double & linear_vel, double & sign); /** * @brief Find the intersection a circle and a line segment. @@ -281,6 +289,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; double min_approach_linear_velocity_; + double approach_velocity_scaling_dist_; double control_duration_; double max_allowed_time_to_collision_up_to_carrot_; bool use_regulated_linear_velocity_scaling_; 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 33166c7af1..c31660b6a9 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 @@ -79,6 +79,9 @@ void RegulatedPurePursuitController::configure( rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); @@ -128,6 +131,14 @@ void RegulatedPurePursuitController::configure( node->get_parameter( plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + approach_velocity_scaling_dist_); + if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); @@ -325,9 +336,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( - fabs(lookahead_dist - sqrt(carrot_dist2)), - lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); + curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity angular_vel = sign * linear_vel * curvature; @@ -571,14 +582,48 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double return static_cast(cost); } +double RegulatedPurePursuitController::approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path +) const +{ + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist_) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); + return distance_to_last_pose / approach_velocity_scaling_dist_; + } else { + return 1.0; + } +} + +void RegulatedPurePursuitController::applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel +) const +{ + double approach_vel = linear_vel; + double velocity_scaling = approachVelocityScalingFactor(path); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); +} + void RegulatedPurePursuitController::applyConstraints( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, - const double & pose_cost, double & linear_vel, double & sign) + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { double curvature_vel = linear_vel; double cost_vel = linear_vel; - double approach_vel = linear_vel; // limit the linear velocity by curvature const double radius = fabs(1.0 / curvature); @@ -605,23 +650,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(cost_vel, curvature_vel); linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); - // if the actual lookahead distance is shorter than requested, that means we're at the - // end of the path. We'll scale linear velocity by error to slow to a smooth stop. - // This expression is eq. to (1) holding time to goal, t, constant using the theoretical - // lookahead distance and proposed velocity and (2) using t with the actual lookahead - // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - if (dist_error > 2.0 * costmap_->getResolution()) { - double velocity_scaling = 1.0 - (dist_error / lookahead_dist); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); - } + applyApproachVelocityScaling(path, linear_vel); // Limit linear velocities to be valid linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9f65a8554f..83d814dda0 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -93,12 +93,11 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure } void applyConstraintsWrapper( - const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & curr_speed, - const double & pose_cost, double & linear_vel, double & sign) + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { return applyConstraints( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, path, linear_vel, sign); } @@ -478,10 +477,20 @@ TEST(RegulatedPurePursuitTest, applyConstraints) auto costmap = std::make_shared("fake_costmap"); rclcpp_lifecycle::State state; costmap->on_configure(state); + + constexpr double approach_velocity_scaling_dist = 0.6; + nav2_util::declare_parameter_if_not_declared( + node, + name + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(approach_velocity_scaling_dist)); + ctrl->configure(node, name, tf, costmap); - double dist_error = 0.0; - double lookahead_dist = 0.6; + auto no_approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(approach_velocity_scaling_dist + 1.0) + }); + double curvature = 0.5; geometry_msgs::msg::Twist curr_speed; double pose_cost = 0.0; @@ -491,7 +500,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // test curvature regulation (default) curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_EQ(linear_vel, 0.25); // min set speed @@ -499,7 +508,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 0.7407; curr_speed.linear.x = 0.5; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature @@ -507,10 +516,23 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 1000.0; curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + // Approach velocity scaling on a path with no distance left + auto approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(0.0) + }); + + linear_vel = 1.0; + curvature = 0.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, approach_path, + linear_vel, sign); + EXPECT_NEAR(linear_vel, 0.05, 0.01); // min out on min approach velocity // now try with cost regulation (turn off velocity and only cost) // ctrl->setCostRegulationScaling();