Redesign DWB dynamic parameters pattern (#5641)

* Redesign dwb dynamic parameters patterns

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix typo

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Activate dynamic parameters callback in Avtivate stage

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Precommit

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix as suggested

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp

@@ -75,6 +75,14 @@ class TrajectoryGenerator
   virtual void initialize(
     const nav2::LifecycleNode::SharedPtr & nh,
     const std::string & plugin_name) = 0;
+  /**
+   * @brief Activate callbacks as needed
+  */
+  virtual void activate() = 0;
+  /**
+   * @brief Deactivate callbacks as needed
+  */
+  virtual void deactivate() = 0;
   virtual void reset() {}
   /**
    * @brief Start a new iteration based on the current velocity

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -151,12 +151,14 @@ void
 DWBLocalPlanner::activate()
 {
   pub_->on_activate();
+  traj_generator_->activate();
 }
 
 void
 DWBLocalPlanner::deactivate()
 {
   pub_->on_deactivate();
+  traj_generator_->deactivate();
 }
 
 void

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

@@ -142,6 +142,8 @@ class KinematicsHandler
   KinematicsHandler();
   ~KinematicsHandler();
   void initialize(const nav2::LifecycleNode::SharedPtr & nh, const std::string & plugin_name);
+  void activate();
+  void deactivate();
 
   inline KinematicParameters getKinematics() {return *kinematics_.load();}
 
@@ -151,16 +153,30 @@ class KinematicsHandler
 
 protected:
   nav2::LifecycleNode::WeakPtr node_;
+  rclcpp::Logger logger_{rclcpp::get_logger("DWBController")};
   std::atomic<KinematicParameters *> kinematics_;
 
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
   /**
-   * @brief Callback executed when a parameter change is detected
-   * @param parameters list of changed parameters
+   * @brief Validate incoming parameter updates before applying them.
+   * This callback is triggered when one or more parameters are about to be updated.
+   * It checks the validity of parameter values and rejects updates that would lead
+   * to invalid or inconsistent configurations
+   * @param parameters List of parameters that are being updated.
+   * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.
    */
-  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
+  rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
     std::vector<rclcpp::Parameter> parameters);
+
+  /**
+   * @brief Apply parameter updates after validation
+   * This callback is executed when parameters have been successfully updated.
+   * It updates the internal configuration of the node with the new parameter values.
+   * @param parameters List of parameters that have been updated.
+   */
+  void updateParametersCallback(std::vector<rclcpp::Parameter> parameters);
   void update_kinematics(KinematicParameters kinematics);
   std::string plugin_name_;
 };

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp

@@ -82,6 +82,16 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
     }
   }
 
