-
Notifications
You must be signed in to change notification settings - Fork 522
Expand file tree
/
Copy pathparameter_event_handler.hpp
More file actions
404 lines (368 loc) · 15 KB
/
parameter_event_handler.hpp
File metadata and controls
404 lines (368 loc) · 15 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
// 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 <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#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 "rclcpp/visibility_control.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
namespace rclcpp
{
struct ParameterCallbackHandle
{
RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle)
using ParameterCallbackType = std::function<void (const rclcpp::Parameter &)>;
std::string parameter_name;
std::string node_name;
ParameterCallbackType callback;
};
struct ParameterEventCallbackHandle
{
RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle)
using ParameterEventCallbackType =
std::function<void (const rcl_interfaces::msg::ParameterEvent &)>;
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:
*
* ```cpp
* auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
* ```
*
* Next, you can supply a callback to the add_parameter_callback method, as follows:
*
* ```cpp
* 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.
*
* Note: the object returned from add_parameter_callback must be captured or the callback will
* be immediately unregistered.
*
* You may also monitor for changes to parameters in other nodes by supplying the node
* name to add_parameter_callback:
*
* ```cpp
* 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, reset the callback handle smart pointer or call
* remove_parameter_callback, passing the handle returned from add_parameter_callback:
*
* ```cpp
* 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:
*
* ```cpp
* auto cb3 =
* [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & 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::ParameterEventHandler::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::ParameterEventHandler::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).
*
* Note: the callback handle returned from add_parameter_event_callback must be captured or
* the callback will immediately be unregistered.
*
* To remove a parameter event callback, reset the callback smart pointer or use:
*
* ```cpp
* param_handler->remove_event_parameter_callback(handle3);
* ```
*/
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<typename NodeT>
explicit ParameterEventHandler(
const 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);
callbacks_ = std::make_shared<Callbacks>();
event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
node_topics, "/parameter_events", qos,
[callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
callbacks->event_callback(event);
});
}
using ParameterEventCallbackType =
ParameterEventCallbackHandle::ParameterEventCallbackType;
/// Set a callback for all parameter events.
/**
* This function may be called multiple times to set multiple parameter event callbacks.
*
* Note: if the returned callback handle smart pointer is not captured, the callback is
* immediatedly unregistered. A compiler warning should be generated to warn of this.
*
* \param[in] callback Function callback to be invoked on parameter updates.
* \returns A handle used to refer to the callback.
*/
RCLCPP_PUBLIC
RCUTILS_WARN_UNUSED
ParameterEventCallbackHandle::SharedPtr
add_parameter_event_callback(
const 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(
const 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.
*
* The configure_nodes_filter() function will affect the behavior of this function.
* If the node specified in this function isn't included in the nodes specified in
* configure_nodes_filter(), the callback will never be called.
*
* Note: if the returned callback handle smart pointer is not captured, the callback
* is immediately unregistered. A compiler warning should be generated to warn
* of this.
*
* \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
RCUTILS_WARN_UNUSED
ParameterCallbackHandle::SharedPtr
add_parameter_callback(
const std::string & parameter_name,
const ParameterCallbackType & callback,
const std::string & node_name = "");
/// Configure which node parameter events will be received.
/**
* This function depends on rmw implementation support for content filtering.
* If middleware doesn't support contentfilter, return false.
*
* If node_names is empty, the configured node filter will be cleared.
*
* If this function return true, only parameter events from the specified node will be received.
* It affects the behavior of the following two functions.
* - add_parameter_event_callback()
* The callback will only be called for parameter events from the specified nodes which are
* configured in this function.
* - add_parameter_callback()
* The callback will only be called for parameter events from the specified nodes which are
* configured in this function and add_parameter_callback().
* If the nodes specified in this function is different from the nodes specified in
* add_parameter_callback(), the callback will never be called.
*
* \param[in] node_names Node names to filter parameter events from.
* \returns true if configuring was successfully applied, false otherwise.
* \throws rclcpp::exceptions::RCLError if internal error occurred when calling the rcl function.
*/
RCLCPP_PUBLIC
bool configure_nodes_filter(const std::vector<std::string> & node_names);
/// 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(
const 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.
* \throws std::runtime_error if input node name doesn't match the node name in parameter 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<rclcpp::Parameter>
get_parameters_from_event(
const rcl_interfaces::msg::ParameterEvent & event);
using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
protected:
// *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<typename T>
inline void hash_combine(std::size_t & seed, const T & v) const
{
std::hash<T> hasher;
seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
inline size_t operator()(const std::pair<std::string, std::string> & s) const
{
size_t seed = 0;
hash_combine(seed, s.first);
hash_combine(seed, s.second);
return seed;
}
};
// *INDENT-ON*
struct Callbacks
{
std::recursive_mutex mutex_;
// Map container for registered parameters
std::unordered_map<
std::pair<std::string, std::string>,
CallbacksContainerType,
StringPairHash
> parameter_callbacks_;
std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
/// Callback for parameter events subscriptions.
RCLCPP_PUBLIC
void
event_callback(const rcl_interfaces::msg::ParameterEvent & event);
};
std::shared_ptr<Callbacks> callbacks_;
// Utility function for resolving node path.
std::string resolve_path(const std::string & path);
// Node interface used for base functionality
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
};
} // namespace rclcpp
#endif // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_