Update dyn parameters of control law

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

nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp

@@ -53,6 +53,24 @@ class SmoothControlLaw
    */
   ~SmoothControlLaw() = default;
 
+  /**
+   * @brief Set the values that define the curvature.
+   *
+   * @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.
+   */
+  void setCurvatureConstants(
+    const double k_phi, const double k_delta, const double beta, const double lambda);
+
+  /**
+   * @brief Set the slowdown radius
+   *
+   * @param slowdown_radius Radial threshold applied to the slowdown rule.
+   */
+  void setSlowdownRadius(const double slowdown_radius);
+
   /**
    * @brief Compute linear and angular velocities command using the curvature.
    *

nav2_graceful_motion_controller/src/graceful_motion_controller.cpp

@@ -120,6 +120,11 @@ geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityComman
     goal_dist_tolerance_ = pose_tolerance.position.x;
   }
 
+  // Update the smooth control law with the new params
+  control_law_->setCurvatureConstants(
+    params_->k_phi, params_->k_delta, params_->beta, params_->lambda);
+  control_law_->setSlowdownRadius(params_->slowdown_radius);
+
   // Transform path to robot base frame and publish it
   auto transformed_plan = path_handler_->transformGlobalPlan(
     pose, params_->max_robot_pose_search_dist);

nav2_graceful_motion_controller/src/smooth_control_law.cpp

@@ -28,6 +28,20 @@ SmoothControlLaw::SmoothControlLaw(
 {
 }
 
+void SmoothControlLaw::setCurvatureConstants(
+  double k_phi, double k_delta, double beta, double lambda)
+{
+  k_phi_ = k_phi;
+  k_delta_ = k_delta;
+  beta_ = beta;
+  lambda_ = lambda;
+}
+
+void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
+{
+  slowdown_radius_ = slowdown_radius;
+}
+
 geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
   const bool & backward)

nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp

@@ -31,6 +31,11 @@ class SCLFixture : public nav2_graceful_motion_controller::SmoothControlLaw
   : nav2_graceful_motion_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda,
       slowdown_radius, 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 getSpeedLinearMin() {return v_linear_min_;}
   double getSpeedLinearMax() {return v_linear_max_;}
   double getSpeedAngularMax() {return v_angular_max_;}
@@ -100,6 +105,24 @@ class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMoti
   }
 };
 
+TEST(SmoothControlLawTest, setCurvatureConstants) {
+  // Initialize SmoothControlLaw
+  SCLFixture scl(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);
+
+  // Set slowdown radius
+  scl.setSlowdownRadius(5.0);
+
+  // Check results
+  EXPECT_EQ(scl.getCurvatureKPhi(), 1.0);
+  EXPECT_EQ(scl.getCurvatureKDelta(), 2.0);
+  EXPECT_EQ(scl.getCurvatureBeta(), 3.0);
+  EXPECT_EQ(scl.getCurvatureLambda(), 4.0);
+  EXPECT_EQ(scl.getSlowdownRadius(), 5.0);
+}
+
 TEST(SmoothControlLawTest, setSpeedLimits) {
   // Initialize SmoothControlLaw
   SCLFixture scl(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);