node_impl.hpp
1
// Copyright 2014 Open Source Robotics Foundation, Inc.
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__NODE_IMPL_HPP_
16
#define RCLCPP__NODE_IMPL_HPP_
17
18
#include <rmw/error_handling.h>
19
#include <rmw/rmw.h>
20
21
#include <algorithm>
22
#include <chrono>
23
#include <cstdlib>
24
#include <iostream>
25
#include <limits>
26
#include <map>
27
#include <memory>
28
#include <sstream>
29
#include <stdexcept>
30
#include <string>
31
#include <utility>
32
#include <vector>
33
34
#include "rcl/publisher.h"
35
#include "rcl/subscription.h"
36
37
#include "rclcpp/contexts/default_context.hpp"
38
#include "rclcpp/create_client.hpp"
39
#include "rclcpp/create_generic_publisher.hpp"
40
#include "rclcpp/create_generic_subscription.hpp"
41
#include "rclcpp/create_publisher.hpp"
42
#include "rclcpp/create_service.hpp"
43
#include "rclcpp/create_generic_service.hpp"
44
#include "rclcpp/create_subscription.hpp"
45
#include "rclcpp/create_timer.hpp"
46
#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
47
#include "rclcpp/parameter.hpp"
48
#include "rclcpp/qos.hpp"
49
#include "rclcpp/timer.hpp"
50
#include "rclcpp/type_support_decl.hpp"
51
#include "rclcpp/visibility_control.hpp"
52
53
#ifndef RCLCPP__NODE_HPP_
54
#include "node.hpp"
55
#endif
56
57
namespace rclcpp
58
{
59
60
RCLCPP_LOCAL
61
inline
62
std::string
63
extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
64
{
65
std::string name_with_sub_namespace(name);
66
if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
67
name_with_sub_namespace = sub_namespace + "/" + name;
68
}
69
return name_with_sub_namespace;
70
}
71
72
template<typename MessageT, typename AllocatorT, typename PublisherT>
73
std::shared_ptr<PublisherT>
74
Node::create_publisher(
75
const std::string & topic_name,
76
const rclcpp::QoS & qos,
77
const PublisherOptionsWithAllocator<AllocatorT> & options)
79
return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
81
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
82
qos,
83
options);
86
template<
87
typename MessageT,
88
typename CallbackT,
89
typename AllocatorT,
90
typename SubscriptionT,
91
typename MessageMemoryStrategyT>
92
std::shared_ptr<SubscriptionT>
93
Node::create_subscription(
94
const std::string & topic_name,
95
const rclcpp::QoS & qos,
96
CallbackT && callback,
97
const SubscriptionOptionsWithAllocator<AllocatorT> & options,
98
typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
100
return rclcpp::create_subscription<MessageT>(
101
*this,
102
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
104
std::forward<CallbackT>(callback),
105
options,
106
msg_mem_strat);
109
template<typename DurationRepT, typename DurationT, typename CallbackT>
110
typename rclcpp::WallTimer<CallbackT>::SharedPtr
111
Node::create_wall_timer(
112
std::chrono::duration<DurationRepT, DurationT> period,
113
CallbackT callback,
114
rclcpp::CallbackGroup::SharedPtr group,
115
bool autostart)
117
return rclcpp::create_wall_timer(
118
period,
119
std::move(callback),
120
group,
121
this->node_base_.get(),
122
this->node_timers_.get(),
123
autostart);
126
template<typename DurationRepT, typename DurationT, typename CallbackT>
127
typename rclcpp::GenericTimer<CallbackT>::SharedPtr
128
Node::create_timer(
129
std::chrono::duration<DurationRepT, DurationT> period,
130
CallbackT callback,
131
rclcpp::CallbackGroup::SharedPtr group)
132
{
133
return rclcpp::create_timer(
134
this->get_clock(),
135
period,
136
std::move(callback),
137
group,
138
this->node_base_.get(),
139
this->node_timers_.get());
140
}
141
142
template<typename ServiceT>
143
typename Client<ServiceT>::SharedPtr
144
Node::create_client(
145
const std::string & service_name,
146
const rclcpp::QoS & qos,
147
rclcpp::CallbackGroup::SharedPtr group)
148
{
149
return rclcpp::create_client<ServiceT>(
150
node_base_,
151
node_graph_,
152
node_services_,
153
extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
154
qos,
155
group);
156
}
157
158
template<typename ServiceT, typename CallbackT>
159
typename rclcpp::Service<ServiceT>::SharedPtr
160
Node::create_service(
161
const std::string & service_name,
162
CallbackT && callback,
163
const rclcpp::QoS & qos,
164
rclcpp::CallbackGroup::SharedPtr group)
165
{
166
return rclcpp::create_service<ServiceT, CallbackT>(
167
node_base_,
168
node_services_,
169
extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
170
std::forward<CallbackT>(callback),
171
qos,
172
group);
173
}
174
175
template<typename CallbackT>
176
typename rclcpp::GenericService::SharedPtr
177
Node::create_generic_service(
178
const std::string & service_name,
179
const std::string & service_type,
180
CallbackT && callback,
181
const rclcpp::QoS & qos,
182
rclcpp::CallbackGroup::SharedPtr group)
183
{
184
return rclcpp::create_generic_service<CallbackT>(
185
node_base_,
186
node_services_,
187
extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
188
service_type,
189
std::forward<CallbackT>(callback),
190
qos,
191
group);
192
}
193
194
template<typename AllocatorT>
195
std::shared_ptr<rclcpp::GenericPublisher>
196
Node::create_generic_publisher(
197
const std::string & topic_name,
198
const std::string & topic_type,
199
const rclcpp::QoS & qos,
200
const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
201
{
202
return rclcpp::create_generic_publisher(
203
node_topics_,
204
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
205
topic_type,
206
qos,
207
options
208
);
209
}
210
211
template<typename CallbackT, typename AllocatorT>
212
std::shared_ptr<rclcpp::GenericSubscription>
213
Node::create_generic_subscription(
214
const std::string & topic_name,
215
const std::string & topic_type,
216
const rclcpp::QoS & qos,
217
CallbackT && callback,
218
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
219
{
220
return rclcpp::create_generic_subscription(
221
node_topics_,
222
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
223
topic_type,
224
qos,
225
std::forward<CallbackT>(callback),
226
options
227
);
228
}
229
230
231
template<typename ParameterT>
232
auto
233
Node::declare_parameter(
234
const std::string & name,
235
const ParameterT & default_value,
236
const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
237
bool ignore_override)
239
try {
240
return this->declare_parameter(
241
name,
242
rclcpp::ParameterValue(default_value),
243
parameter_descriptor,
244
ignore_override
245
).get<ParameterT>();
246
} catch (const ParameterTypeException & ex) {
247
throw exceptions::InvalidParameterTypeException(name, ex.what());
248
}
249
}
250
251
template<typename ParameterT>
252
auto
253
Node::declare_parameter(
254
const std::string & name,
255
const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
256
bool ignore_override)
257
{
258
// get advantage of parameter value template magic to get
259
// the correct rclcpp::ParameterType from ParameterT
260
rclcpp::ParameterValue value{ParameterT{}};
261
try {
262
return this->declare_parameter(
263
name,
264
value.get_type(),
265
parameter_descriptor,
266
ignore_override
267
).get<ParameterT>();
268
} catch (const ParameterTypeException &) {
269
throw exceptions::UninitializedStaticallyTypedParameterException(name);
270
}
273
template<typename ParameterT>
274
std::vector<ParameterT>
275
Node::declare_parameters(
276
const std::string & namespace_,
277
const std::map<std::string, ParameterT> & parameters,
278
bool ignore_overrides)
279
{
280
std::vector<ParameterT> result;
281
std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
282
std::transform(
283
parameters.begin(), parameters.end(), std::back_inserter(result),
284
[this, &normalized_namespace, ignore_overrides](auto element) {
285
return this->declare_parameter(
286
normalized_namespace + element.first,
287
element.second,
288
rcl_interfaces::msg::ParameterDescriptor(),
289
ignore_overrides);
290
}
291
);
292
return result;
293
}
294
295
template<typename ParameterT>
296
std::vector<ParameterT>
297
Node::declare_parameters(
298
const std::string & namespace_,
299
const std::map<
300
std::string,
301
std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
302
> & parameters,
303
bool ignore_overrides)
304
{
305
std::vector<ParameterT> result;
306
std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
307
std::transform(
308
parameters.begin(), parameters.end(), std::back_inserter(result),
309
[this, &normalized_namespace, ignore_overrides](auto element) {
310
return static_cast<ParameterT>(
311
this->declare_parameter(
312
normalized_namespace + element.first,
313
element.second.first,
314
element.second.second,
315
ignore_overrides)
316
);
317
}
318
);
319
return result;
322
template<typename ParameterT>
324
Node::get_parameter(const std::string & name, ParameterT & parameter) const
326
std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
327
328
rclcpp::Parameter parameter_variant;
330
bool result = get_parameter(sub_name, parameter_variant);
331
if (result) {
332
parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
334
335
return result;
336
}
337
338
template<typename ParameterT>
339
bool
340
Node::get_parameter_or(
341
const std::string & name,
342
ParameterT & parameter,
343
const ParameterT & alternative_value) const
344
{
345
std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
346
347
bool got_parameter = get_parameter(sub_name, parameter);
348
if (!got_parameter) {
349
parameter = alternative_value;
350
}
351
return got_parameter;
352
}
353
354
template<typename ParameterT>
355
ParameterT
356
Node::get_parameter_or(
357
const std::string & name,
358
const ParameterT & alternative_value) const
359
{
360
ParameterT parameter;
361
get_parameter_or(name, parameter, alternative_value);
362
return parameter;
363
}
364
365
// this is a partially-specialized version of get_parameter above,
366
// where our concrete type for ParameterT is std::map, but the to-be-determined
367
// type is the value in the map.
368
template<typename ParameterT>
369
bool
370
Node::get_parameters(
371
const std::string & prefix,
372
std::map<std::string, ParameterT> & values) const
373
{
374
std::map<std::string, rclcpp::Parameter> params;
375
bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
376
if (result) {
377
for (const auto & param : params) {
378
values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
379
}
380
}
381
382
return result;
383
}
384
385
} // namespace rclcpp
386
387
#endif // RCLCPP__NODE_IMPL_HPP_