Redesign dynamic parameters patterns for opennav_docking, mppi_controller and theta_star

nav2_docking/opennav_docking/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(${library_name} SHARED
   src/docking_server.cpp
   src/dock_database.cpp
   src/navigator.cpp
+  src/parameter_handler.cpp
 )
 target_include_directories(${library_name}
   PUBLIC

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -92,12 +92,23 @@ class Controller
     const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false);
 
   /**
-   * @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(
+    const 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(const std::vector<rclcpp::Parameter> & parameters);
 
   /**
    * @brief Configure the collision checker.
@@ -112,7 +123,8 @@ class Controller
     std::string costmap_topic, std::string footprint_topic, double transform_tolerance);
 
   // 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_;
   std::mutex dynamic_params_lock_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("Controller")};

nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp

@@ -43,7 +43,7 @@ class DockDatabase
   /**
    * @brief A constructor for opennav_docking::DockDatabase
    */
-  explicit DockDatabase(std::shared_ptr<std::mutex> mutex = std::make_shared<std::mutex>());
+  explicit DockDatabase(std::mutex & mutex);
 
   /**
    * @brief A setup function to populate database
@@ -129,7 +129,7 @@ class DockDatabase
     std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response);
 
   nav2::LifecycleNode::WeakPtr node_;
-  std::shared_ptr<std::mutex> mutex_;  // Don't reload database while actively docking
+  std::mutex & mutex_;  // Don't reload database while actively docking
   DockPluginMap dock_plugins_;
   DockMap dock_instances_;
   pluginlib::ClassLoader<opennav_docking_core::ChargingDock> dock_loader_;

nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

@@ -33,6 +33,7 @@
 #include "opennav_docking/types.hpp"
 #include "opennav_docking/dock_database.hpp"
 #include "opennav_docking/navigator.hpp"
+#include "opennav_docking/parameter_handler.hpp"
 #include "opennav_docking_core/charging_dock.hpp"
 #include "tf2_ros/transform_listener.hpp"
 
@@ -221,44 +222,12 @@ class DockingServer : public nav2::LifecycleNode
    */
   void undockRobot();
 
-  /**
-   * @brief Callback executed when a parameter change is detected
-   * @param event ParameterEvent message
-   */
-  rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
-  // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
-
-  // Mutex for dynamic parameters and dock database
-  std::shared_ptr<std::mutex> mutex_;
-
-  // Frequency to run control loops
-  double controller_frequency_;
-  // Timeout for initially detecting the charge dock
-  double initial_perception_timeout_;
-  // Timeout after making contact with dock for charging to start
-  // If this is exceeded, the robot returns to the staging pose and retries
-  double wait_charge_timeout_;
-  // Timeout to approach into the dock and reset its approach is retrying
-  double dock_approach_timeout_;
-  // Timeout to rotate to the dock
-  double rotate_to_dock_timeout_;
-  // When undocking, these are the tolerances for arriving at the staging pose
-  double undock_linear_tolerance_, undock_angular_tolerance_;
+  // Parameter handler
+  std::unique_ptr<opennav_docking::ParameterHandler> param_handler_;
+  Parameters * params_;
+
   // Maximum number of times the robot will return to staging pose and retry docking
-  int max_retries_, num_retries_;
-  // This is the root frame of the robot - typically "base_link"
-  std::string base_frame_;
-  // This is our fixed frame for controlling - typically "odom"
-  std::string fixed_frame_;
-  // Does the robot drive backwards onto the dock? Default is forwards
-  std::optional<bool> dock_backwards_;
-  // The tolerance to the dock's staging pose not requiring navigation
-  double dock_prestaging_tolerance_;
-  // Angular tolerance to exit the rotation loop when rotate_to_dock is enabled
-  double rotation_angular_tolerance_;
+  int num_retries_;
 
   // This is a class member so it can be accessed in publish feedback
   rclcpp::Time action_start_time_;

nav2_docking/opennav_docking/include/opennav_docking/parameter_handler.hpp

