Redesign graceful controller dynamic parameters patterns (#5600)

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

Author: mini-1235

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -83,15 +83,28 @@ class ParameterHandler
   nav2::LifecycleNode::WeakPtr node_;
 
   /**
-   * @brief Callback executed when a parameter change is detected
-   * @param event ParameterEvent message
+   * @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(std::vector<rclcpp::Parameter> parameters);
+  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);
 
   // Dynamic parameters handler
   std::mutex mutex_;
-  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_;
   Parameters params_;
   std::string plugin_name_;
   rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")};

nav2_graceful_controller/src/parameter_handler.cpp

@@ -115,31 +115,77 @@ ParameterHandler::ParameterHandler(
     params_.allow_backward = false;
   }
 
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
-    std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
+    std::bind(
+      &ParameterHandler::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ParameterHandler::validateParameterUpdatesCallback,
+      this, std::placeholders::_1));
 }
 
 ParameterHandler::~ParameterHandler()
 {
   auto node = node_.lock();
-  if (dyn_params_handler_ && node) {
-    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  if (post_set_params_handler_ && node) {
+    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
   }
-  dyn_params_handler_.reset();
+  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();
 }
 
-rcl_interfaces::msg::SetParametersResult
-ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
+  std::vector<rclcpp::Parameter> parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
-  std::lock_guard<std::mutex> lock_reinit(mutex_);
-
+  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) {
+        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 (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == plugin_name_ + ".allow_backward") {
+        if (params_.initial_rotation && parameter.as_bool()) {
+          RCLCPP_WARN(
+            logger_, "Initial rotation and allow backward parameters are both true, "
+            "rejecting parameter change.");
+          result.successful = false;
+        }
+      } else if(param_name == plugin_name_ + ".initial_rotation") {
+        if (parameter.as_bool() && params_.allow_backward) {
+          RCLCPP_WARN(
+            logger_, "Initial rotation and allow backward parameters are both true, "
+            "rejecting parameter change.");
+          result.successful = false;
+        }
+      }
+    }
+  }
+  return result;
+}
+void
+ParameterHandler::updateParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  for (const auto & parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
 
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == plugin_name_ + ".transform_tolerance") {
@@ -177,31 +223,16 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == plugin_name_ + ".initial_rotation") {
-        if (parameter.as_bool() && params_.allow_backward) {
-          RCLCPP_WARN(
-            logger_, "Initial rotation and allow backward parameters are both true, "
-            "rejecting parameter change.");
-          continue;
-        }
         params_.initial_rotation = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
         params_.prefer_final_rotation = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".allow_backward") {
-        if (params_.initial_rotation && parameter.as_bool()) {
-          RCLCPP_WARN(
-            logger_, "Initial rotation and allow backward parameters are both true, "
-            "rejecting parameter change.");
-          continue;
-        }
         params_.allow_backward = parameter.as_bool();
       } else if (param_name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
       }
     }
   }
-
-  result.successful = true;
-  return result;
 }
 
 }  // namespace nav2_graceful_controller

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -283,7 +283,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.initial_rotation_tolerance", 12.0),
       rclcpp::Parameter("test.prefer_final_rotation", false),
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
-      rclcpp::Parameter("test.allow_backward", true),
+      rclcpp::Parameter("test.allow_backward", false),
       rclcpp::Parameter("test.use_collision_detection", false),
       rclcpp::Parameter("test.in_place_collision_resolution", 15.0)});
 
@@ -307,10 +307,17 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.initial_rotation_tolerance").as_double(), 12.0);
   EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0);
-  EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.use_collision_detection").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0);
 
+  // Set allow backward to true
+  results = params->set_parameters_atomically(
+    {rclcpp::Parameter("test.allow_backward", true)});
+  rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
+  EXPECT_EQ(controller->getInitialRotation(), false);
+  EXPECT_EQ(controller->getAllowBackward(), true);
+
   // Set initial rotation to true
   results = params->set_parameters_atomically(
     {rclcpp::Parameter("test.initial_rotation", true)});

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -91,14 +91,27 @@ class ParameterHandler
 
 protected:
   nav2::LifecycleNode::WeakPtr node_;
+
   /**
-   * @brief Callback executed when a parameter change is detected
-   * @param event ParameterEvent message
+   * @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);
+
+  /**
+   * @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
   validateParameterUpdatesCallback(std::vector<rclcpp::Parameter> parameters);
+
   // Dynamic parameters handler
   std::mutex mutex_;
   rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;