+  /**
+  * @brief Registers callbacks for dynamic parameter handling.
+  */
+  void activate();
+
+  /**
+  * @brief Resets callbacks for dynamic parameter handling.
+  */
+  void deactivate();
+
 protected:
   /**
    * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding

nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -55,11 +55,6 @@ KinematicsHandler::KinematicsHandler()
 
 KinematicsHandler::~KinematicsHandler()
 {
-  auto node = node_.lock();
-  if (dyn_params_handler_ && node) {
-    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
-  }
-  dyn_params_handler_.reset();
   delete kinematics_.load();
 }
 
@@ -69,6 +64,7 @@ void KinematicsHandler::initialize(
 {
   node_ = nh;
   plugin_name_ = plugin_name;
+  logger_ = nh->get_logger();
 
   declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));
   declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_y", rclcpp::ParameterValue(0.0));
@@ -119,16 +115,39 @@ void KinematicsHandler::initialize(
   kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
   kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
 
-  // Add callback for dynamic parameters
-  dyn_params_handler_ = nh->add_on_set_parameters_callback(
-    std::bind(&KinematicsHandler::dynamicParametersCallback, this, _1));
-
   kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
   kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
 
   update_kinematics(kinematics);
 }
 
+void KinematicsHandler::activate()
+{
+  auto node = node_.lock();
+  // Add callback for dynamic parameters
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
+    std::bind(
+      &KinematicsHandler::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &KinematicsHandler::validateParameterUpdatesCallback,
+      this, std::placeholders::_1));
+}
+
+void KinematicsHandler::deactivate()
+{
+  auto node = node_.lock();
+  if (post_set_params_handler_ && node) {
+    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
+  }
+  post_set_params_handler_.reset();
+  if (on_set_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(on_set_params_handler_.get());
+  }
+  on_set_params_handler_.reset();
+}
+
 void KinematicsHandler::setSpeedLimit(
   const double & speed_limit, const bool & percentage)
 {
@@ -169,8 +188,48 @@ void KinematicsHandler::setSpeedLimit(
   update_kinematics(kinematics);
 }
 
-rcl_interfaces::msg::SetParametersResult
-KinematicsHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+rcl_interfaces::msg::SetParametersResult KinematicsHandler::validateParameterUpdatesCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+    if (param_name.find(plugin_name_ + ".") != 0) {
+      continue;
+    }
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (parameter.as_double() < 0.0 &&
+        (param_name == plugin_name_ + ".max_vel_x" || param_name == plugin_name_ + ".max_vel_y" ||
+        param_name == plugin_name_ + ".max_vel_theta" ||
+        param_name == plugin_name_ + ".max_speed_xy" ||
+        param_name == plugin_name_ + ".acc_lim_x" || param_name == plugin_name_ + ".acc_lim_y" ||
+        param_name == plugin_name_ + ".acc_lim_theta"))
+      {
+        RCLCPP_WARN(
+        logger_, "The value of parameter '%s' is incorrectly set to %f, "
+        "it should be >= 0. Ignoring parameter update.",
+        param_name.c_str(), parameter.as_double());
+        result.successful = false;
+      } else if (parameter.as_double() > 0.0 && // NOLINT
+        (param_name == plugin_name_ + ".decel_lim_x" ||
+        param_name == plugin_name_ + ".decel_lim_y" ||
+        param_name == plugin_name_ + ".decel_lim_theta"))
+      {
+        RCLCPP_WARN(
+        logger_, "The value of parameter '%s' is incorrectly set to %f, "
+        "it should be <= 0. Ignoring parameter update.",
+        param_name.c_str(), parameter.as_double());
+        result.successful = false;
+      }
+    }
+  }
+  return result;
+}
+
+void
+KinematicsHandler::updateParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
   KinematicParameters kinematics(*kinematics_.load());
@@ -221,8 +280,6 @@ KinematicsHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
     }
   }
   update_kinematics(kinematics);
-  result.successful = true;
-  return result;
 }
 
 void KinematicsHandler::update_kinematics(KinematicParameters kinematics)

nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp

@@ -97,6 +97,16 @@ void StandardTrajectoryGenerator::initialize(
   nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_);
 }
 
+void StandardTrajectoryGenerator::activate()
+{
+  kinematics_handler_->activate();
+}
+
+void StandardTrajectoryGenerator::deactivate()
+{
+  kinematics_handler_->deactivate();
+}
+
 void StandardTrajectoryGenerator::initializeIterator(
   const nav2::LifecycleNode::SharedPtr & nh)
 {

nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp

@@ -43,56 +43,39 @@ using rcl_interfaces::msg::Parameter;
 using rcl_interfaces::msg::ParameterType;
 using rcl_interfaces::msg::ParameterEvent;
 
-class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
-{
-public:
-  void simulate_event(
-    std::vector<rclcpp::Parameter> parameters)
-  {
-    dynamicParametersCallback(parameters);
-  }
-};
-
 TEST(KinematicParameters, SetAllParameters) {
   std::string nodeName = "test_node";
   auto node = std::make_shared<nav2::LifecycleNode>(nodeName);
-  KinematicsHandlerTest kh;
+  dwb_plugins::KinematicsHandler kh;
   kh.initialize(node, nodeName);
+  kh.activate();
 
-  std::vector<rclcpp::Parameter> parameters;
-  rclcpp::Parameter
-    p_minX(nodeName + ".min_vel_x", 12.34),
-  p_maxX(nodeName + ".max_vel_x", 23.45),
-  p_minY(nodeName + ".min_vel_y", 34.56),
-  p_maxY(nodeName + ".max_vel_y", 45.67),
-  p_accX(nodeName + ".acc_lim_x", 56.78),
-  p_decelX(nodeName + ".acc_lim_y", 67.89),
-  p_accY(nodeName + ".decel_lim_x", 78.90),
-  p_decelY(nodeName + ".decel_lim_y", 89.01),
-  p_minSpeedXY(nodeName + ".min_speed_xy", 90.12),
-  p_maxSpeedXY(nodeName + ".max_speed_xy", 123.456),
-  p_maxTheta(nodeName + ".max_vel_theta", 345.678),
-  p_accTheta(nodeName + ".acc_lim_theta", 234.567),
-  p_decelTheta(nodeName + ".decel_lim_theta", 456.789),
-  p_minSpeedTheta(nodeName + ".min_speed_theta", 567.890);
-
-  parameters.push_back(p_minX);
-  parameters.push_back(p_minX);
-  parameters.push_back(p_maxX);
-  parameters.push_back(p_minY);
-  parameters.push_back(p_maxY);
-  parameters.push_back(p_accX);
-  parameters.push_back(p_accY);
-  parameters.push_back(p_decelX);
-  parameters.push_back(p_decelY);
-  parameters.push_back(p_minSpeedXY);
-  parameters.push_back(p_maxSpeedXY);
-  parameters.push_back(p_maxTheta);
-  parameters.push_back(p_accTheta);
-  parameters.push_back(p_decelTheta);
-  parameters.push_back(p_minSpeedTheta);
-
-  kh.simulate_event(parameters);
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+  {
+    rclcpp::Parameter(nodeName + ".min_vel_x", 12.34),
+    rclcpp::Parameter(nodeName + ".max_vel_x", 23.45),
+    rclcpp::Parameter(nodeName + ".min_vel_y", 34.56),
+    rclcpp::Parameter(nodeName + ".max_vel_y", 45.67),
+    rclcpp::Parameter(nodeName + ".acc_lim_x", 56.78),
+    rclcpp::Parameter(nodeName + ".acc_lim_y", 67.89),
+    rclcpp::Parameter(nodeName + ".decel_lim_x", -78.90),
+    rclcpp::Parameter(nodeName + ".decel_lim_y", -89.01),
+    rclcpp::Parameter(nodeName + ".min_speed_xy", 90.12),
+    rclcpp::Parameter(nodeName + ".max_speed_xy", 123.456),
+    rclcpp::Parameter(nodeName + ".max_vel_theta", 345.678),
+    rclcpp::Parameter(nodeName + ".acc_lim_theta", 234.567),
+    rclcpp::Parameter(nodeName + ".decel_lim_theta", -456.789),
+    rclcpp::Parameter(nodeName + ".min_speed_theta", 567.890),
+  });
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
 
   dwb_plugins::KinematicParameters kp = kh.getKinematics();
 
@@ -102,14 +85,47 @@ TEST(KinematicParameters, SetAllParameters) {
   EXPECT_EQ(kp.getMaxY(), 45.67);
   EXPECT_EQ(kp.getAccX(), 56.78);
   EXPECT_EQ(kp.getAccY(), 67.89);
-  EXPECT_EQ(kp.getDecelX(), 78.90);
-  EXPECT_EQ(kp.getDecelY(), 89.01);
+  EXPECT_EQ(kp.getDecelX(), -78.90);
+  EXPECT_EQ(kp.getDecelY(), -89.01);
   EXPECT_EQ(kp.getMinSpeedXY(), 90.12);
   EXPECT_EQ(kp.getMaxSpeedXY(), 123.456);
   EXPECT_EQ(kp.getAccTheta(), 234.567);
   EXPECT_EQ(kp.getMaxTheta(), 345.678);
-  EXPECT_EQ(kp.getDecelTheta(), 456.789);
+  EXPECT_EQ(kp.getDecelTheta(), -456.789);
   EXPECT_EQ(kp.getMinSpeedTheta(), 567.890);
+
+  results = rec_param->set_parameters_atomically(
+  {
+    rclcpp::Parameter(nodeName + ".decel_lim_x", 1.0)
+  });
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(kp.getDecelX(), -78.90);
+
+  results = rec_param->set_parameters_atomically(
+  {
+    rclcpp::Parameter(nodeName + ".max_vel_x", -1.0)
+  });
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(kp.getMaxX(), 23.45);
+
+  results = rec_param->set_parameters_atomically(
+  {
+    rclcpp::Parameter(nodeName + ".acc_lim_x", -0.1)
+  });
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(kp.getAccX(), 56.78);
 }