diff --git a/README.md b/README.md index b61a2a7..6a2fc99 100644 --- a/README.md +++ b/README.md @@ -422,6 +422,11 @@ if (param_listener->is_old(params_)) { } ``` +Alternatively, you can bind a callback function that triggers whenever a parameter is updated. When activated, the callback receives the updated parameters as an argument. +```c++ +parameter_listener.setUserCallback([this](const auto& params) { reconfigure_callback(params); }); +``` + ### Parameter documentation In some cases, parameters might be unknown only at compile-time, and cannot be part of the generated C++ code. However, for documentation purpose of such parameters, the type `none` was introduced. diff --git a/example/include/generate_parameter_library_example/minimal_publisher.hpp b/example/include/generate_parameter_library_example/minimal_publisher.hpp index 9a002b7..10fef02 100644 --- a/example/include/generate_parameter_library_example/minimal_publisher.hpp +++ b/example/include/generate_parameter_library_example/minimal_publisher.hpp @@ -41,6 +41,7 @@ class MinimalPublisher : public rclcpp::Node { private: void timer_callback(); + void reconfigure_callback(const admittance_controller::Params& params); rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr param_listener_; diff --git a/example/src/minimal_publisher.cpp b/example/src/minimal_publisher.cpp index 62cdaa4..be9f571 100644 --- a/example/src/minimal_publisher.cpp +++ b/example/src/minimal_publisher.cpp @@ -42,6 +42,8 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions& options) 500ms, std::bind(&MinimalPublisher::timer_callback, this)); param_listener_ = std::make_shared(get_node_parameters_interface()); + param_listener_->setUserCallback( + [this](const auto& params) { reconfigure_callback(params); }); params_ = param_listener_->get_params(); [[maybe_unused]] StackParams s_params = param_listener_->get_stack_params(); @@ -71,4 +73,17 @@ void MinimalPublisher::timer_callback() { } } +void MinimalPublisher::reconfigure_callback( + const admittance_controller::Params& params) { + RCLCPP_INFO(get_logger(), "Reconfigure callback fired!"); + RCLCPP_INFO(get_logger(), "New control frame parameter is: '%s'", + params.control.frame.id.c_str()); + RCLCPP_INFO(get_logger(), "fixed string is: '%s'", + std::string{params.fixed_string}.c_str()); + const auto fixed_array = params.fixed_array; + for (auto d : fixed_array) { + RCLCPP_INFO(get_logger(), "value: '%s'", std::to_string(d).c_str()); + } +} + } // namespace admittance_controller diff --git a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header index 179d506..74509c6 100644 --- a/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header +++ b/generate_parameter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header @@ -182,6 +182,9 @@ struct StackParams { {%- endif %} updated_params.__stamp = clock_.now(); update_internal_params(updated_params); + if (user_callback_) { + user_callback_(updated_params); + } return rsl::to_parameter_result_msg({}); } @@ -208,6 +211,15 @@ struct StackParams { update_internal_params(updated_params); } + using userParameterUpdateCB = std::function; + void setUserCallback(const userParameterUpdateCB& callback){ + user_callback_ = callback; + } + + void clearUserCallback(){ + user_callback_ = {}; + } + private: void update_internal_params(Params updated_params) { std::lock_guard lock(mutex_); @@ -219,6 +231,7 @@ struct StackParams { rclcpp::Clock clock_; std::shared_ptr handle_; std::shared_ptr parameters_interface_; + userParameterUpdateCB user_callback_; // rclcpp::Logger cannot be default-constructed // so we must provide a initialization here even though