[Graceful Controller] Add deceleration limit

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -121,7 +121,8 @@ class Controller
   // Smooth control law
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
   double k_phi_, k_delta_, beta_, lambda_;
-  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
+  double slowdown_radius_, deceleration_max_;
+  double v_linear_min_, v_linear_max_, v_angular_max_;
   double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_;
 
   // The trajectory of the robot while dock / undock for visualization / debug purposes

nav2_docking/opennav_docking/src/controller.cpp

@@ -48,6 +48,8 @@ Controller::Controller(
     node, "controller.v_angular_max", rclcpp::ParameterValue(0.75));
   nav2::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
+  nav2::declare_parameter_if_not_declared(
+    node, "controller.deceleration_max", rclcpp::ParameterValue(0.1));
   nav2::declare_parameter_if_not_declared(
       node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0));
   nav2::declare_parameter_if_not_declared(
@@ -77,9 +79,10 @@ Controller::Controller(
   node->get_parameter("controller.v_linear_max", v_linear_max_);
   node->get_parameter("controller.v_angular_max", v_angular_max_);
   node->get_parameter("controller.slowdown_radius", slowdown_radius_);
+  node->get_parameter("controller.deceleration_max", deceleration_max_);
   control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
-    v_angular_max_);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, deceleration_max_,
+    v_linear_min_, v_linear_max_, v_angular_max_);
 
   // Add callback for dynamic parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -131,6 +131,7 @@ class GracefulController : public nav2_core::Controller
    *
    * @param motion_target Motion target point (in costmap local frame?)
    * @param costmap_transform Transform between global and local costmap
+   * @param target_distance Path distance to target point
    * @param trajectory Simulated trajectory
    * @param cmd_vel Initial command velocity during simulation
    * @param backward Flag to indicate if the robot is moving backward
@@ -139,6 +140,7 @@ class GracefulController : public nav2_core::Controller
   bool simulateTrajectory(
     const geometry_msgs::msg::PoseStamped & motion_target,
     const geometry_msgs::msg::TransformStamped & costmap_transform,
+    const double & target_distance,
     nav_msgs::msg::Path & trajectory,
     geometry_msgs::msg::TwistStamped & cmd_vel,
     bool backward);

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -46,6 +46,7 @@ struct Parameters
   double v_angular_max_initial;
   double v_angular_min_in_place;
   double slowdown_radius;
+  double deceleration_max;
   bool initial_rotation;
   double initial_rotation_tolerance;
   bool prefer_final_rotation;

nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp

@@ -40,12 +40,14 @@ class SmoothControlLaw
    * @param beta Constant factor applied to the path curvature: dropping velocity.
    * @param lambda Constant factor applied to the path curvature for sharpness.
    * @param slowdown_radius Radial threshold applied to the slowdown rule.
+   * @param deceleration_max Maximum deceleration.
    * @param v_linear_min Minimum linear velocity.
    * @param v_linear_max Maximum linear velocity.
    * @param v_angular_max Maximum angular velocity.
    */
   SmoothControlLaw(
-    double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
+    double k_phi, double k_delta, double beta, double lambda,
+    double slowdown_radius, double deceleration_max,
     double v_linear_min, double v_linear_max, double v_angular_max);
 
   /**
@@ -71,6 +73,13 @@ class SmoothControlLaw
    */
   void setSlowdownRadius(const double slowdown_radius);
 
+  /**
+   * @brief Set the max deceleration
+   *
+   * @param deceleration_max Maximum deceleration possible.
+   */
+  void setMaxDeceleration(const double deceleration_max);
+
   /**
    * @brief Update the velocity limits.
    *
@@ -86,12 +95,14 @@ class SmoothControlLaw
    *
    * @param target Pose of the target in the robot frame.
    * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
    * @param backward If true, the robot is moving backwards. Defaults to false.
    * @return Velocity command.
    */
   geometry_msgs::msg::Twist calculateRegularVelocity(
     const geometry_msgs::msg::Pose & target,
     const geometry_msgs::msg::Pose & current,
+    const double & target_distance,
     const bool & backward = false);
 
   /**
@@ -111,13 +122,15 @@ class SmoothControlLaw
    * @param dt Time step.
    * @param target Pose of the target in the robot frame.
    * @param current Current pose of the robot in the robot frame.
+   * @param target_distance Path distance from current to target frame.
    * @param backward If true, the robot is moving backwards. Defaults to false.
    * @return geometry_msgs::msg::Pose
    */
   geometry_msgs::msg::Pose calculateNextPose(
     const double dt,
     const geometry_msgs::msg::Pose & target,
     const geometry_msgs::msg::Pose & current,
+    const double & target_distance,
     const bool & backward = false);
 
 protected:
@@ -170,6 +183,11 @@ class SmoothControlLaw
    */
   double slowdown_radius_;
 
+  /**
+   * @brief Maximum deceleration.
+   */
+  double deceleration_max_;
+
   /**
    * @brief Minimum linear velocity.
    */