@@ -0,0 +1,101 @@
+// Copyright (c) 2022 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OPENNAV_DOCKING__PARAMETER_HANDLER_HPP_
+#define OPENNAV_DOCKING__PARAMETER_HANDLER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_util/parameter_handler.hpp"
+#include "nav2_ros_common/node_utils.hpp"
+
+namespace opennav_docking
+{
+
+struct Parameters
+{
+  // Frequency to run control loops
+  double controller_frequency;
+  // Timeout for initially detecting the charge dock
+  double initial_perception_timeout;
+  // Timeout after making contact with dock for charging to start
+  // If this is exceeded, the robot returns to the staging pose and retries
+  double wait_charge_timeout;
+  // Timeout to approach into the dock and reset its approach is retrying
+  double dock_approach_timeout;
+  // Timeout to rotate to the dock
+  double rotate_to_dock_timeout;
+  // When undocking, these are the tolerances for arriving at the staging pose
+  double undock_linear_tolerance, undock_angular_tolerance;
+  // Maximum number of times the robot will return to staging pose and retry docking
+  int max_retries;
+  // This is the root frame of the robot - typically "base_link"
+  std::string base_frame;
+  // This is our fixed frame for controlling - typically "odom"
+  std::string fixed_frame;
+  // The tolerance to the dock's staging pose not requiring navigation
+  double dock_prestaging_tolerance;
+  // Angular tolerance to exit the rotation loop when rotate_to_dock is enabled
+  double rotation_angular_tolerance;
+  // Does the robot drive backwards onto the dock? Default is forwards
+  std::optional<bool> dock_backwards;
+  // Parameters for OdomSmoother
+  std::string odom_topic;
+  double odom_duration;
+};
+
+/**
+ * @class opennav_docking::ParameterHandler
+ * @brief Handles parameters and dynamic parameters for Controller Server
+ */
+class ParameterHandler : public nav2_util::ParameterHandler<Parameters>
+{
+public:
+  /**
+   * @brief Constructor for opennav_docking::ParameterHandler
+   */
+  ParameterHandler(
+    const nav2::LifecycleNode::SharedPtr & node,
+    const rclcpp::Logger & logger);
+
+protected:
+  /**
+   * @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(
+    const std::vector<rclcpp::Parameter> & parameters) override;
+
+  /**
+   * @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(const std::vector<rclcpp::Parameter> & parameters) override;
+};
+
+}  // namespace opennav_docking
+
+#endif  // OPENNAV_DOCKING__PARAMETER_HANDLER_HPP_

nav2_docking/opennav_docking/src/controller.cpp

@@ -21,6 +21,8 @@
 #include "nav2_ros_common/node_utils.hpp"
 #include "tf2/utils.hpp"
 
+using rcl_interfaces::msg::ParameterType;
+
 namespace opennav_docking
 {
 
@@ -65,8 +67,14 @@ Controller::Controller(
     v_angular_max_);
 
   // Add callback for dynamic parameters
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
-    std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
+    std::bind(
+      &Controller::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &Controller::validateParameterUpdatesCallback,
+      this, std::placeholders::_1));
 
   if (use_collision_detection_) {
     configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_);
@@ -203,12 +211,35 @@ void Controller::configureCollisionChecker(
     *costmap_sub_, *footprint_sub_, node->get_name());
 }
 
-rcl_interfaces::msg::SetParametersResult
-Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+rcl_interfaces::msg::SetParametersResult Controller::validateParameterUpdatesCallback(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  for (const auto & parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+    if (param_name.find("controller.") != 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;
+      }
+    }
+  }
+  return result;
+}
+
+void
+Controller::updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(dynamic_params_lock_);
 
-  rcl_interfaces::msg::SetParametersResult result;
   for (auto parameter : parameters) {
     const auto & param_type = parameter.get_type();
     const auto & param_name = parameter.get_name();
@@ -250,9 +281,6 @@ Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
       control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
     }
   }
-
-  result.successful = true;
-  return result;
 }
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/src/dock_database.cpp

@@ -17,7 +17,7 @@
 namespace opennav_docking
 {
 
-DockDatabase::DockDatabase(std::shared_ptr<std::mutex> mutex)
+DockDatabase::DockDatabase(std::mutex & mutex)
 : mutex_(mutex),
   dock_loader_("opennav_docking_core", "opennav_docking_core::ChargingDock")
 {}
@@ -85,7 +85,7 @@ void DockDatabase::reloadDbCb(
   const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
-  if (!mutex_->try_lock()) {
+  if (!mutex_.try_lock()) {
     RCLCPP_ERROR(node_.lock()->get_logger(), "Cannot reload database while docking!");
     response->success = false;
     return;
@@ -99,11 +99,11 @@ void DockDatabase::reloadDbCb(
     RCLCPP_INFO(
       node->get_logger(),
       "Dock database reloaded from file %s.", request->filepath.c_str());
-    mutex_->unlock();
+    mutex_.unlock();
     return;
   }
   response->success = false;
-  mutex_->unlock();
+  mutex_.unlock();
 }
 
 Dock * DockDatabase::findDock(const std::string & dock_id)