diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 745d745447..b39e8a0964 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -20,6 +20,7 @@ #include #include "nav2_util/node_thread.hpp" +#include "nav2_util/parameter_events_subscriber.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -125,6 +126,8 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // When creating a local node, this class will launch a separate thread created to spin the node std::unique_ptr rclcpp_thread_; + + std::shared_ptr param_subscriber_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp new file mode 100644 index 0000000000..e15dd01549 --- /dev/null +++ b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp @@ -0,0 +1,239 @@ +// Copyright 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ +#define NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +namespace nav2_util +{ + +struct ParameterEventsCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle) + + using ParameterEventsCallbackType = std::function; + + std::string parameter_name; + std::string node_name; + ParameterEventsCallbackType callback; +}; + +class ParameterEventsSubscriber +{ +public: + ParameterEventsSubscriber( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS()); + + template + ParameterEventsSubscriber( + NodeT node, + const rclcpp::QoS & qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) + : ParameterEventsSubscriber( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface(), + qos) + {} + + /// Set a custom callback for parameter events. + /** + * If no namespace is provided, a subscription will be created for the current namespace. + * Repeated calls to this function will overwrite the callback. + * If more than one namespace already has a subscription to its parameter events topic, then the + * provided callback will be applied to all of them. + * + * \param[in] callback Function callback to be evaluated on event. + * \param[in] node_namespaces Vector of namespaces for which a subscription will be created. + */ + void + set_event_callback( + std::function callback, + 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. + */ + void + remove_event_callback(); + + using ParameterEventsCallbackType = ParameterEventsCallbackHandle::ParameterEventsCallbackType; + + /// Add a custom callback for a specified parameter. + /** + * If a node_name is not provided, defaults to the current node. + * + * \param[in] parameter_name Name of parameter. + * \param[in] callback Function callback to be evaluated upon parameter event. + * \param[in] node_name Name of node which hosts the parameter. + */ + 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. + */ + void + 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. + */ + void + remove_parameter_callback( + const std::string & parameter_name, + 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 + */ + static bool + get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event, + rclcpp::Parameter & parameter, + 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 + */ + static rclcpp::Parameter + get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event, + 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); + + // Utility functions for string and path name operations. + std::string resolve_path(const std::string & path); + std::pair split_path(const std::string & str); + std::string join_path(std::string path, std::string name); + + // Node Interfaces used for logging and creating subscribers. + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + + rclcpp::QoS qos_; + + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels + // Hash function for string pair required in std::unordered_map + class StringPairHash + { + public: + template + inline void hash_combine(std::size_t & seed, const T & v) const + { + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + + inline size_t operator()(const std::pair & s) const + { + size_t seed = 0; + hash_combine(seed, s.first); + hash_combine(seed, s.second); + return seed; + } + }; + // *INDENT-ON* + + // Map container for registered parameters. + std::unordered_map< + std::pair, + CallbacksContainerType, + StringPairHash + > parameter_callbacks_; + + // Vector of unique namespaces added. + 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 event_callback_; + + std::recursive_mutex mutex_; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 70f1c7b624..bf5aae60c5 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(${library_name} SHARED lifecycle_node.cpp robot_utils.cpp node_thread.cpp + parameter_events_subscriber.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e6efb5f6a9..74216894a8 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -57,6 +57,8 @@ LifecycleNode::LifecycleNode( "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); rclcpp_thread_ = std::make_unique(rclcpp_node_); } + + param_subscriber_ = std::make_shared(shared_from_this()); } LifecycleNode::~LifecycleNode() diff --git a/nav2_util/src/parameter_events_subscriber.cpp b/nav2_util/src/parameter_events_subscriber.cpp new file mode 100644 index 0000000000..5d1b9c8053 --- /dev/null +++ b/nav2_util/src/parameter_events_subscriber.cpp @@ -0,0 +1,308 @@ +// Copyright 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/parameter_events_subscriber.hpp" + +namespace nav2_util +{ + +ParameterEventsSubscriber::ParameterEventsSubscriber( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const rclcpp::QoS & qos) +: node_base_(node_base), + node_topics_(node_topics), + node_logging_(node_logging), + qos_(qos) +{} + +void +ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace) +{ + std::lock_guard lock(mutex_); + if (std::find(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), + node_namespace) == subscribed_namespaces_.end()) + { + 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()); + + auto event_sub = rclcpp::create_subscription( + node_topics_, topic, qos_, + std::bind(&ParameterEventsSubscriber::event_callback, this, std::placeholders::_1)); + + event_subscriptions_.push_back(event_sub); + } +} + +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::vector & node_namespaces) +{ + std::lock_guard lock(mutex_); + remove_event_callback(); + std::string 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); + } + + event_callback_ = callback; +} + +void +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, + 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); + + 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 +ParameterEventsSubscriber::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event, + rclcpp::Parameter & parameter, + const std::string parameter_name, + const std::string node_name) +{ + if (event->node == node_name) { + rclcpp::ParameterEventsFilter filter(event, {parameter_name}, + {rclcpp::ParameterEventsFilter::EventType::NEW, + rclcpp::ParameterEventsFilter::EventType::CHANGED}); + if (!filter.get_events().empty()) { + const auto & events = filter.get_events(); + auto param_msg = events.back().second; + parameter = rclcpp::Parameter::from_parameter_msg(*param_msg); + return true; + } + } + return false; +} + +rclcpp::Parameter +ParameterEventsSubscriber::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event, + const std::string parameter_name, + const std::string node_name) +{ + rclcpp::Parameter p(parameter_name); + get_parameter_from_event(event, p, parameter_name, node_name); + return p; +} + +void +ParameterEventsSubscriber::event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + std::lock_guard lock(mutex_); + const std::string & node_name = event->node; + RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s", + node_name.c_str()); + + 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)) { + 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 (event_callback_) { + event_callback_(event); + } +} + +std::string +ParameterEventsSubscriber::resolve_path(const std::string & path) +{ + std::string full_path; + + if (path == "") { + full_path = node_base_->get_fully_qualified_name(); + } else { + full_path = path; + if (*full_path.begin() != '/') { + full_path = join_path(node_base_->get_namespace(), full_path); + } + } + + return full_path; +} + +std::pair +ParameterEventsSubscriber::split_path(const std::string & str) +{ + std::string path; + std::size_t found = str.find_last_of("/\\"); + if (found == 0) { + path = str.substr(0, found + 1); + } else { + path = str.substr(0, found); + } + std::string name = str.substr(found + 1); + return {path, name}; +} + +std::string +ParameterEventsSubscriber::join_path(std::string path, std::string name) +{ + std::string joined_path = path; + if (*joined_path.rbegin() != '/' && *name.begin() != '/') { + joined_path = joined_path + "/"; + } + + return joined_path + name; +} + +} // namespace nav2_util