Added dynamic reconfigure and export controller in docking (ros-navig…

…ation#4429)

* Added dynamic reconfigure and export controller in docking

Signed-off-by: Alberto Tudela <[email protected]>

* Fix vector

Signed-off-by: Alberto Tudela <[email protected]>

* Update controller.cpp

Signed-off-by: Alberto Tudela <[email protected]>

---------

Signed-off-by: Alberto Tudela <[email protected]>

nav2_docking/opennav_docking/CMakeLists.txt

@@ -56,8 +56,15 @@ set(dependencies
   opennav_docking_core
 )
 
-add_library(${library_name} SHARED
+add_library(controller SHARED
   src/controller.cpp
+)
+
+ament_target_dependencies(controller
+  ${dependencies}
+)
+
+add_library(${library_name} SHARED
   src/docking_server.cpp
   src/dock_database.cpp
   src/navigator.cpp
@@ -68,7 +75,9 @@ ament_target_dependencies(${library_name}
 )
 
 target_link_libraries(${library_name}
-  yaml-cpp::yaml-cpp)
+  yaml-cpp::yaml-cpp
+  controller
+)
 
 add_library(pose_filter SHARED
   src/pose_filter.cpp
@@ -100,7 +109,7 @@ target_link_libraries(simple_charging_dock pose_filter)
 
 pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml)
 
-install(TARGETS ${library_name} pose_filter simple_charging_dock
+install(TARGETS ${library_name} controller pose_filter simple_charging_dock
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -127,6 +136,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name} pose_filter)
+ament_export_libraries(${library_name} controller pose_filter)
 ament_export_dependencies(${dependencies} yaml-cpp)
 ament_package()

nav2_docking/opennav_docking/include/opennav_docking/controller.hpp

@@ -16,6 +16,7 @@
 #define OPENNAV_DOCKING__CONTROLLER_HPP_
 
 #include <memory>
+#include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
@@ -47,8 +48,22 @@ class Controller
     const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
     bool backward = false);
 
+  /**
+   * @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_;
+  std::mutex dynamic_params_lock_;
+
 protected:
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
+
+  double k_phi_, k_delta_, beta_, lambda_;
+  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
 };
 
 }  // namespace opennav_docking

nav2_docking/opennav_docking/src/controller.cpp

@@ -40,25 +40,69 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
 
-  double k_phi, k_delta, beta, lambda;
-  double slowdown_radius, v_linear_min, v_linear_max, v_angular_max;
-  node->get_parameter("controller.k_phi", k_phi);
-  node->get_parameter("controller.k_delta", k_delta);
-  node->get_parameter("controller.beta", beta);
-  node->get_parameter("controller.lambda", lambda);
-  node->get_parameter("controller.v_linear_min", v_linear_min);
-  node->get_parameter("controller.v_linear_max", v_linear_max);
-  node->get_parameter("controller.v_angular_max", v_angular_max);
-  node->get_parameter("controller.slowdown_radius", slowdown_radius);
+  node->get_parameter("controller.k_phi", k_phi_);
+  node->get_parameter("controller.k_delta", k_delta_);
+  node->get_parameter("controller.beta", beta_);
+  node->get_parameter("controller.lambda", lambda_);
+  node->get_parameter("controller.v_linear_min", v_linear_min_);
+  node->get_parameter("controller.v_linear_max", v_linear_max_);
+  node->get_parameter("controller.v_angular_max", v_angular_max_);
+  node->get_parameter("controller.slowdown_radius", slowdown_radius_);
   control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
+    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));
 }
 
 bool Controller::computeVelocityCommand(
   const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward)
 {
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
   cmd = control_law_->calculateRegularVelocity(pose, backward);
   return true;
 }
 
+rcl_interfaces::msg::SetParametersResult
+Controller::dynamicParametersCallback(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 & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      if (name == "controller.k_phi") {
+        k_phi_ = parameter.as_double();
+      } else if (name == "controller.k_delta") {
+        k_delta_ = parameter.as_double();
+      } else if (name == "controller.beta") {
+        beta_ = parameter.as_double();
+      } else if (name == "controller.lambda") {
+        lambda_ = parameter.as_double();
+      } else if (name == "controller.v_linear_min") {
+        v_linear_min_ = parameter.as_double();
+      } else if (name == "controller.v_linear_max") {
+        v_linear_max_ = parameter.as_double();
+      } else if (name == "controller.v_angular_max") {
+        v_angular_max_ = parameter.as_double();
+      } else if (name == "controller.slowdown_radius") {
+        slowdown_radius_ = parameter.as_double();
+      }
+
+      // Update the smooth control law with the new params
+      control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
+      control_law_->setSlowdownRadius(slowdown_radius_);
+      control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
 }  // namespace opennav_docking

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -45,4 +45,38 @@ TEST(ControllerTests, ObjectLifecycle)
   controller.reset();
 }
 
+TEST(ControllerTests, DynamicParameters) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto controller = std::make_shared<opennav_docking::Controller>(node);
+
+  auto params = 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());
+
+  // Set parameters
+  auto results = params->set_parameters_atomically(
+    {rclcpp::Parameter("controller.k_phi", 1.0),
+      rclcpp::Parameter("controller.k_delta", 2.0),
+      rclcpp::Parameter("controller.beta", 3.0),
+      rclcpp::Parameter("controller.lambda", 4.0),
+      rclcpp::Parameter("controller.v_linear_min", 5.0),
+      rclcpp::Parameter("controller.v_linear_max", 6.0),
+      rclcpp::Parameter("controller.v_angular_max", 7.0),
+      rclcpp::Parameter("controller.slowdown_radius", 8.0)});
+
+  // Spin
+  rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
+
+  // Check parameters
+  EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0);
+  EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0);
+  EXPECT_EQ(node->get_parameter("controller.beta").as_double(), 3.0);
+  EXPECT_EQ(node->get_parameter("controller.lambda").as_double(), 4.0);
+  EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0);
+  EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0);
+  EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0);
+}
+
 }  // namespace opennav_docking