Skip to content

Commit c8713ed

Browse files
bpwilcoxMichael Jeronimo
bpwilcox
and
Michael Jeronimo
authored
Add ParameterEventsSubscriber class (#829)
* add ParameterEventsSubscriber class and tests Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * test improvements, path name fixes, and more documentation Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * fix lint and uncrustify issues Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * add try-catch and warning around getting parameter value Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * pass rclcpp::Parameter object to callback, rename functions, get param from event Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * use unordered map with string pair, add test for different nodes same parameter, address feedback Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * address feedback (wjwwood) part 1 Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> use const string & for node name Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * add RCLCPP_PUBLIC macro Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * address feedback part 1: static get_parameter method, remove register_parameter_update, mutex for thread-safety Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * 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 <bpwilcox@eng.ucsd.edu> * use absolute parameter event topic for parameter event subscriber Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * support multiple parameter event callbacks Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * use get_child for parameter event subscriber logger Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * update utility function description Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * address feedback: move HandleCompare, test exceptions, reference code source Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * address feedback: replace ParameterEventsFilter dependency, fix copyright, add get_node_logging_interface, modify constructor Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu> * Get rid of a few compiler warnings; add test to build Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Remove some unneeded code Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Remove a stray debug trace Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Add a function to get all parameter values from a parameter event Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Address code review feedback Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Another name change; using Handler instead of the more passive term, Monitor Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Pass SharedPtrs callback remove functions instead of bare pointers Per William's review feedback. Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * Add a comment block describing usage Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> * 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 <michael.jeronimo@openrobotics.org> * A couple more doc fixes from review comments Signed-off-by: Michael Jeronimo <michael.jeronimo@openrobotics.org> Co-authored-by: Michael Jeronimo <michael.jeronimo@openrobotics.org>
1 parent 5a832e4 commit c8713ed

File tree

6 files changed

+1004
-2
lines changed

6 files changed

+1004
-2
lines changed

‎rclcpp/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ set(${PROJECT_NAME}_SRCS
8080
src/rclcpp/parameter.cpp
8181
src/rclcpp/parameter_value.cpp
8282
src/rclcpp/parameter_client.cpp
83+
src/rclcpp/parameter_event_handler.cpp
8384
src/rclcpp/parameter_events_filter.cpp
8485
src/rclcpp/parameter_map.cpp
8586
src/rclcpp/parameter_service.cpp

‎rclcpp/include/rclcpp/parameter_event_handler.hpp‎

Lines changed: 336 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,336 @@
1+
// Copyright 2019 Intel Corporation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
16+
#define RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
17+
18+
#include <list>
19+
#include <memory>
20+
#include <string>
21+
#include <unordered_map>
22+
#include <utility>
23+
#include <vector>
24+
25+
#include "rclcpp/create_subscription.hpp"
26+
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
27+
#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
28+
#include "rclcpp/node_interfaces/node_base_interface.hpp"
29+
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
30+
#include "rclcpp/parameter.hpp"
31+
#include "rclcpp/qos.hpp"
32+
#include "rclcpp/subscription.hpp"
33+
#include "rcl_interfaces/msg/parameter_event.hpp"
34+
35+
namespace rclcpp
36+
{
37+
38+
struct ParameterCallbackHandle
39+
{
40+
RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle)
41+
42+
using ParameterCallbackType = std::function<void (const rclcpp::Parameter &)>;
43+
44+
std::string parameter_name;
45+
std::string node_name;
46+
ParameterCallbackType callback;
47+
};
48+
49+
struct ParameterEventCallbackHandle
50+
{
51+
RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle)
52+
53+
using ParameterEventCallbackType =
54+
std::function<void (const rcl_interfaces::msg::ParameterEvent::SharedPtr &)>;
55+
56+
ParameterEventCallbackType callback;
57+
};
58+
59+
/// A class used to "handle" (monitor and respond to) changes to parameters.
60+
/**
61+
* The ParameterEventHandler class allows for the monitoring of changes to node parameters,
62+
* either a node's own parameters or parameters owned by other nodes in the system.
63+
* Multiple parameter callbacks can be set and will be invoked when the specified parameter
64+
* changes.
65+
*
66+
* The first step is to instantiate a ParameterEventHandler, providing a ROS node to use
67+
* to create any required subscriptions:
68+
*
69+
* auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
70+
*
71+
* Next, you can supply a callback to the add_parameter_callback method, as follows:
72+
*
73+
* auto cb1 = [&node](const rclcpp::Parameter & p) {
74+
* RCLCPP_INFO(
75+
* node->get_logger(),
76+
* "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
77+
* p.get_name().c_str(),
78+
* p.get_type_name().c_str(),
79+
* p.as_int());
80+
* };
81+
* auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
82+
*
83+
* In this case, we didn't supply a node name (the third, optional, parameter) so the
84+
* default will be to monitor for changes to the "an_int_param" parameter associated with
85+
* the ROS node supplied in the ParameterEventHandler constructor.
86+
* The callback, a lambda function in this case, simply prints out the value of the parameter.
87+
*
88+
* You may also monitor for changes to parameters in other nodes by supplying the node
89+
* name to add_parameter_callback:
90+
*
91+
* auto cb2 = [&node](const rclcpp::Parameter & p) {
92+
* RCLCPP_INFO(
93+
* node->get_logger(),
94+
* "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
95+
* p.get_name().c_str(),
96+
* p.get_type_name().c_str(),
97+
* p.as_string().c_str());
98+
* };
99+
* auto handle2 = param_handler->add_parameter_callback(
100+
* "some_remote_param_name", cb2, "some_remote_node_name");
101+
*
102+
* In this case, the callback will be invoked whenever "some_remote_param_name" changes
103+
* on remote node "some_remote_node_name".
104+
*
105+
* To remove a parameter callback, call remove_parameter_callback, passing the handle returned
106+
* from add_parameter_callback:
107+
*
108+
* param_handler->remove_parameter_callback(handle2);
109+
*
110+
* You can also monitor for *all* parameter changes, using add_parameter_event_callback.
111+
* In this case, the callback will be invoked whenever any parameter changes in the system.
112+
* You are likely interested in a subset of these parameter changes, so in the callback it
113+
* is convenient to use a regular expression on the node names or namespaces of interest.
114+
* For example:
115+
*
116+
* auto cb3 =
117+
* [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) {
118+
* // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
119+
* // to our own node ("this_node")
120+
* std::regex re("(/a_namespace/.*)|(/this_node)");
121+
* if (regex_match(event->node, re)) {
122+
* // Now that we know the event matches the regular expression we scanned for, we can
123+
* // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
124+
* rclcpp::Parameter p;
125+
* if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event(
126+
* *event, p, remote_param_name, fqn))
127+
* {
128+
* RCLCPP_INFO(
129+
* node->get_logger(),
130+
* "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
131+
* p.get_name().c_str(),
132+
* p.get_type_name().c_str(),
133+
* p.as_string().c_str());
134+
* }
135+
*
136+
* // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
137+
* // in on this event
138+
* auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(*event);
139+
* for (auto & p : params) {
140+
* RCLCPP_INFO(
141+
* node->get_logger(),
142+
* "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
143+
* p.get_name().c_str(),
144+
* p.get_type_name().c_str(),
145+
* p.value_to_string().c_str());
146+
* }
147+
* }
148+
* };
149+
* auto handle3 = param_handler->add_parameter_event_callback(cb3);
150+
*
151+
* For both parameter callbacks and parameter event callbacks, when multiple callbacks are added,
152+
* the callbacks are invoked last-in, first-called order (LIFO).
153+
*
154+
* To remove a parameter event callback, use:
155+
*
156+
* param_handler->remove_event_parameter_callback(handle);
157+
*/
158+
class ParameterEventHandler
159+
{
160+
public:
161+
/// Construct a parameter events monitor.
162+
/**
163+
* \param[in] node The node to use to create any required subscribers.
164+
* \param[in] qos The QoS settings to use for any subscriptions.
165+
*/
166+
template<typename NodeT>
167+
ParameterEventHandler(
168+
NodeT node,
169+
const rclcpp::QoS & qos =
170+
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
171+
{
172+
node_base_ = rclcpp::node_interfaces::get_node_base_interface(node);
173+
auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);
174+
175+
event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
176+
node_topics, "/parameter_events", qos,
177+
std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1));
178+
}
179+
180+
using ParameterEventCallbackType =
181+
ParameterEventCallbackHandle::ParameterEventCallbackType;
182+
183+
/// Set a callback for all parameter events.
184+
/**
185+
* This function may be called multiple times to set multiple parameter event callbacks.
186+
*
187+
* \param[in] callback Function callback to be invoked on parameter updates.
188+
* \returns A handle used to refer to the callback.
189+
*/
190+
RCLCPP_PUBLIC
191+
ParameterEventCallbackHandle::SharedPtr
192+
add_parameter_event_callback(
193+
ParameterEventCallbackType callback);
194+
195+
/// Remove parameter event callback registered with add_parameter_event_callback.
196+
/**
197+
* \param[in] callback_handle Handle of the callback to remove.
198+
*/
199+
RCLCPP_PUBLIC
200+
void
201+
remove_parameter_event_callback(
202+
ParameterEventCallbackHandle::SharedPtr callback_handle);
203+
204+
using ParameterCallbackType = ParameterCallbackHandle::ParameterCallbackType;
205+
206+
/// Add a callback for a specified parameter.
207+
/**
208+
* If a node_name is not provided, defaults to the current node.
209+
*
210+
* \param[in] parameter_name Name of parameter to monitor.
211+
* \param[in] callback Function callback to be invoked upon parameter update.
212+
* \param[in] node_name Name of node which hosts the parameter.
213+
* \returns A handle used to refer to the callback.
214+
*/
215+
RCLCPP_PUBLIC
216+
ParameterCallbackHandle::SharedPtr
217+
add_parameter_callback(
218+
const std::string & parameter_name,
219+
ParameterCallbackType callback,
220+
const std::string & node_name = "");
221+
222+
/// Remove a parameter callback registered with add_parameter_callback.
223+
/**
224+
* The parameter name and node name are inspected from the callback handle. The callback handle
225+
* is erased from the list of callback handles on the {parameter_name, node_name} in the map.
226+
* An error is thrown if the handle does not exist and/or was already removed.
227+
*
228+
* \param[in] callback_handle Handle of the callback to remove.
229+
*/
230+
RCLCPP_PUBLIC
231+
void
232+
remove_parameter_callback(
233+
ParameterCallbackHandle::SharedPtr callback_handle);
234+
235+
/// Get an rclcpp::Parameter from a parameter event.
236+
/**
237+
* If a node_name is not provided, defaults to the current node.
238+
*
239+
* \param[in] event Event msg to be inspected.
240+
* \param[out] parameter Reference to rclcpp::Parameter to be assigned.
241+
* \param[in] parameter_name Name of parameter.
242+
* \param[in] node_name Name of node which hosts the parameter.
243+
* \returns Output parameter is set with requested parameter info and returns true if
244+
* requested parameter name and node is in event. Otherwise, returns false.
245+
*/
246+
RCLCPP_PUBLIC
247+
static bool
248+
get_parameter_from_event(
249+
const rcl_interfaces::msg::ParameterEvent & event,
250+
rclcpp::Parameter & parameter,
251+
const std::string parameter_name,
252+
const std::string node_name = "");
253+
254+
/// Get an rclcpp::Parameter from parameter event
255+
/**
256+
* If a node_name is not provided, defaults to the current node.
257+
*
258+
* The user is responsible to check if the returned parameter has been properly assigned.
259+
* By default, if the requested parameter is not found in the event, the returned parameter
260+
* has parameter value of type rclcpp::PARAMETER_NOT_SET.
261+
*
262+
* \param[in] event Event msg to be inspected.
263+
* \param[in] parameter_name Name of parameter.
264+
* \param[in] node_name Name of node which hosts the parameter.
265+
* \returns The resultant rclcpp::Parameter from the event.
266+
*/
267+
RCLCPP_PUBLIC
268+
static rclcpp::Parameter
269+
get_parameter_from_event(
270+
const rcl_interfaces::msg::ParameterEvent & event,
271+
const std::string parameter_name,
272+
const std::string node_name = "");
273+
274+
/// Get all rclcpp::Parameter values from a parameter event
275+
/**
276+
* \param[in] event Event msg to be inspected.
277+
* \returns A vector rclcpp::Parameter values from the event.
278+
*/
279+
RCLCPP_PUBLIC
280+
static std::vector<rclcpp::Parameter>
281+
get_parameters_from_event(
282+
const rcl_interfaces::msg::ParameterEvent & event);
283+
284+
using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
285+
286+
protected:
287+
/// Callback for parameter events subscriptions.
288+
void
289+
event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
290+
291+
// Utility function for resolving node path.
292+
std::string resolve_path(const std::string & path);
293+
294+
// Node interface used for base functionality
295+
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
296+
297+
// *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
298+
// Hash function for string pair required in std::unordered_map
299+
// See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
300+
class StringPairHash
301+
{
302+
public:
303+
template<typename T>
304+
inline void hash_combine(std::size_t & seed, const T & v) const
305+
{
306+
std::hash<T> hasher;
307+
seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
308+
}
309+
310+
inline size_t operator()(const std::pair<std::string, std::string> & s) const
311+
{
312+
size_t seed = 0;
313+
hash_combine(seed, s.first);
314+
hash_combine(seed, s.second);
315+
return seed;
316+
}
317+
};
318+
// *INDENT-ON*
319+
320+
// Map container for registered parameters
321+
std::unordered_map<
322+
std::pair<std::string, std::string>,
323+
CallbacksContainerType,
324+
StringPairHash
325+
> parameter_callbacks_;
326+
327+
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
328+
329+
std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
330+
331+
std::recursive_mutex mutex_;
332+
};
333+
334+
} // namespace rclcpp
335+
336+
#endif // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_

‎rclcpp/include/rclcpp/rclcpp.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -147,14 +147,15 @@
147147
#include "rclcpp/guard_condition.hpp"
148148
#include "rclcpp/logging.hpp"
149149
#include "rclcpp/node.hpp"
150-
#include "rclcpp/parameter.hpp"
151150
#include "rclcpp/parameter_client.hpp"
151+
#include "rclcpp/parameter_event_handler.hpp"
152+
#include "rclcpp/parameter.hpp"
152153
#include "rclcpp/parameter_service.hpp"
153154
#include "rclcpp/rate.hpp"
154155
#include "rclcpp/time.hpp"
155156
#include "rclcpp/utilities.hpp"
156157
#include "rclcpp/visibility_control.hpp"
157-
#include "rclcpp/wait_set.hpp"
158158
#include "rclcpp/waitable.hpp"
159+
#include "rclcpp/wait_set.hpp"
159160

160161
#endif // RCLCPP__RCLCPP_HPP_

0 commit comments

Comments
 (0)