Redesign AMCL dynamic parameters pattern

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -92,14 +92,27 @@ class AmclNode : public nav2::LifecycleNode
   nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   /**
-   * @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 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.
    */
-  rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+  void updateParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
   // 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_;
 
   // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
   // respond until we're in the active state

nav2_amcl/src/amcl_node.cpp

@@ -116,9 +116,13 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   auto node = shared_from_this();
   // Add callback for dynamic parameters
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
     std::bind(
-      &AmclNode::dynamicParametersCallback,
+      &AmclNode::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &AmclNode::validateParameterUpdatesCallback,
       this, std::placeholders::_1));
 
   // create bond connection
@@ -139,8 +143,10 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   particle_cloud_pub_->on_deactivate();
 
   // shutdown and reset dynamic parameter handler
-  remove_on_set_parameters_callback(dyn_params_handler_.get());
-  dyn_params_handler_.reset();
+  remove_post_set_parameters_callback(post_set_params_handler_.get());
+  post_set_params_handler_.reset();
+  remove_on_set_parameters_callback(on_set_params_handler_.get());
+  on_set_params_handler_.reset();
 
   // destroy bond connection
   destroyBond();
@@ -986,84 +992,85 @@ AmclNode::initParameters()
   }
 }
 
-/**
-  * @brief Callback executed when a parameter change is detected
-  * @param event ParameterEvent message
-  */
-rcl_interfaces::msg::SetParametersResult
-AmclNode::dynamicParametersCallback(
+rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallback(
   std::vector<rclcpp::Parameter> parameters)
 {
-  std::lock_guard<std::recursive_mutex> cfl(mutex_);
   rcl_interfaces::msg::SetParametersResult result;
-  double save_pose_rate;
-  double tmp_tol;
+  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('.') != std::string::npos) {
+      continue;
+    }
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (parameter.as_double() < 0.0 &&
+        (param_name != "laser_min_range" || param_name != "laser_max_range"))
+      {
+        RCLCPP_WARN(
+        get_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_INTEGER) {
+      if (parameter.as_int() <= 0.0 && param_name == "resample_interval") {
+        RCLCPP_WARN(
+        get_logger(), "The value of resample_interval is incorrectly set, "
+        "it should be >0. Ignoring parameter update.");
+        result.successful = false;
+      } else if (parameter.as_int() < 0.0) {
+        RCLCPP_WARN(
+        get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
+        "it should be >=0. Ignoring parameter update.",
+        param_name.c_str(), parameter.as_int());
+        result.successful = false;
+      } else if (param_name == "max_particles" && parameter.as_int() < min_particles_) {
+        RCLCPP_WARN(
+        get_logger(), "The value of max_particles is incorrectly set, "
+        "it should be larger than min_particles. Ignoring parameter update.");
+        result.successful = false;
+      } else if (param_name == "min_particles" && parameter.as_int() > max_particles_) {
+        RCLCPP_WARN(
+        get_logger(), "The value of min_particles is incorrectly set, "
+        "it should be smaller than max particles. Ignoring parameter update.");
+        result.successful = false;
+      }
+    }
+  }
+  return result;
+}
 
-  int max_particles = max_particles_;
-  int min_particles = min_particles_;
+void
+AmclNode::updateParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   bool reinit_pf = false;
   bool reinit_odom = false;
   bool reinit_laser = false;
   bool reinit_map = false;
 
-  for (auto parameter : parameters) {
+  for (const auto & parameter : parameters) {
     const auto & param_type = parameter.get_type();
     const auto & param_name = parameter.get_name();
-    if (param_name.find('.') != std::string::npos) {
-      continue;
-    }
 
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == "alpha1") {
         alpha1_ = parameter.as_double();
-        // alpha restricted to be non-negative
-        if (alpha1_ < 0.0) {
-          RCLCPP_WARN(
-            get_logger(), "You've set alpha1 to be negative,"
-            " this isn't allowed, so the alpha1 will be set to be zero.");
-          alpha1_ = 0.0;
-        }
         reinit_odom = true;
       } else if (param_name == "alpha2") {
         alpha2_ = parameter.as_double();
-        // alpha restricted to be non-negative
-        if (alpha2_ < 0.0) {
-          RCLCPP_WARN(
-            get_logger(), "You've set alpha2 to be negative,"
-            " this isn't allowed, so the alpha2 will be set to be zero.");
-          alpha2_ = 0.0;
-        }
         reinit_odom = true;
       } else if (param_name == "alpha3") {
         alpha3_ = parameter.as_double();
-        // alpha restricted to be non-negative
-        if (alpha3_ < 0.0) {
-          RCLCPP_WARN(
-            get_logger(), "You've set alpha3 to be negative,"
-            " this isn't allowed, so the alpha3 will be set to be zero.");
-          alpha3_ = 0.0;
-        }
         reinit_odom = true;
       } else if (param_name == "alpha4") {
         alpha4_ = parameter.as_double();
-        // alpha restricted to be non-negative
-        if (alpha4_ < 0.0) {
-          RCLCPP_WARN(
-            get_logger(), "You've set alpha4 to be negative,"
-            " this isn't allowed, so the alpha4 will be set to be zero.");
-          alpha4_ = 0.0;
-        }
         reinit_odom = true;
       } else if (param_name == "alpha5") {
         alpha5_ = parameter.as_double();
-        // alpha restricted to be non-negative
-        if (alpha5_ < 0.0) {
-          RCLCPP_WARN(
-            get_logger(), "You've set alpha5 to be negative,"
-            " this isn't allowed, so the alpha5 will be set to be zero.");
-          alpha5_ = 0.0;
-        }
         reinit_odom = true;
       } else if (param_name == "beam_skip_distance") {
         beam_skip_distance_ = parameter.as_double();
@@ -1099,13 +1106,13 @@ AmclNode::dynamicParametersCallback(
         alpha_slow_ = parameter.as_double();
         reinit_pf = true;
       } else if (param_name == "save_pose_rate") {
-        save_pose_rate = parameter.as_double();
+        double save_pose_rate = parameter.as_double();
         save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
       } else if (param_name == "sigma_hit") {
         sigma_hit_ = parameter.as_double();
         reinit_laser = true;
       } else if (param_name == "transform_tolerance") {
-        tmp_tol = parameter.as_double();
+        double tmp_tol = parameter.as_double();
         transform_tolerance_ = tf2::durationFromSec(tmp_tol);
         reinit_laser = true;
       } else if (param_name == "update_min_a") {
@@ -1173,19 +1180,6 @@ AmclNode::dynamicParametersCallback(
     }
   }
 
-  // Checking if the minimum particles is greater than max_particles_
-  if (min_particles_ > max_particles_) {
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "You've set min_particles to be greater than max particles,"
-      " this isn't allowed.");
-    // sticking to the old values
-    max_particles_ = max_particles;
-    min_particles_ = min_particles;
-    result.successful = false;
-    return result;
-  }
-
   // Re-initialize the particle filter
   if (reinit_pf) {
     if (pf_ != NULL) {
@@ -1221,9 +1215,6 @@ AmclNode::dynamicParametersCallback(
       std::bind(&AmclNode::mapReceived, this, std::placeholders::_1),
       nav2::qos::LatchedSubscriptionQoS());
   }
-
-  result.successful = true;
-  return result;
 }
 
 void