Update setSpeedLimits with angular vel max

Signed-off-by: Alberto Tudela <[email protected]>

nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp

@@ -43,6 +43,7 @@ struct Parameters
   double v_linear_max;
   double v_linear_max_initial;
   double v_angular_max;
+  double v_angular_max_initial;
   double slowdown_radius;
   bool initial_rotation;
   double initial_rotation_min_angle;

nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp

@@ -71,6 +71,17 @@ class SmoothControlLaw
    */
   void setSlowdownRadius(const double slowdown_radius);
 
+  /**
+   * @brief Update the velocity limits.
+   *
+   * @param v_linear_min The minimum absolute velocity in the linear direction.
+   * @param v_linear_max The maximum absolute velocity in the linear direction.
+   * @param v_angular_max The maximum absolute velocity in the angular direction.
+   */
+  void setSpeedLimit(
+    const double v_linear_min, const double v_linear_max,
+    const double v_angular_max);
+
   /**
    * @brief Compute linear and angular velocities command using the curvature.
    *
@@ -95,16 +106,6 @@ class SmoothControlLaw
     const geometry_msgs::msg::Pose & target,
     const bool & backward = false);
 
-  /**
-   * @brief Update the velocity limits.
-   * @param v_linear_min The minimum absolute velocity in the linear direction.
-   * @param v_linear_max The maximum absolute velocity in the linear direction.
-   * @param v_angular_max The maximum absolute velocity in the angular direction.
-   */
-  void setSpeedLimit(
-    const double v_linear_min, const double v_linear_max,
-    const double v_angular_max);
-
   /**
    * @brief Calculate the next pose of the robot by generating a velocity command using the
    * curvature and the current pose.

nav2_graceful_motion_controller/src/graceful_motion_controller.cpp

@@ -230,6 +230,10 @@ void GracefulMotionController::setSpeedLimit(
     params_->v_linear_max = std::max(speed_limit, params_->v_linear_min);
   }
 
+  // Limit the angular velocity to be proportional to the linear velocity
+  params_->v_angular_max = params_->v_linear_max * params_->rotation_scaling_factor;
+  params_->v_angular_max = std::min(params_->v_angular_max, params_->v_angular_max_initial);
+
   // Update the speed limit in the control law
   control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 }

nav2_graceful_motion_controller/src/parameter_handler.cpp

@@ -83,6 +83,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max);
   params_.v_linear_max_initial = params_.v_linear_max;
   node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max);
+  params_.v_angular_max_initial = params_.v_angular_max;
   node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
   node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
   node->get_parameter(
@@ -132,6 +133,7 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.v_linear_max_initial = params_.v_linear_max;
       } else if (name == plugin_name_ + ".v_angular_max") {
         params_.v_angular_max = parameter.as_double();
+        params_.v_angular_max_initial = params_.v_angular_max;
       } else if (name == plugin_name_ + ".slowdown_radius") {
         params_.slowdown_radius = parameter.as_double();
       } else if (name == plugin_name_ + ".initial_rotation_min_angle") {

nav2_graceful_motion_controller/src/smooth_control_law.cpp

@@ -42,6 +42,15 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
   slowdown_radius_ = slowdown_radius;
 }
 
+void SmoothControlLaw::setSpeedLimit(
+  const double v_linear_min, const double v_linear_max,
+  const double v_angular_max)
+{
+  v_linear_min_ = v_linear_min;
+  v_linear_max_ = v_linear_max;
+  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 bool & backward)
@@ -82,15 +91,6 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   return calculateRegularVelocity(target, geometry_msgs::msg::Pose(), backward);
 }
 
-void SmoothControlLaw::setSpeedLimit(
-  const double v_linear_min, const double v_linear_max,
-  const double v_angular_max)
-{
-  v_linear_min_ = v_linear_min;
-  v_linear_max_ = v_linear_max;
-  v_angular_max_ = v_angular_max;
-}
-
 geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose(
   const double dt,
   const geometry_msgs::msg::Pose & target,