do not depend on velocity for approach scaling (#3047)

* do not depend on velocity for approach scaling

* add approach_velocity_scaling_dist to README

* do not pass references to shared ptr

* fixup! do not pass references to shared ptr

* fix approach velocity scaling compile issues

* default approach_velocity_scaling_dist based on costmap size

* change default approach_velocity_scaling_dist to 0.6 to match previous behavior

* update tests for approach velocity scaling

* remove use_approach_linear_velocity_scaling from readme

* smooth approach velocity scaling

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

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_;

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<double>(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_);

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<nav2_costmap_2d::Costmap2DROS>("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<path_utils::Straight>(approach_velocity_scaling_dist + 1.0)
+  });
+
   double curvature = 0.5;
   geometry_msgs::msg::Twist curr_speed;
   double pose_cost = 0.0;
@@ -491,26 +500,39 @@ 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
 
   linear_vel = 1.0;
   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
 
   linear_vel = 1.0;
   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<path_utils::Straight>(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();