From 0d55936f112da5f177aa9d344f3c29836bc746ba Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Mon, 27 Jun 2022 17:04:42 -0600 Subject: [PATCH 01/10] do not depend on velocity for approach scaling --- .../regulated_pure_pursuit_controller.hpp | 13 ++++- .../src/regulated_pure_pursuit_controller.cpp | 57 ++++++++++++------- 2 files changed, 48 insertions(+), 22 deletions(-) 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 9224921de2f..b6f13ac2b0f 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,17 +208,25 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double costAtPose(const double & x, const double & y); + double approachVelocityScalingFactor( + const nav_msgs::msg::Path::SharedPtr & path + ) const; + + void applyApproachVelocityScaling( + const nav_msgs::msg::Path::SharedPtr & 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 & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & speed, const double & pose_cost, double & linear_vel, double & sign); @@ -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 3428faae854..38479a3ac42 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,8 @@ 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(1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); @@ -128,6 +130,9 @@ 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_); node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); @@ -325,7 +330,6 @@ 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); @@ -571,10 +575,39 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double return static_cast(cost); } +double RegulatedPurePursuitController::approachVelocityScalingFactor( + const nav_msgs::msg::Path::SharedPtr & path +) const +{ + double remaining_distance = nav2_util::geometry_utils::calculate_path_length(path); + if (remaining_distance < approach_velocity_scaling_dist_) { + return remaining_distance / approach_velocity_scaling_dist_; + } else { + return 1.0; + } +} + +void RegulatedPurePursuitController::applyApproachVelocityScaling( + const nav_msgs::msg::Path::SharedPtr & path + double & linear_vel +) const +{ + 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 & 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; @@ -605,23 +638,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_); From 7764ce53a53b8d1c96fa6768834fdfdd040f86d2 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 10:38:43 -0600 Subject: [PATCH 02/10] add approach_velocity_scaling_dist to README --- nav2_regulated_pure_pursuit_controller/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 839c0b0925c..6c49a608fea 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -62,6 +62,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `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 | | `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 +112,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 From cc99f729ea80f87187c25fe1bb12ea9c87c82a2a Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 10:39:08 -0600 Subject: [PATCH 03/10] do not pass references to shared ptr --- .../src/regulated_pure_pursuit_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 38479a3ac42..758a67893b8 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 @@ -576,7 +576,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double } double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path::SharedPtr & path + const nav_msgs::msg::Path::SharedPtr path ) const { double remaining_distance = nav2_util::geometry_utils::calculate_path_length(path); @@ -588,7 +588,7 @@ double RegulatedPurePursuitController::approachVelocityScalingFactor( } void RegulatedPurePursuitController::applyApproachVelocityScaling( - const nav_msgs::msg::Path::SharedPtr & path + const nav_msgs::msg::Path::SharedPtr path double & linear_vel ) const { From 0d9ae5fff2fe207b816162fccce71660b1a94395 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 10:44:59 -0600 Subject: [PATCH 04/10] fixup! do not pass references to shared ptr --- .../regulated_pure_pursuit_controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 b6f13ac2b0f..def30506b00 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 @@ -209,11 +209,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller double costAtPose(const double & x, const double & y); double approachVelocityScalingFactor( - const nav_msgs::msg::Path::SharedPtr & path + const nav_msgs::msg::Path::SharedPtr path ) const; void applyApproachVelocityScaling( - const nav_msgs::msg::Path::SharedPtr & path + const nav_msgs::msg::Path::SharedPtr path double & linear_vel ) const; From c55fce9db9785b246fac16f032750e5e66e621ee Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 10:55:58 -0600 Subject: [PATCH 05/10] fix approach velocity scaling compile issues --- .../regulated_pure_pursuit_controller.hpp | 8 ++++---- .../src/regulated_pure_pursuit_controller.cpp | 12 ++++++------ .../test/test_regulated_pp.cpp | 14 ++++++-------- 3 files changed, 16 insertions(+), 18 deletions(-) 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 def30506b00..01a41df0ac1 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 @@ -209,11 +209,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller double costAtPose(const double & x, const double & y); double approachVelocityScalingFactor( - const nav_msgs::msg::Path::SharedPtr path + const nav_msgs::msg::Path & path ) const; void applyApproachVelocityScaling( - const nav_msgs::msg::Path::SharedPtr path + const nav_msgs::msg::Path & path, double & linear_vel ) const; @@ -226,9 +226,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose_cost cost at this pose */ void applyConstraints( - 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. 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 758a67893b8..c7969694156 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 @@ -330,8 +330,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( - 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; @@ -576,7 +577,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double } double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path::SharedPtr path + const nav_msgs::msg::Path & path ) const { double remaining_distance = nav2_util::geometry_utils::calculate_path_length(path); @@ -588,10 +589,11 @@ double RegulatedPurePursuitController::approachVelocityScalingFactor( } void RegulatedPurePursuitController::applyApproachVelocityScaling( - const nav_msgs::msg::Path::SharedPtr path + 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_) { @@ -605,13 +607,11 @@ void RegulatedPurePursuitController::applyApproachVelocityScaling( } void RegulatedPurePursuitController::applyConstraints( - const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, 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); 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 9f65a8554fe..f6c2ea27fa9 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); } @@ -469,6 +468,7 @@ TEST(RegulatedPurePursuitTest, rotateTests) EXPECT_NEAR(ang_v, 0.84, 0.01); } +// TODO(aposhian): fix this test for approach velocity scaling changes TEST(RegulatedPurePursuitTest, applyConstraints) { auto ctrl = std::make_shared(); @@ -480,8 +480,6 @@ TEST(RegulatedPurePursuitTest, applyConstraints) costmap->on_configure(state); ctrl->configure(node, name, tf, costmap); - double dist_error = 0.0; - double lookahead_dist = 0.6; double curvature = 0.5; geometry_msgs::msg::Twist curr_speed; double pose_cost = 0.0; @@ -491,7 +489,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, nav_msgs::msg::Path(), linear_vel, sign); EXPECT_EQ(linear_vel, 0.25); // min set speed @@ -499,7 +497,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, nav_msgs::msg::Path(), linear_vel, sign); EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature @@ -507,7 +505,7 @@ 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, nav_msgs::msg::Path(), linear_vel, sign); EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature From 021abde3a9416a3f7198705e3c89360997e6c0a6 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 8 Jul 2022 15:06:18 -0600 Subject: [PATCH 06/10] default approach_velocity_scaling_dist based on costmap size --- nav2_regulated_pure_pursuit_controller/README.md | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 6c49a608fea..d580808773a 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -62,7 +62,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `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 | +| `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 | 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 c7969694156..03458fcc6f6 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,8 +79,12 @@ void RegulatedPurePursuitController::configure( rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + + double forward_costmap_extent = costmap_->getSizeInMetersX() / 2.0; + declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(forward_costmap_extent - costmap_->getResolution())); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); From 5a9f86d363693d2e7d4b86b1f42c6b03b58990c1 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Thu, 21 Jul 2022 10:42:02 -0600 Subject: [PATCH 07/10] change default approach_velocity_scaling_dist to 0.6 to match previous behavior --- .../src/regulated_pure_pursuit_controller.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 03458fcc6f6..7f481c53b8e 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,12 +79,9 @@ void RegulatedPurePursuitController::configure( rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - - double forward_costmap_extent = costmap_->getSizeInMetersX() / 2.0; - declare_parameter_if_not_declared( node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(forward_costmap_extent - costmap_->getResolution())); + rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); @@ -137,6 +134,11 @@ void RegulatedPurePursuitController::configure( 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_); From c645021bfb3840b359974b2c3bdd2d843e108459 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Thu, 21 Jul 2022 10:58:18 -0600 Subject: [PATCH 08/10] update tests for approach velocity scaling --- .../test/test_regulated_pp.cpp | 32 ++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) 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 f6c2ea27fa9..83d814dda07 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -468,7 +468,6 @@ TEST(RegulatedPurePursuitTest, rotateTests) EXPECT_NEAR(ang_v, 0.84, 0.01); } -// TODO(aposhian): fix this test for approach velocity scaling changes TEST(RegulatedPurePursuitTest, applyConstraints) { auto ctrl = std::make_shared(); @@ -478,8 +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); + 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; @@ -489,7 +500,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // test curvature regulation (default) curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - curvature, curr_speed, pose_cost, nav_msgs::msg::Path(), + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_EQ(linear_vel, 0.25); // min set speed @@ -497,7 +508,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 0.7407; curr_speed.linear.x = 0.5; ctrl->applyConstraintsWrapper( - curvature, curr_speed, pose_cost, nav_msgs::msg::Path(), + curvature, curr_speed, pose_cost, no_approach_path, linear_vel, sign); EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature @@ -505,10 +516,23 @@ TEST(RegulatedPurePursuitTest, applyConstraints) curvature = 1000.0; curr_speed.linear.x = 0.25; ctrl->applyConstraintsWrapper( - curvature, curr_speed, pose_cost, nav_msgs::msg::Path(), + 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(); From b258e9fd12eb944eea0b9bb927e3baff88e40df9 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Thu, 21 Jul 2022 11:02:19 -0600 Subject: [PATCH 09/10] remove use_approach_linear_velocity_scaling from readme --- nav2_regulated_pure_pursuit_controller/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index d580808773a..379e2a56f27 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -61,7 +61,6 @@ 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 | From 2c8539dd3522bbda622d6d084718c559ebed12ac Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Thu, 21 Jul 2022 12:09:25 -0600 Subject: [PATCH 10/10] smooth approach velocity scaling --- .../src/regulated_pure_pursuit_controller.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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 7f481c53b8e..18af8ed0df4 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 @@ -583,12 +583,18 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double } double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path & path + const nav_msgs::msg::Path & transformed_path ) const { - double remaining_distance = nav2_util::geometry_utils::calculate_path_length(path); + // 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_) { - return 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; }