nav2_graceful_controller/src/graceful_controller.cpp

@@ -53,7 +53,8 @@ void GracefulController::configure(
 
   // Handles the control law to generate the velocity commands
   control_law_ = std::make_unique<SmoothControlLaw>(
-    params_->k_phi, params_->k_delta, params_->beta, params_->lambda, params_->slowdown_radius,
+    params_->k_phi, params_->k_delta, params_->beta, params_->lambda,
+    params_->slowdown_radius, params_->deceleration_max,
     params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
   // Initialize footprint collision checker
@@ -134,6 +135,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   control_law_->setCurvatureConstants(
     params_->k_phi, params_->k_delta, params_->beta, params_->lambda);
   control_law_->setSlowdownRadius(params_->slowdown_radius);
+  control_law_->setMaxDeceleration(params_->deceleration_max);
   control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
   // Transform path to robot base frame
@@ -310,7 +312,9 @@ bool GracefulController::validateTargetPose(
   }
 
   // Actually simulate the path
-  if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
+  if (simulateTrajectory(target_pose, costmap_transform, dist_to_target, trajectory, cmd_vel,
+      reversing))
+  {
     // Successfully simulated to target_pose
     return true;
   }
@@ -322,6 +326,7 @@ bool GracefulController::validateTargetPose(
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
+  const double & target_distance,
   nav_msgs::msg::Path & trajectory,
   geometry_msgs::msg::TwistStamped & cmd_vel,
   bool backward)
@@ -372,12 +377,12 @@ bool GracefulController::simulateTrajectory(
       // If this is first iteration, this is our current target velocity
       if (trajectory.poses.empty()) {
         cmd_vel.twist = control_law_->calculateRegularVelocity(
-          motion_target.pose, next_pose.pose, backward);
+          motion_target.pose, next_pose.pose, target_distance, backward);
       }
 
       // Apply velocities to calculate next pose
       next_pose.pose = control_law_->calculateNextPose(
-        dt, motion_target.pose, next_pose.pose, backward);
+        dt, motion_target.pose, next_pose.pose, target_distance, backward);
     }
 
     // Add the pose to the trajectory for visualization

nav2_graceful_controller/src/parameter_handler.cpp

@@ -58,6 +58,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".deceleration_max", rclcpp::ParameterValue(3.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
@@ -97,6 +99,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place);
   node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
+  node->get_parameter(plugin_name_ + ".deceleration_max", params_.deceleration_max);
   node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
   node->get_parameter(
     plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance);
@@ -168,6 +171,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.v_angular_min_in_place = parameter.as_double();
       } else if (param_name == plugin_name_ + ".slowdown_radius") {
         params_.slowdown_radius = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".deceleration_max") {
+        params_.deceleration_max = parameter.as_double();
       } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") {
         params_.initial_rotation_tolerance = parameter.as_double();
       } else if (param_name == plugin_name_ + ".rotation_scaling_factor") {

nav2_graceful_controller/src/smooth_control_law.cpp

@@ -21,9 +21,11 @@ namespace nav2_graceful_controller
 {
 
 SmoothControlLaw::SmoothControlLaw(
-  double k_phi, double k_delta, double beta, double lambda, double slowdown_radius,
+  double k_phi, double k_delta, double beta, double lambda,
+  double slowdown_radius, double deceleration_max,
   double v_linear_min, double v_linear_max, double v_angular_max)
-: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda), slowdown_radius_(slowdown_radius),
+: k_phi_(k_phi), k_delta_(k_delta), beta_(beta), lambda_(lambda),
+  slowdown_radius_(slowdown_radius), deceleration_max_(deceleration_max),
   v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
 {
 }
@@ -42,6 +44,11 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
   slowdown_radius_ = slowdown_radius;
 }
 
+void SmoothControlLaw::setMaxDeceleration(double deceleration_max)
+{
+  deceleration_max_ = deceleration_max;
+}
+
 void SmoothControlLaw::setSpeedLimit(
   const double v_linear_min, const double v_linear_max, const double v_angular_max)
 {
@@ -52,7 +59,7 @@ void SmoothControlLaw::setSpeedLimit(
 
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-  const bool & backward)
+  const double & target_distance, const bool & backward)
 {
   // Convert the target to polar coordinates
   auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
@@ -66,7 +73,10 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   double v = v_linear_max_ / (1.0 + beta_ * std::pow(fabs(curvature), lambda_));
 
   // Slowdown when the robot is near the target to remove singularity
-  v = std::min(v_linear_max_ * (ego_coords.r / slowdown_radius_), v);
+  v = std::min(v_linear_max_ * (target_distance / slowdown_radius_), v);
+
+  // Constraint robot velocity within deceleration limits
+  v = std::min(sqrt(2 * target_distance * deceleration_max_), v);
 
   // Set some small v_min when far away from origin to promote faster
   // turning motion when the curvature is very high
@@ -92,16 +102,20 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const bool & backward)
 {
-  return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward);
+  geometry_msgs::msg::Pose current_pose = geometry_msgs::msg::Pose();
+  double target_distance = nav2_util::geometry_utils::euclidean_distance(target, current_pose);
+  return calculateRegularVelocity(target, current_pose, target_distance, backward);
 }
 
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   const double dt,
   const geometry_msgs::msg::Pose & target,
   const geometry_msgs::msg::Pose & current,
