diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 561beb5d3e..d9471d27e1 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -79,6 +79,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter.cpp src/rclcpp/parameter_value.cpp src/rclcpp/parameter_client.cpp + src/rclcpp/parameter_event_handler.cpp src/rclcpp/parameter_events_filter.cpp src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp new file mode 100644 index 0000000000..f16c6bfe89 --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -0,0 +1,336 @@ +// 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 RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ +#define RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" + +namespace rclcpp +{ + +struct ParameterCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle) + + using ParameterCallbackType = std::function; + + std::string parameter_name; + std::string node_name; + ParameterCallbackType callback; +}; + +struct ParameterEventCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle) + + using ParameterEventCallbackType = + std::function; + + ParameterEventCallbackType callback; +}; + +/// A class used to "handle" (monitor and respond to) changes to parameters. +/** + * The ParameterEventHandler class allows for the monitoring of changes to node parameters, + * either a node's own parameters or parameters owned by other nodes in the system. + * Multiple parameter callbacks can be set and will be invoked when the specified parameter + * changes. + * + * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use + * to create any required subscriptions: + * + * auto param_handler = std::make_shared(node); + * + * Next, you can supply a callback to the add_parameter_callback method, as follows: + * + * auto cb1 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_int()); + * }; + * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1); + * + * In this case, we didn't supply a node name (the third, optional, parameter) so the + * default will be to monitor for changes to the "an_int_param" parameter associated with + * the ROS node supplied in the ParameterEventHandler constructor. + * The callback, a lambda function in this case, simply prints out the value of the parameter. + * + * You may also monitor for changes to parameters in other nodes by supplying the node + * name to add_parameter_callback: + * + * auto cb2 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * }; + * auto handle2 = param_handler->add_parameter_callback( + * "some_remote_param_name", cb2, "some_remote_node_name"); + * + * In this case, the callback will be invoked whenever "some_remote_param_name" changes + * on remote node "some_remote_node_name". + * + * To remove a parameter callback, call remove_parameter_callback, passing the handle returned + * from add_parameter_callback: + * + * param_handler->remove_parameter_callback(handle2); + * + * You can also monitor for *all* parameter changes, using add_parameter_event_callback. + * In this case, the callback will be invoked whenever any parameter changes in the system. + * You are likely interested in a subset of these parameter changes, so in the callback it + * is convenient to use a regular expression on the node names or namespaces of interest. + * For example: + * + * auto cb3 = + * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) { + * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes + * // to our own node ("this_node") + * std::regex re("(/a_namespace/.*)|(/this_node)"); + * if (regex_match(event->node, re)) { + * // Now that we know the event matches the regular expression we scanned for, we can + * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for + * rclcpp::Parameter p; + * if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event( + * *event, p, remote_param_name, fqn)) + * { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * } + * + * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came + * // in on this event + * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(*event); + * for (auto & p : params) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.value_to_string().c_str()); + * } + * } + * }; + * auto handle3 = param_handler->add_parameter_event_callback(cb3); + * + * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added, + * the callbacks are invoked last-in, first-called order (LIFO). + * + * To remove a parameter event callback, use: + * + * param_handler->remove_event_parameter_callback(handle); + */ +class ParameterEventHandler +{ +public: + /// Construct a parameter events monitor. + /** + * \param[in] node The node to use to create any required subscribers. + * \param[in] qos The QoS settings to use for any subscriptions. + */ + template + ParameterEventHandler( + NodeT node, + const rclcpp::QoS & qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) + { + node_base_ = rclcpp::node_interfaces::get_node_base_interface(node); + auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); + + event_subscription_ = rclcpp::create_subscription( + node_topics, "/parameter_events", qos, + std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1)); + } + + using ParameterEventCallbackType = + ParameterEventCallbackHandle::ParameterEventCallbackType; + + /// Set a callback for all parameter events. + /** + * This function may be called multiple times to set multiple parameter event callbacks. + * + * \param[in] callback Function callback to be invoked on parameter updates. + * \returns A handle used to refer to the callback. + */ + RCLCPP_PUBLIC + ParameterEventCallbackHandle::SharedPtr + add_parameter_event_callback( + ParameterEventCallbackType callback); + + /// Remove parameter event callback registered with add_parameter_event_callback. + /** + * \param[in] callback_handle Handle of the callback to remove. + */ + RCLCPP_PUBLIC + void + remove_parameter_event_callback( + ParameterEventCallbackHandle::SharedPtr callback_handle); + + using ParameterCallbackType = ParameterCallbackHandle::ParameterCallbackType; + + /// Add a callback for a specified parameter. + /** + * If a node_name is not provided, defaults to the current node. + * + * \param[in] parameter_name Name of parameter to monitor. + * \param[in] callback Function callback to be invoked upon parameter update. + * \param[in] node_name Name of node which hosts the parameter. + * \returns A handle used to refer to the callback. + */ + RCLCPP_PUBLIC + ParameterCallbackHandle::SharedPtr + add_parameter_callback( + const std::string & parameter_name, + ParameterCallbackType callback, + const std::string & node_name = ""); + + /// Remove a parameter callback registered with add_parameter_callback. + /** + * 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] callback_handle Handle of the callback to remove. + */ + RCLCPP_PUBLIC + void + remove_parameter_callback( + ParameterCallbackHandle::SharedPtr callback_handle); + + /// Get an rclcpp::Parameter from a parameter 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 Output parameter is set with requested parameter info and returns true if + * requested parameter name and node is in event. Otherwise, returns false. + */ + RCLCPP_PUBLIC + static bool + get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + rclcpp::Parameter & parameter, + const std::string parameter_name, + const std::string node_name = ""); + + /// Get an 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( + const rcl_interfaces::msg::ParameterEvent & event, + const std::string parameter_name, + const std::string node_name = ""); + + /// Get all rclcpp::Parameter values from a parameter event + /** + * \param[in] event Event msg to be inspected. + * \returns A vector rclcpp::Parameter values from the event. + */ + RCLCPP_PUBLIC + static std::vector + get_parameters_from_event( + const rcl_interfaces::msg::ParameterEvent & event); + + using CallbacksContainerType = std::list; + +protected: + /// Callback for parameter events subscriptions. + void + event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + + // Utility function for resolving node path. + std::string resolve_path(const std::string & path); + + // Node interface used for base functionality + std::shared_ptr node_base_; + + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels + // Hash function for string pair required in std::unordered_map + // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values + 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_; + + rclcpp::Subscription::SharedPtr event_subscription_; + + std::list event_callbacks_; + + std::recursive_mutex mutex_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index dc22c7e649..56e999f702 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -147,14 +147,15 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_event_handler.hpp" +#include "rclcpp/parameter.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/rate.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp/waitable.hpp" +#include "rclcpp/wait_set.hpp" #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp new file mode 100644 index 0000000000..9fd5994864 --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -0,0 +1,222 @@ +// 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 "rclcpp/parameter_event_handler.hpp" +#include "rcpputils/join.hpp" + +namespace rclcpp +{ + +ParameterEventCallbackHandle::SharedPtr +ParameterEventHandler::add_parameter_event_callback( + ParameterEventCallbackType callback) +{ + std::lock_guard lock(mutex_); + auto handle = std::make_shared(); + handle->callback = callback; + event_callbacks_.emplace_front(handle); + + return handle; +} + +template +struct HandleCompare + : public std::unary_function +{ + explicit HandleCompare(const CallbackHandleT * const base) + : base_(base) {} + bool operator()(const typename CallbackHandleT::WeakPtr & handle) + { + auto shared_handle = handle.lock(); + if (base_ == shared_handle.get()) { + return true; + } + return false; + } + const CallbackHandleT * const base_; +}; + +void +ParameterEventHandler::remove_parameter_event_callback( + ParameterEventCallbackHandle::SharedPtr callback_handle) +{ + std::lock_guard lock(mutex_); + auto it = std::find_if( + event_callbacks_.begin(), + event_callbacks_.end(), + HandleCompare(callback_handle.get())); + if (it != event_callbacks_.end()) { + event_callbacks_.erase(it); + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +ParameterCallbackHandle::SharedPtr +ParameterEventHandler::add_parameter_callback( + const std::string & parameter_name, + ParameterCallbackType callback, + const std::string & node_name) +{ + std::lock_guard lock(mutex_); + auto full_node_name = resolve_path(node_name); + + 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; +} + +void +ParameterEventHandler::remove_parameter_callback( + ParameterCallbackHandle::SharedPtr callback_handle) +{ + std::lock_guard lock(mutex_); + auto handle = callback_handle.get(); + 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}); + } + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +bool +ParameterEventHandler::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + rclcpp::Parameter & parameter, + const std::string parameter_name, + const std::string node_name) +{ + if (event.node != node_name) { + return false; + } + + for (auto & new_parameter : event.new_parameters) { + if (new_parameter.name == parameter_name) { + parameter = rclcpp::Parameter::from_parameter_msg(new_parameter); + return true; + } + } + + for (auto & changed_parameter : event.changed_parameters) { + if (changed_parameter.name == parameter_name) { + parameter = rclcpp::Parameter::from_parameter_msg(changed_parameter); + return true; + } + } + + return false; +} + +rclcpp::Parameter +ParameterEventHandler::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + const std::string parameter_name, + const std::string node_name) +{ + rclcpp::Parameter p; + if (!get_parameter_from_event(event, p, parameter_name, node_name)) { + throw std::runtime_error( + "Parameter '" + parameter_name + "' of node '" + node_name + + "' is not part of parameter event"); + } + return p; +} + +std::vector +ParameterEventHandler::get_parameters_from_event( + const rcl_interfaces::msg::ParameterEvent & event) +{ + std::vector params; + + for (auto & new_parameter : event.new_parameters) { + params.push_back(rclcpp::Parameter::from_parameter_msg(new_parameter)); + } + + for (auto & changed_parameter : event.changed_parameters) { + params.push_back(rclcpp::Parameter::from_parameter_msg(changed_parameter)); + } + + return params; +} + +void +ParameterEventHandler::event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + std::lock_guard lock(mutex_); + + 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); + } + } + } + } + + for (auto event_cb = event_callbacks_.begin(); event_cb != event_callbacks_.end(); ++event_cb) { + auto shared_event_handle = event_cb->lock(); + if (nullptr != shared_event_handle) { + shared_event_handle->callback(event); + } else { + event_cb = event_callbacks_.erase(event_cb); + } + } +} + +std::string +ParameterEventHandler::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 (*path.begin() != '/') { + auto ns = node_base_->get_namespace(); + const std::vector paths{ns, path}; + full_path = (ns == std::string("/")) ? ns + path : rcpputils::join(paths, "/"); + } + } + + return full_path; +} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index a1005836a0..428ef9bcc2 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -323,6 +323,16 @@ if(TARGET test_parameter) ) target_link_libraries(test_parameter ${PROJECT_NAME}) endif() +ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) +if(TARGET test_parameter_event_handler) + ament_target_dependencies(test_parameter_event_handler + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) +endif() ament_add_gtest(test_parameter_map test_parameter_map.cpp) if(TARGET test_parameter_map) target_link_libraries(test_parameter_map ${PROJECT_NAME}) diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp new file mode 100644 index 0000000000..5f4ed6fc83 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -0,0 +1,432 @@ +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +class TestParameterEventHandler : public rclcpp::ParameterEventHandler +{ +public: + explicit TestParameterEventHandler(rclcpp::Node::SharedPtr node) + : ParameterEventHandler(node) + {} + + void test_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + { + event_callback(event); + } + + size_t num_event_callbacks() + { + return event_callbacks_.size(); + } + + size_t num_parameter_callbacks() + { + return parameter_callbacks_.size(); + } +}; + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + rclcpp::NodeOptions options; + node = std::make_shared( + "test_parameter_events_subscriber", options); + + remote_node_name = "/remote_node"; + diff_ns_name = "/ns/remote_node"; + + param_handler = std::make_shared(node); + + same_node_int = std::make_shared(); + same_node_double = std::make_shared(); + multiple = std::make_shared(); + remote_node_string = std::make_shared(); + diff_ns_bool = std::make_shared(); + diff_node_int = std::make_shared(); + + same_node_int->node = node->get_fully_qualified_name(); + same_node_double->node = node->get_fully_qualified_name(); + multiple->node = node->get_fully_qualified_name(); + remote_node_string->node = remote_node_name; + diff_ns_bool->node = diff_ns_name; + diff_node_int->node = remote_node_name; + + rcl_interfaces::msg::Parameter p; + p.name = "my_int"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + p.value.integer_value = 1; + same_node_int->changed_parameters.push_back(p); + diff_node_int->changed_parameters.push_back(p); + multiple->changed_parameters.push_back(p); + + p.name = "my_double"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + p.value.double_value = 1.0; + same_node_double->changed_parameters.push_back(p); + multiple->changed_parameters.push_back(p); + + p.name = "my_string"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + p.value.string_value = "test"; + remote_node_string->changed_parameters.push_back(p); + + p.name = "my_bool"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + p.value.bool_value = true; + diff_ns_bool->changed_parameters.push_back(p); + } + + void TearDown() + { + node.reset(); + param_handler.reset(); + } + + rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_int; + rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_double; + rcl_interfaces::msg::ParameterEvent::SharedPtr diff_node_int; + rcl_interfaces::msg::ParameterEvent::SharedPtr remote_node_string; + rcl_interfaces::msg::ParameterEvent::SharedPtr multiple; + rcl_interfaces::msg::ParameterEvent::SharedPtr diff_ns_bool; + + rclcpp::Node::SharedPtr node; + std::string remote_node_name; + std::string diff_ns_name; + std::shared_ptr param_handler; +}; + +TEST_F(TestNode, RegisterParameterCallback) +{ + bool received; + auto cb = [&received](const rclcpp::Parameter &) {received = true;}; + + auto h1 = param_handler->add_parameter_callback("my_double", cb); + auto h2 = param_handler->add_parameter_callback("my_int", cb); + auto h3 = param_handler->add_parameter_callback("my_string", cb, remote_node_name); + auto h4 = param_handler->add_parameter_callback("my_bool", cb, diff_ns_name); + + received = false; + param_handler->test_event(same_node_double); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(same_node_int); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(remote_node_string); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(received, true); +} + +TEST_F(TestNode, SameParameterDifferentNode) +{ + int64_t int_param_node1; + int64_t int_param_node2; + + auto cb1 = [&int_param_node1](const rclcpp::Parameter & p) { + int_param_node1 = p.get_value(); + }; + auto cb2 = [&int_param_node2](const rclcpp::Parameter & p) { + int_param_node2 = p.get_value(); + }; + + // Set individual parameters + auto h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2, remote_node_name); + + param_handler->test_event(same_node_int); + EXPECT_EQ(int_param_node1, 1); + EXPECT_NE(int_param_node2, 1); + + int_param_node1 = 0; + int_param_node2 = 0; + + param_handler->test_event(diff_node_int); + EXPECT_NE(int_param_node1, 1); + EXPECT_EQ(int_param_node2, 1); + + param_handler->remove_parameter_callback(h1); + param_handler->remove_parameter_callback(h2); + EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL); +} + +TEST_F(TestNode, GetParameterFromEvent) +{ + using rclcpp::ParameterEventHandler; + std::string node_name = node->get_fully_qualified_name(); + std::string wrong_name = "/wrong_node_name"; + + rclcpp::Parameter p; + EXPECT_TRUE( + ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", node_name)); + EXPECT_EQ(p.get_value(), 1); + // False if parameter not with correct node name + EXPECT_FALSE( + ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", wrong_name)); + // False if parameter not part of event + EXPECT_FALSE( + ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, p, "my_int", node_name)); + + EXPECT_NO_THROW( + ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", node_name)); + // Throws if parameter not with correct node name + EXPECT_THROW( + ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name), + std::runtime_error); + // Throws if parameter not part of event + EXPECT_THROW( + ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, "my_int", node_name), + std::runtime_error); +} + +TEST_F(TestNode, GetParametersFromEvent) +{ + using rclcpp::ParameterEventHandler; + std::string node_name = node->get_fully_qualified_name(); + + auto params = ParameterEventHandler::get_parameters_from_event(*multiple); + EXPECT_EQ(params.size(), 2u); + bool found_int = false; + bool found_double = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_int")) { + found_int = true; + EXPECT_EQ(p.get_value(), 1); + } else if (p.get_name() == std::string("my_double")) { + found_double = true; + EXPECT_EQ(p.get_value(), 1.0); + } + } + EXPECT_EQ(found_int, true); + EXPECT_EQ(found_double, true); + + params = ParameterEventHandler::get_parameters_from_event(*remote_node_string); + EXPECT_EQ(params.size(), 1u); + bool found_string = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_string")) { + found_string = true; + EXPECT_EQ(p.get_value(), std::string("test")); + } + } + EXPECT_EQ(found_string, true); + + params = ParameterEventHandler::get_parameters_from_event(*diff_ns_bool); + EXPECT_EQ(params.size(), 1u); + bool found_bool = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_bool")) { + found_bool = true; + EXPECT_EQ(p.get_value(), true); + } + } + EXPECT_EQ(found_bool, true); +} + +TEST_F(TestNode, EventCallback) +{ + using rclcpp::ParameterEventHandler; + + double double_param = 0.0; + int64_t int_param = 0; + bool bool_param{false}; + bool received{false}; + + double product; + auto cb = + [&int_param, &double_param, &product, &bool_param, &received, + this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + { + auto node_name = node->get_fully_qualified_name(); + + if (event->node == node_name) { + received = true; + } + + rclcpp::Parameter p; + if (ParameterEventHandler::get_parameter_from_event(*event, p, "my_int", node_name)) { + int_param = p.get_value(); + } + try { + p = ParameterEventHandler::get_parameter_from_event(*event, "my_double", node_name); + double_param = p.get_value(); + } catch (...) { + } + + product = static_cast(int_param) * double_param; + }; + + auto cb2 = + [&bool_param, this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + { + rclcpp::Parameter p; + if (event->node == diff_ns_name) { + if (ParameterEventHandler::get_parameter_from_event( + *event, p, "my_bool", diff_ns_name)) + { + bool_param = p.get_value(); + } + } + }; + + auto event_handle1 = param_handler->add_parameter_event_callback(cb); + auto event_handle2 = param_handler->add_parameter_event_callback(cb2); + + bool_param = false; + param_handler->test_event(multiple); + EXPECT_EQ(received, true); + EXPECT_EQ(product, 1.0); + EXPECT_EQ(bool_param, false); + + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(bool_param, true); + + // Test removal of event callback + received = false; + bool_param = false; + param_handler->remove_parameter_event_callback(event_handle1); + param_handler->test_event(multiple); + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(received, false); + EXPECT_EQ(bool_param, true); + + // Should throw if callback handle no longer exists or already removed + EXPECT_THROW( + param_handler->remove_parameter_event_callback(event_handle1), std::runtime_error); +} + +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 h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2); + auto h3 = param_handler->add_parameter_callback("my_double", cb3); + + // Test multiple callbacks per parameter + param_handler->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; + param_handler->remove_parameter_callback(h1); + param_handler->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; + param_handler->remove_parameter_callback(h2); + param_handler->test_event(same_node_int); + EXPECT_EQ(received_2, false); + + // Should throw if callback handle no longer exists or already removed + EXPECT_THROW(param_handler->remove_parameter_callback(h1), std::runtime_error); + EXPECT_THROW(param_handler->remove_parameter_callback(h2), std::runtime_error); + + param_handler->remove_parameter_callback(h3); + + // All callbacks should have been removed + EXPECT_EQ(received_2, 0); + EXPECT_EQ(param_handler->num_event_callbacks(), 0UL); +} + +TEST_F(TestNode, LastInFirstCallForParameterCallbacks) +{ + rclcpp::Time time_1; + rclcpp::Time time_2; + + // The callbacks will log the current time for comparison purposes. Add a bit of a stall + // to ensure that the time noted in the back-to-back calls isn't the same + auto cb1 = [this, &time_1](const rclcpp::Parameter &) { + time_1 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + auto cb2 = [this, &time_2](const rclcpp::Parameter &) { + time_2 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + + auto h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2); + + // Test multiple callbacks per parameter + param_handler->test_event(same_node_int); + + // The most-recently install handler should be called first + EXPECT_EQ(time_2 < time_1, true); + + param_handler->remove_parameter_callback(h1); + param_handler->remove_parameter_callback(h2); + EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL); +} + +TEST_F(TestNode, LastInFirstCallForParameterEventCallbacks) +{ + rclcpp::Time time_1; + rclcpp::Time time_2; + + // The callbacks will log the current time for comparison purposes. Add a bit of a stall + // to ensure that the time noted in the back-to-back calls isn't the same + auto cb1 = + [this, &time_1](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + { + time_1 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + auto cb2 = + [this, &time_2](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + { + time_2 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + + auto h1 = param_handler->add_parameter_event_callback(cb1); + auto h2 = param_handler->add_parameter_event_callback(cb2); + + // Test multiple callbacks per parameter + param_handler->test_event(same_node_int); + + // The most-recently install handler should be called first + EXPECT_EQ(time_2 < time_1, true); + + param_handler->remove_parameter_event_callback(h1); + param_handler->remove_parameter_event_callback(h2); + EXPECT_EQ(param_handler->num_event_callbacks(), 0UL); +}