diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 38e9b795164..e50590d3e41 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/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() diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 54472e720fd..afae5965d22 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -16,6 +16,7 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include +#include #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 parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; + protected: std::unique_ptr control_law_; + + double k_phi_, k_delta_, beta_, lambda_; + double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 3060aaf8c8c..8613f2d27fc 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/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( - 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 lock(dynamic_params_lock_); cmd = control_law_->calculateRegularVelocity(pose, backward); return true; } +rcl_interfaces::msg::SetParametersResult +Controller::dynamicParametersCallback(std::vector parameters) +{ + std::lock_guard 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 diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index da8885d89d1..c2f1e25f1c1 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/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("test"); + auto controller = std::make_shared(node); + + auto params = std::make_shared( + 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