+  const double & target_distance,
   const bool & backward)
 {
-  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, backward);
+  geometry_msgs::msg::Twist vel = calculateRegularVelocity(target, current, target_distance,
+      backward);
   geometry_msgs::msg::Pose next;
   double yaw = tf2::getYaw(current.orientation);
   next.position.x = current.position.x + vel.linear.x * dt * cos(yaw);

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -27,15 +27,17 @@ class SCLFixture : public nav2_graceful_controller::SmoothControlLaw
 public:
   SCLFixture(
     double k_phi, double k_delta, double beta, double lambda,
-    double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max)
+    double slowdown_radius, double deceleration_max,
+    double v_linear_min, double v_linear_max, double v_angular_max)
   : nav2_graceful_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda,
-      slowdown_radius, v_linear_min, v_linear_max, v_angular_max) {}
+      slowdown_radius, deceleration_max, v_linear_min, v_linear_max, v_angular_max) {}
 
   double getCurvatureKPhi() {return k_phi_;}
   double getCurvatureKDelta() {return k_delta_;}
   double getCurvatureBeta() {return beta_;}
   double getCurvatureLambda() {return lambda_;}
   double getSlowdownRadius() {return slowdown_radius_;}
+  double getMaxDeceleration() {return deceleration_max_;}
   double getSpeedLinearMin() {return v_linear_min_;}
   double getSpeedLinearMax() {return v_linear_max_;}
   double getSpeedAngularMax() {return v_angular_max_;}
@@ -84,7 +86,7 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController
 
 TEST(SmoothControlLawTest, setCurvatureConstants) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0);
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 
   // Set curvature constants
   scl.setCurvatureConstants(1.0, 2.0, 3.0, 4.0);
@@ -100,9 +102,20 @@ TEST(SmoothControlLawTest, setCurvatureConstants) {
   EXPECT_EQ(scl.getSlowdownRadius(), 5.0);
 }
 
+TEST(SmoothControlLawTest, setMaxDeceleration) {
+  // Initialize SmoothControlLaw
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+
+  // Set max deceleration
+  scl.setMaxDeceleration(3.0);
+
+  // Check results
+  EXPECT_EQ(scl.getMaxDeceleration(), 3.0);
+}
+
 TEST(SmoothControlLawTest, setSpeedLimits) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 
   // Set speed limits
   scl.setSpeedLimit(1.0, 2.0, 3.0);
@@ -115,7 +128,7 @@ TEST(SmoothControlLawTest, setSpeedLimits) {
 
 TEST(SmoothControlLawTest, calculateCurvature) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -150,7 +163,7 @@ TEST(SmoothControlLawTest, calculateCurvature) {
 
 TEST(SmoothControlLawTest, calculateRegularVelocity) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -163,7 +176,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
   current.position.y = 0.0;
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate velocity
-  auto cmd_vel = scl.calculateRegularVelocity(target, current);
+  double target_distance = 5.0;
+  auto cmd_vel = scl.calculateRegularVelocity(target, current, target_distance);
 
   // Check results: both linear and angular velocity must be positive
   EXPECT_NEAR(cmd_vel.linear.x, 0.1460446, 0.0001);
@@ -178,7 +192,8 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
   current.position.y = 0.0;
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate velocity
-  cmd_vel = scl.calculateRegularVelocity(target, current);
+  target_distance = 4.995;
+  cmd_vel = scl.calculateRegularVelocity(target, current, target_distance);
 
   // Check results: linear velocity must be positive and angular velocity must be negative
   EXPECT_NEAR(cmd_vel.linear.x, 0.96651200, 0.0001);
@@ -187,7 +202,7 @@ TEST(SmoothControlLawTest, calculateRegularVelocity) {
 
 TEST(SmoothControlLawTest, calculateNextPose) {
   // Initialize SmoothControlLaw
-  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 0.0, 1.0, 1.0);
+  SCLFixture scl(1.0, 10.0, 0.2, 2.0, 0.1, 3.0, 0.0, 1.0, 1.0);
 
   // Initialize target
   geometry_msgs::msg::Pose target;
@@ -201,7 +216,8 @@ TEST(SmoothControlLawTest, calculateNextPose) {
   current.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
   // Calculate next pose
   float dt = 0.1;
-  auto next_pose = scl.calculateNextPose(dt, target, current);
+  double target_distance = 5.0;
+  auto next_pose = scl.calculateNextPose(dt, target, current, target_distance);
 
   // Check results
   EXPECT_NEAR(next_pose.position.x, 0.1, 0.1);