Add namespace for Rotation Shim primary controller for better dynamic parameters handling

nav2_rotation_shim_controller/CMakeLists.txt

@@ -24,7 +24,8 @@ nav2_package()
 set(library_name nav2_rotation_shim_controller)
 
 add_library(${library_name} SHARED
-  src/nav2_rotation_shim_controller.cpp)
+  src/nav2_rotation_shim_controller.cpp
+  src/parameter_handler.cpp)
 target_include_directories(${library_name}
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -29,6 +29,7 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_controller/plugins/position_goal_checker.hpp"
+#include "nav2_rotation_shim_controller/parameter_handler.hpp"
 
 namespace nav2_rotation_shim_controller
 {
@@ -162,13 +163,6 @@ class RotationShimController : public nav2_core::Controller
    */
   bool isGoalChanged(const nav_msgs::msg::Path & path);
 
-  /**
-   * @brief Callback executed when a parameter change is detected
-   * @param event ParameterEvent message
-   */
-  rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
   nav2::LifecycleNode::WeakPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;
@@ -182,18 +176,11 @@ class RotationShimController : public nav2_core::Controller
   nav2_core::Controller::Ptr primary_controller_;
   bool path_updated_;
   nav_msgs::msg::Path current_path_;
-  double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_;
-  double rotate_to_heading_angular_vel_, max_angular_accel_;
-  double control_duration_, simulate_ahead_time_;
-  bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
-  bool closed_loop_;
-  bool use_path_orientations_;
+  Parameters * params_;
+  bool in_rotation_;
   double last_angular_vel_ = std::numeric_limits<double>::max();
-
-  // Dynamic parameters handler
-  std::mutex mutex_;
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
+  std::unique_ptr<nav2_rotation_shim_controller::ParameterHandler> param_handler_;
 };
 
 }  // namespace nav2_rotation_shim_controller

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/parameter_handler.hpp

@@ -0,0 +1,117 @@
+// 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 NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_
+#define NAV2_ROTATION_SHIM_CONTROLLER__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/odometry_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_ros_common/node_utils.hpp"
+
+namespace nav2_rotation_shim_controller
+{
+
+struct Parameters
+{
+  double forward_sampling_distance;
+  double angular_dist_threshold;
+  double angular_disengage_threshold;
+  double rotate_to_heading_angular_vel;
+  double max_angular_accel;
+  double simulate_ahead_time;
+  double control_duration;
+  bool rotate_to_goal_heading;
+  bool rotate_to_heading_once;
+  bool closed_loop;
+  bool use_path_orientations;
+  std::string primary_controller;
+};
+
+/**
+ * @class nav2_rotation_shim_controller::ParameterHandler
+ * @brief Handles parameters and dynamic parameters for Rotation Shim
+ */
+class ParameterHandler
+{
+public:
+  /**
+   * @brief Constructor for nav2_rotation_shim_controller::ParameterHandler
+   */
+  ParameterHandler(
+    const nav2::LifecycleNode::SharedPtr & node,
+    std::string & plugin_name,
+    rclcpp::Logger & logger);
+
+  /**
+   * @brief Destrructor for nav2_rotation_shim_controller::ParameterHandler
+   */
+  ~ParameterHandler();
+
+  std::mutex & getMutex() {return mutex_;}
+
+  Parameters * getParams() {return &params_;}
+
+    /**
+  * @brief Registers callbacks for dynamic parameter handling.
+  */
+  void activate();
+
+  /**
+  * @brief Resets callbacks for dynamic parameter handling.
+  */
+  void deactivate();
+
+protected:
+  nav2::LifecycleNode::WeakPtr node_;
+
+  /**
+   * @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);
+
+  /**
+   * @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);
+
+  // Dynamic parameters handler
+  std::mutex mutex_;
+  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("RotationShimController")};
+};
+
+}  // namespace nav2_rotation_shim_controller
+
+#endif  // NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_