From 94ea26bffb6f0f4f339cc94830988d57703b1b38 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Wed, 24 Jul 2019 14:01:23 -0300 Subject: [PATCH] Allow registering multiple on_parameters_set_callback (#772) Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/node.hpp | 63 ++++++++++- .../node_interfaces/node_parameters.hpp | 13 +++ .../node_parameters_interface.hpp | 36 +++++- rclcpp/src/rclcpp/node.cpp | 12 ++ .../node_interfaces/node_parameters.cpp | 105 ++++++++++++++++-- rclcpp/test/test_node.cpp | 77 +++++++++++++ 6 files changed, 288 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e752404..6a9258c 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -889,10 +889,12 @@ public: rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; - /// Register a callback to be called anytime a parameter is about to be changed. + /// Add a callback for when parameters are being set. /** * The callback signature is designed to allow handling of any of the above * `set_parameter*` or `declare_parameter*` methods, and so it takes a const @@ -936,9 +938,60 @@ public: * of the latter things, * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown. * - * There may only be one callback set at a time, so the previously set - * callback is returned when this method is used, or nullptr will be returned - * if no callback was previously set. + * The callback functions must remain valid as long as the + * returned smart pointer is valid. + * The returned smart pointer can be promoted to a shared version. + * + * Resetting or letting the smart pointer go out of scope unregisters the callback. + * `remove_on_set_parameters_callback` can also be used. + * + * The registered callbacks are called when a parameter is set. + * When a callback returns a not successful result, the remaining callbacks aren't called. + * The order of the callback is the reverse from the registration order. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback); + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * Delete a handler returned by `add_on_set_parameters_callback`. + * + * e.g.: + * + * `remove_on_set_parameters_callback(scoped_callback.get())` + * + * As an alternative, the smart pointer can be reset: + * + * `scoped_callback.reset()` + * + * Supposing that `scoped_callback` was the only owner. + * + * Calling `remove_on_set_parameters_callback` more than once with the same handler, + * or calling it after the shared pointer has been reset is an error. + * Resetting or letting the smart pointer go out of scope after calling + * `remove_on_set_parameters_callback` is not a problem. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + + /// Register a callback to be called anytime a parameter is about to be changed. + /** + * With this method, only one callback can be set at a time. The callback that was previously + * set by this method is returned or `nullptr` if no callback was previously set. + * + * The callbacks added with `add_on_set_parameters_callback` are stored in a different place. + * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this + * method. For removing it, use `set_on_parameters_set_callback(nullptr)`. * * \param[in] callback The callback to be called when the value for a * parameter is about to be set. @@ -946,7 +999,7 @@ public: * otherwise nullptr. */ RCLCPP_PUBLIC - rclcpp::Node::OnParametersSetCallbackType + OnParametersSetCallbackType set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback); /// Register the callback for parameter changes diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 5c9cb69..b114e1b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -157,6 +158,14 @@ public: rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const override; + RCLCPP_PUBLIC + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback) override; + + RCLCPP_PUBLIC + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + RCLCPP_PUBLIC OnParametersSetCallbackType set_on_parameters_set_callback(OnParametersSetCallbackType callback) override; @@ -170,6 +179,8 @@ public: const std::map & get_parameter_overrides() const override; + using CallbacksContainerType = std::list; + private: RCLCPP_DISABLE_COPY(NodeParameters) @@ -182,6 +193,8 @@ private: OnParametersSetCallbackType on_parameters_set_callback_ = nullptr; + CallbacksContainerType on_parameters_set_callback_container_; + std::map parameters_; std::map parameter_overrides_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 68f8c55..79cc357 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -16,6 +16,7 @@ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ #include +#include #include #include @@ -32,6 +33,18 @@ namespace rclcpp namespace node_interfaces { +struct OnSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) + + using OnParametersSetCallbackType = + std::function< + rcl_interfaces::msg::SetParametersResult( + const std::vector &)>; + + OnParametersSetCallbackType callback; +}; + /// Pure virtual interface class for the NodeParameters part of the Node API. class NodeParametersInterface { @@ -158,14 +171,29 @@ public: rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; - using OnParametersSetCallbackType = - std::function< - rcl_interfaces::msg::SetParametersResult(const std::vector &) - >; + using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] = OnParametersSetCallbackType; + /// Add a callback for when parameters are being set. + /** + * \sa rclcpp::Node::add_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0; + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + /// Register a callback for when parameters are being set, return an existing one. /** * \sa rclcpp::Node::set_on_parameters_set_callback diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 98aab12..902a5be 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -326,6 +326,18 @@ Node::list_parameters(const std::vector & prefixes, uint64_t depth) return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr +Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +{ + return node_parameters_->add_on_set_parameters_callback(callback); +} + +void +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +{ + return node_parameters_->remove_on_set_parameters_callback(callback); +} + rclcpp::Node::OnParametersSetCallbackType Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index f48cd5e..8dd064e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -289,20 +290,50 @@ __check_parameters( using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; +using CallbacksContainerType = + rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__call_on_parameters_set_callbacks( + const std::vector & parameters, + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + result = shared_handle->callback(parameters); + if (!result.successful) { + return result; + } + it++; + } else { + it = callback_container.erase(it); + } + } + if (callback) { + result = callback(parameters); + } + return result; +} RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult __set_parameters_atomically_common( const std::vector & parameters, std::map & parameter_infos, - OnParametersSetCallbackType on_set_parameters_callback) + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback) { // Call the user callback to see if the new value(s) are allowed. - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - if (on_set_parameters_callback) { - result = on_set_parameters_callback(parameters); - } + rcl_interfaces::msg::SetParametersResult result = + __call_on_parameters_set_callbacks(parameters, callback_container, callback); if (!result.successful) { return result; } @@ -333,7 +364,8 @@ __declare_parameter_common( const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, std::map & parameters_out, const std::map & overrides, - OnParametersSetCallbackType on_set_parameters_callback, + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback, rcl_interfaces::msg::ParameterEvent * parameter_event_out, bool ignore_override = false) { @@ -354,7 +386,8 @@ __declare_parameter_common( auto result = __set_parameters_atomically_common( parameter_wrappers, parameter_infos, - on_set_parameters_callback); + callback_container, + callback); // Add declared parameters to storage. parameters_out[name] = parameter_infos.at(name); @@ -396,6 +429,7 @@ NodeParameters::declare_parameter( parameter_descriptor, parameters_, parameter_overrides_, + on_parameters_set_callback_container_, on_parameters_set_callback_, ¶meter_event, ignore_override); @@ -523,6 +557,7 @@ NodeParameters::set_parameters_atomically(const std::vector & std::map staged_parameter_changes; rcl_interfaces::msg::ParameterEvent parameter_event_msg; parameter_event_msg.node = combined_name_; + CallbacksContainerType empty_callback_container; for (auto parameter_to_be_declared : parameters_to_be_declared) { // This should not throw, because we validated the name and checked that // the parameter was not already declared. @@ -532,7 +567,9 @@ NodeParameters::set_parameters_atomically(const std::vector & rcl_interfaces::msg::ParameterDescriptor(), // Implicit declare uses default descriptor. staged_parameter_changes, parameter_overrides_, - nullptr, // callback is explicitly null, so that it is called only once, when setting below. + // Only call callbacks once below + empty_callback_container, // callback_container is explicitly empty + nullptr, // callback is explicitly null. ¶meter_event_msg, true); if (!result.successful) { @@ -588,6 +625,9 @@ NodeParameters::set_parameters_atomically(const std::vector & // they are actually set on the official parameter storage parameters_, // this will get called once, with all the parameters to be set + on_parameters_set_callback_container_, + // These callbacks are called once. When a callback returns an unsuccessful result, + // the remaining aren't called. on_parameters_set_callback_); // If not successful, then stop here. @@ -820,6 +860,53 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } +struct HandleCompare + : public std::unary_function +{ + explicit HandleCompare(const OnSetParametersCallbackHandle * const base) + : base_(base) {} + bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle) + { + auto shared_handle = handle.lock(); + if (base_ == shared_handle.get()) { + return true; + } + return false; + } + const OnSetParametersCallbackHandle * const base_; +}; + +void +NodeParameters::remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + on_parameters_set_callback_container_.begin(), + on_parameters_set_callback_container_.end(), + HandleCompare(handle)); + if (it != on_parameters_set_callback_container_.end()) { + on_parameters_set_callback_container_.erase(it); + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +OnSetParametersCallbackHandle::SharedPtr +NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + on_parameters_set_callback_container_.emplace_front(handle); + return handle; +} + NodeParameters::OnParametersSetCallbackType NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback) { diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 414de0e..26707d9 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -362,6 +362,83 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { } } +auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful) +{ + return + [name, successful](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; + result.successful = successful; + return result; + }; +} + +TEST_F(TestNode, test_registering_multiple_callbacks_api) { + auto node = std::make_shared("test_declare_parameter_node"_unq); + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + scoped_callback2.reset(); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + node->remove_on_set_parameters_callback(scoped_callback2.get()); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback( + node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, false))); + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback_copy(scoped_callback); + scoped_callback.reset(); + + EXPECT_THROW( + {node->declare_parameter("parameter"_unq, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + scoped_callback_copy.reset(); + // All the shared_ptr has been reset + int64_t value = node->declare_parameter("parameter"_unq, default_value); + EXPECT_EQ(value, default_value); + } +} + TEST_F(TestNode, declare_parameter_with_overrides) { // test cases with overrides rclcpp::NodeOptions no;