[Graceful Controller] Add deceleration limit

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

@@ -32,6 +32,24 @@ namespace nav2_graceful_controller
 class SmoothControlLaw
 {
 public:
+  /**
+   * @brief Constructor for nav2_graceful_controller::SmoothControlLaw
+   *
+   * @param k_phi Ratio of the rate of change in phi to the rate of change in r.
+   * @param k_delta Constant factor applied to the heading error feedback.
+   * @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 deceleration_max,
+    double v_linear_min, double v_linear_max, double v_angular_max);
+
   /**
    * @brief Constructor for nav2_graceful_controller::SmoothControlLaw
    *
@@ -71,6 +89,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,6 +111,22 @@ 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);
+
+  /**
+   * @brief Compute linear and angular velocities command using the curvature.
+   *
+   * @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.
    */
@@ -111,6 +152,25 @@ 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);
+
+  /**
+   * @brief Calculate the next pose of the robot by generating a velocity command using the
+   * curvature and the current pose.
+   *
+   * @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
    */
@@ -170,6 +230,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

@@ -20,6 +20,16 @@
 namespace nav2_graceful_controller
 {
 
+SmoothControlLaw::SmoothControlLaw(
+  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), deceleration_max_(deceleration_max),
+  v_linear_min_(v_linear_min), v_linear_max_(v_linear_max), v_angular_max_(v_angular_max)
+{
+}
+
 SmoothControlLaw::SmoothControlLaw(
   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)
@@ -42,6 +52,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)
 {
@@ -50,6 +65,48 @@ void SmoothControlLaw::setSpeedLimit(
   v_angular_max_ = v_angular_max;
 }
 
+geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+  const double & target_distance, const bool & backward)
+{
+  // Convert the target to polar coordinates
+  auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
+  // Calculate the curvature
+  double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta);
+  // Invert the curvature if the robot is moving backwards
+  curvature = backward ? -curvature : curvature;
+
+  // Adjust the linear velocity as a function of the path curvature to
+  // slowdown the controller as it approaches its target
+  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_ * (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
+  v = std::clamp(v, v_linear_min_, v_linear_max_);
+
+  // Set the velocity to negative if the robot is moving backwards
+  v = backward ? -v : v;
+
+  // Compute the angular velocity
+  double w = curvature * v;
+  // Bound angular velocity between [-max_angular_vel, max_angular_vel]
+  double w_bound = std::clamp(w, -v_angular_max_, v_angular_max_);
+  // And linear velocity to follow the curvature
+  v = (curvature != 0.0) ? (w_bound / curvature) : v;
+
+  // Return the velocity command
+  geometry_msgs::msg::Twist cmd_vel;
+  cmd_vel.linear.x = v;
+  cmd_vel.angular.z = w_bound;
+  return cmd_vel;
+}
+
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
   const bool & backward)
@@ -95,6 +152,24 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), 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, 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);
+  next.position.y = current.position.y + vel.linear.x * dt * sin(yaw);
+  yaw += vel.angular.z * dt;
+  next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+  return next;
+}
+
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   const double dt,
   const geometry_msgs::msg::Pose & target,