From a91f464d9788ecb37f6fd7dfae751661f1db89cf Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Wed, 25 Sep 2019 13:15:43 -0700 Subject: [PATCH] map parameters to list of callbacks functions to remove parameter callbacks add functions to remove event callbacks, remove subscriptions, allow subscribing event callback to many namespaces, additional test coverage Signed-off-by: bpwilcox --- .../rclcpp/parameter_events_subscriber.hpp | 100 ++++++++++- .../rclcpp/parameter_events_subscriber.cpp | 162 ++++++++++++++++-- .../test/test_parameter_events_subscriber.cpp | 82 +++++++-- 3 files changed, 311 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_events_subscriber.hpp b/rclcpp/include/rclcpp/parameter_events_subscriber.hpp index 0c92d5f111..f8f813a0ab 100644 --- a/rclcpp/include/rclcpp/parameter_events_subscriber.hpp +++ b/rclcpp/include/rclcpp/parameter_events_subscriber.hpp @@ -26,6 +26,17 @@ namespace rclcpp { +struct ParameterEventsCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle) + + using ParameterEventsCallbackType = std::function; + + std::string parameter_name; + std::string node_name; + ParameterEventsCallbackType callback; +}; + class ParameterEventsSubscriber { public: @@ -56,13 +67,25 @@ class ParameterEventsSubscriber * provided callback will be applied to all of them. * * \param[in] callback Function callback to be evaluated on event. - * \param[in] node_namespace Name of namespace for which a subscription will be created. + * \param[in] node_namespaces Vector of namespaces for which a subscription will be created. */ RCLCPP_PUBLIC void set_event_callback( std::function callback, - const std::string & node_namespace = ""); + const std::vector & node_namespaces = {""}); + + /// Remove parameter event callback. + /** + * Calling this function will set the event callback to nullptr. This function will also remove + * event subscriptions on the namespaces for which there are no other callbacks (from parameter + * callbacks) active on that namespace. + */ + RCLCPP_PUBLIC + void + remove_event_callback(); + + using ParameterEventsCallbackType = ParameterEventsCallbackHandle::ParameterEventsCallbackType; /// Add a custom callback for a specified parameter. /** @@ -73,12 +96,50 @@ class ParameterEventsSubscriber * \param[in] node_name Name of node which hosts the parameter. */ RCLCPP_PUBLIC + ParameterEventsCallbackHandle::SharedPtr + add_parameter_callback( + const std::string & parameter_name, + ParameterEventsCallbackType callback, + const std::string & node_name = ""); + + /// Remove a custom callback for a specified parameter given its callback handle. + /** + * The parameter name and node name are inspected from the callback handle. The callback handle + * is erased from the list of callback handles on the {parameter_name, node_name} in the map. + * An error is thrown if the handle does not exist and/or was already removed. + * + * \param[in] handle Pointer to callback handle to be removed. + */ + RCLCPP_PUBLIC void - register_parameter_callback( + remove_parameter_callback( + const ParameterEventsCallbackHandle * const handle); + + /// Remove a custom callback for a specified parameter given its name and respective node. + /** + * If a node_name is not provided, defaults to the current node. + * The {parameter_name, node_name} key on the parameter_callbacks_ map will be erased, removing + * all callback associated with that parameter. + * + * \param[in] parameter_name Name of parameter. + * \param[in] node_name Name of node which hosts the parameter. + */ + RCLCPP_PUBLIC + void + remove_parameter_callback( const std::string & parameter_name, - std::function callback, const std::string & node_name = ""); + /// Get a rclcpp::Parameter from parameter event, return true if parameter name & node in event. + /** + * If a node_name is not provided, defaults to the current node. + * + * \param[in] event Event msg to be inspected. + * \param[out] parameter Reference to rclcpp::Parameter to be assigned. + * \param[in] parameter_name Name of parameter. + * \param[in] node_name Name of node which hosts the parameter. + * \returns true if requested parameter name & node is in event, false otherwise + */ RCLCPP_PUBLIC static bool get_parameter_from_event( @@ -87,6 +148,19 @@ class ParameterEventsSubscriber const std::string parameter_name, const std::string node_name = ""); + /// Get a rclcpp::Parameter from parameter event + /** + * If a node_name is not provided, defaults to the current node. + * + * The user is responsible to check if the returned parameter has been properly assigned. + * By default, if the requested parameter is not found in the event, the returned parameter + * has parameter value of type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] event Event msg to be inspected. + * \param[in] parameter_name Name of parameter. + * \param[in] node_name Name of node which hosts the parameter. + * \returns The resultant rclcpp::Parameter from the event + */ RCLCPP_PUBLIC static rclcpp::Parameter get_parameter_from_event( @@ -94,11 +168,21 @@ class ParameterEventsSubscriber const std::string parameter_name, const std::string node_name = ""); + using CallbacksContainerType = std::list; + protected: /// Add a subscription (if unique) to a namespace parameter events topic. void add_namespace_event_subscriber(const std::string & node_namespace); + /// Remove a subscription to a namespace parameter events topic. + void + remove_namespace_event_subscriber(const std::string & node_namespace); + + /// Return true if any callbacks still exist on a namespace event topic, otherwise return false + bool + should_unsubscribe_to_namespace(const std::string & node_namespace); + /// Callback for parameter events subscriptions. void event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); @@ -140,18 +224,20 @@ class ParameterEventsSubscriber // Map container for registered parameters. std::unordered_map< std::pair, - std::function, + CallbacksContainerType, StringPairHash > parameter_callbacks_; // Vector of unique namespaces added. - std::vector node_namespaces_; + std::vector subscribed_namespaces_; + // Vector of event callback namespaces + std::vector event_namespaces_; // Vector of event subscriptions for each namespace. std::vector::SharedPtr> event_subscriptions_; - std::function user_callback_; + std::function event_callback_; std::recursive_mutex mutex_; }; diff --git a/rclcpp/src/rclcpp/parameter_events_subscriber.cpp b/rclcpp/src/rclcpp/parameter_events_subscriber.cpp index b1d77a8c3c..ec88ca28b9 100644 --- a/rclcpp/src/rclcpp/parameter_events_subscriber.cpp +++ b/rclcpp/src/rclcpp/parameter_events_subscriber.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include +#include #include "rclcpp/parameter_events_subscriber.hpp" @@ -37,10 +39,10 @@ void ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace) { std::lock_guard lock(mutex_); - if (std::find(node_namespaces_.begin(), node_namespaces_.end(), - node_namespace) == node_namespaces_.end()) + if (std::find(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), + node_namespace) == subscribed_namespaces_.end()) { - node_namespaces_.push_back(node_namespace); + subscribed_namespaces_.push_back(node_namespace); auto topic = join_path(node_namespace, "parameter_events"); RCLCPP_DEBUG(node_logging_->get_logger(), "Subscribing to topic: %s", topic.c_str()); @@ -52,32 +54,153 @@ ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & no } } +void +ParameterEventsSubscriber::remove_namespace_event_subscriber(const std::string & node_namespace) +{ + std::lock_guard lock(mutex_); + auto check_sub_cb = [this, &node_namespace]( + rclcpp::Subscription::SharedPtr sub) { + return sub->get_topic_name() == join_path(node_namespace, "parameter_events"); + }; + + auto it = std::find_if( + event_subscriptions_.begin(), + event_subscriptions_.end(), + check_sub_cb); + + if (it != event_subscriptions_.end()) { + event_subscriptions_.erase(it); + subscribed_namespaces_.erase( + std::remove(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), node_namespace)); + } +} + void ParameterEventsSubscriber::set_event_callback( std::function callback, - const std::string & node_namespace) + const std::vector & node_namespaces) { std::lock_guard lock(mutex_); + remove_event_callback(); std::string full_namespace; - if (node_namespace == "") { - full_namespace = node_base_->get_namespace(); - } else { - full_namespace = resolve_path(full_namespace); + for (auto & ns : node_namespaces) { + if (ns == "") { + full_namespace = node_base_->get_namespace(); + } else { + full_namespace = resolve_path(ns); + } + add_namespace_event_subscriber(full_namespace); + event_namespaces_.push_back(full_namespace); } - add_namespace_event_subscriber(full_namespace); - user_callback_ = callback; + + event_callback_ = callback; } void -ParameterEventsSubscriber::register_parameter_callback( +ParameterEventsSubscriber::remove_event_callback() +{ + std::lock_guard lock(mutex_); + // Clear current vector of event namespaces and remove subscriptions + auto temp_namespaces = event_namespaces_; + event_namespaces_.clear(); + for (auto temp_ns : temp_namespaces) { + if (should_unsubscribe_to_namespace(temp_ns)) { + remove_namespace_event_subscriber(temp_ns); + } + } + + event_callback_ = nullptr; +} + +ParameterEventsCallbackHandle::SharedPtr +ParameterEventsSubscriber::add_parameter_callback( const std::string & parameter_name, - std::function callback, + ParameterEventsCallbackType callback, const std::string & node_name) { std::lock_guard lock(mutex_); auto full_node_name = resolve_path(node_name); add_namespace_event_subscriber(split_path(full_node_name).first); - parameter_callbacks_[{parameter_name, full_node_name}] = callback; + + auto handle = std::make_shared(); + handle->callback = callback; + handle->parameter_name = parameter_name; + handle->node_name = full_node_name; + // the last callback registered is executed first. + parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); + + return handle; +} + +struct HandleCompare + : public std::unary_function +{ + explicit HandleCompare(const ParameterEventsCallbackHandle * const base) + : base_(base) {} + bool operator()(const ParameterEventsCallbackHandle::WeakPtr & handle) + { + auto shared_handle = handle.lock(); + if (base_ == shared_handle.get()) { + return true; + } + return false; + } + const ParameterEventsCallbackHandle * const base_; +}; + +void +ParameterEventsSubscriber::remove_parameter_callback( + const ParameterEventsCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}]; + auto it = std::find_if( + container.begin(), + container.end(), + HandleCompare(handle)); + if (it != container.end()) { + container.erase(it); + if (container.empty()) { + parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); + if (should_unsubscribe_to_namespace(split_path(handle->node_name).first)) { + remove_namespace_event_subscriber(split_path(handle->node_name).first); + } + } + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +bool +ParameterEventsSubscriber::should_unsubscribe_to_namespace(const std::string & node_namespace) +{ + auto it1 = std::find(event_namespaces_.begin(), event_namespaces_.end(), node_namespace); + if (it1 != event_namespaces_.end()) { + return false; + } + + auto it2 = parameter_callbacks_.begin(); + while (it2 != parameter_callbacks_.end()) { + if (split_path(it2->first.second).first == node_namespace) { + return false; + } + ++it2; + } + return true; +} + +void +ParameterEventsSubscriber::remove_parameter_callback( + const std::string & parameter_name, + const std::string & node_name) +{ + std::lock_guard lock(mutex_); + auto full_node_name = resolve_path(node_name); + parameter_callbacks_.erase({parameter_name, full_node_name}); + if (should_unsubscribe_to_namespace(split_path(full_node_name).first)) { + RCLCPP_INFO(node_logging_->get_logger(), "Removing namespace event subscription"); + remove_namespace_event_subscriber(split_path(full_node_name).first); + } } bool @@ -124,12 +247,19 @@ ParameterEventsSubscriber::event_callback( for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) { rclcpp::Parameter p; if (get_parameter_from_event(event, p, it->first.first, it->first.second)) { - it->second(p); + for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) { + auto shared_handle = cb->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(p); + } else { + cb = it->second.erase(cb); + } + } } } - if (user_callback_) { - user_callback_(event); + if (event_callback_) { + event_callback_(event); } } diff --git a/rclcpp/test/test_parameter_events_subscriber.cpp b/rclcpp/test/test_parameter_events_subscriber.cpp index cb7532bb65..c4c784a9f9 100644 --- a/rclcpp/test/test_parameter_events_subscriber.cpp +++ b/rclcpp/test/test_parameter_events_subscriber.cpp @@ -30,6 +30,11 @@ class TestParameterEventsSubscriber : public rclcpp::ParameterEventsSubscriber { event_callback(event); } + + int get_number_of_subscriptions() + { + return event_subscriptions_.size(); + } }; class TestNode : public ::testing::Test @@ -116,10 +121,10 @@ TEST_F(TestNode, RegisterParameterCallback) bool received; auto cb = [&received](const rclcpp::Parameter &) {received = true;}; - ParamSubscriber->register_parameter_callback("my_double", cb); // same node - ParamSubscriber->register_parameter_callback("my_int", cb); // same node - ParamSubscriber->register_parameter_callback("my_string", cb, remote_node_name); // remote node - ParamSubscriber->register_parameter_callback("my_bool", cb, diff_ns_name); // different namespace + auto h1 = ParamSubscriber->add_parameter_callback("my_double", cb); + auto h2 = ParamSubscriber->add_parameter_callback("my_int", cb); + auto h3 = ParamSubscriber->add_parameter_callback("my_string", cb, remote_node_name); + auto h4 = ParamSubscriber->add_parameter_callback("my_bool", cb, diff_ns_name); received = false; ParamSubscriber->test_event(same_node_double); @@ -151,8 +156,8 @@ TEST_F(TestNode, SameParameterDifferentNode) }; // Set individual parameters - ParamSubscriber->register_parameter_callback("my_int", cb1); - ParamSubscriber->register_parameter_callback("my_int", cb2, remote_node_name); + auto h1 = ParamSubscriber->add_parameter_callback("my_int", cb1); + auto h2 = ParamSubscriber->add_parameter_callback("my_int", cb2, remote_node_name); ParamSubscriber->test_event(same_node_int); EXPECT_EQ(int_param_node1, 1); @@ -166,17 +171,18 @@ TEST_F(TestNode, SameParameterDifferentNode) EXPECT_EQ(int_param_node2, 1); } -TEST_F(TestNode, UserCallback) +TEST_F(TestNode, EventCallback) { using rclcpp::ParameterEventsSubscriber; double double_param = 0.0; int int_param = 0; - bool received = false; + bool bool_param{false}; + bool received{false}; double product; auto cb = - [&int_param, &double_param, &product, &received, + [&int_param, &double_param, &product, &bool_param, &received, this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) { auto node_name = node->get_fully_qualified_name(); @@ -197,13 +203,69 @@ TEST_F(TestNode, UserCallback) } product = int_param * double_param; + + if (ParameterEventsSubscriber::get_parameter_from_event(event, p, "my_bool", diff_ns_name)) { + bool_param = p.get_value(); + } }; - ParamSubscriber->set_event_callback(cb); + ParamSubscriber->set_event_callback(cb, {"ns", node->get_namespace()}); + EXPECT_EQ(ParamSubscriber->get_number_of_subscriptions(), 2); ParamSubscriber->test_event(diff_ns_bool); EXPECT_EQ(received, false); + EXPECT_EQ(bool_param, true); + bool_param = false; ParamSubscriber->test_event(multiple); + EXPECT_EQ(received, true); EXPECT_EQ(product, 1.0); + EXPECT_EQ(bool_param, false); + + ParamSubscriber->remove_event_callback(); + EXPECT_EQ(ParamSubscriber->get_number_of_subscriptions(), 0); +} + +TEST_F(TestNode, MultipleParameterCallbacks) +{ + bool received_1{false}; + bool received_2{false}; + + auto cb1 = [&received_1](const rclcpp::Parameter &) {received_1 = true;}; + auto cb2 = [&received_2](const rclcpp::Parameter &) {received_2 = true;}; + auto cb3 = [](const rclcpp::Parameter &) {/*do nothing*/}; + auto event_cb = [](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) {/*do nothing*/}; + + auto h1 = ParamSubscriber->add_parameter_callback("my_int", cb1); + auto h2 = ParamSubscriber->add_parameter_callback("my_int", cb2); + auto h3 = ParamSubscriber->add_parameter_callback("my_double", cb3); + + // Test multiple callbacks per parameter + ParamSubscriber->test_event(same_node_int); + EXPECT_EQ(received_1, true); + EXPECT_EQ(received_2, true); + + // Test removal of parameter callback by callback handle + received_1 = false; + received_2 = false; + ParamSubscriber->remove_parameter_callback(h1.get()); + ParamSubscriber->test_event(same_node_int); + EXPECT_EQ(received_1, false); + EXPECT_EQ(received_2, true); + + // Test removal of parameter callback by name + received_2 = false; + ParamSubscriber->remove_parameter_callback("my_int"); + ParamSubscriber->test_event(same_node_int); + EXPECT_EQ(received_2, false); + EXPECT_EQ(ParamSubscriber->get_number_of_subscriptions(), 1); // still has other parameter + + // Test subscription removal when all parameter callbacks removed (with event callback) + ParamSubscriber->set_event_callback(event_cb); + ParamSubscriber->remove_parameter_callback("my_double"); + EXPECT_EQ(ParamSubscriber->get_number_of_subscriptions(), 1); // has event callback + + // Test subscription removal when all parameter and event callbacks removed + ParamSubscriber->remove_event_callback(); + EXPECT_EQ(ParamSubscriber->get_number_of_subscriptions(), 0); // no more callbacks }