From 18e72cfb3f8c80ec8a300c6632e4e5bb55771abc Mon Sep 17 00:00:00 2001 From: Michael Jeronimo Date: Thu, 11 Feb 2021 12:31:11 -0800 Subject: [PATCH 1/2] Add a comment block describing usage Signed-off-by: Michael Jeronimo --- .../rclcpp/parameter_event_handler.hpp | 90 +++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 134cac2617..58dc966ae7 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -58,6 +58,96 @@ struct ParameterEventCallbackHandle 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 instance 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); + * + * To remove a parameter event callback, use + * + * param_handler->remove_event_parameter_callback(handle); + */ class ParameterEventHandler { public: From e08612c74dcf2697854cfcd9a261b8f70c617f85 Mon Sep 17 00:00:00 2001 From: Michael Jeronimo Date: Thu, 11 Feb 2021 13:36:48 -0800 Subject: [PATCH 2/2] Address review feedback * Remove unused interfaces * Document LIFO order for invoking callbacks * Add test cases to verify LIFO order for callbacks Signed-off-by: Michael Jeronimo --- .../rclcpp/parameter_event_handler.hpp | 9 +-- .../rclcpp/test_parameter_event_handler.cpp | 79 ++++++++++++++++++- 2 files changed, 81 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 58dc966ae7..4f3d570fb9 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -24,10 +24,8 @@ #include "rclcpp/create_subscription.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" -#include "rclcpp/node_interfaces/get_node_logging_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" @@ -144,6 +142,9 @@ struct ParameterEventCallbackHandle * }; * 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); @@ -163,7 +164,6 @@ class ParameterEventHandler rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) { node_base_ = rclcpp::node_interfaces::get_node_base_interface(node); - node_logging_ = rclcpp::node_interfaces::get_node_logging_interface(node); auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); event_subscription_ = rclcpp::create_subscription( @@ -285,9 +285,8 @@ class ParameterEventHandler // Utility function for resolving node path. std::string resolve_path(const std::string & path); - // Node Interfaces used for base and logging. + // Node interface used for base functionality std::shared_ptr node_base_; - std::shared_ptr node_logging_; // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels // Hash function for string pair required in std::unordered_map diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index f93f43d418..5f4ed6fc83 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -12,8 +12,10 @@ // 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" @@ -30,10 +32,15 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler event_callback(event); } - size_t num_installed_handlers() + size_t num_event_callbacks() { return event_callbacks_.size(); } + + size_t num_parameter_callbacks() + { + return parameter_callbacks_.size(); + } }; class TestNode : public ::testing::Test @@ -166,6 +173,10 @@ TEST_F(TestNode, SameParameterDifferentNode) 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) @@ -353,5 +364,69 @@ TEST_F(TestNode, MultipleParameterCallbacks) // All callbacks should have been removed EXPECT_EQ(received_2, 0); - EXPECT_EQ(param_handler->num_installed_handlers(), 0UL); + 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); }