-
Notifications
You must be signed in to change notification settings - Fork 406
/
qos.hpp
536 lines (470 loc) · 15.1 KB
/
qos.hpp
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
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// 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__QOS_HPP_
#define RCLCPP__QOS_HPP_
#include <string>
#include "rclcpp/duration.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rcl/logging_rosout.h"
#include "rmw/incompatible_qos_events_statuses.h"
#include "rmw/qos_profiles.h"
#include "rmw/types.h"
namespace rclcpp
{
RCLCPP_PUBLIC
std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
enum class HistoryPolicy
{
KeepLast = RMW_QOS_POLICY_HISTORY_KEEP_LAST,
KeepAll = RMW_QOS_POLICY_HISTORY_KEEP_ALL,
SystemDefault = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
Unknown = RMW_QOS_POLICY_HISTORY_UNKNOWN,
};
enum class ReliabilityPolicy
{
BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE,
SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
BestAvailable = RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE,
Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN,
};
enum class DurabilityPolicy
{
Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE,
TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
BestAvailable = RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE,
Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN,
};
enum class LivelinessPolicy
{
Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
BestAvailable = RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE,
Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN,
};
enum class QoSCompatibility
{
Ok = RMW_QOS_COMPATIBILITY_OK,
Warning = RMW_QOS_COMPATIBILITY_WARNING,
Error = RMW_QOS_COMPATIBILITY_ERROR,
};
/// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
struct RCLCPP_PUBLIC QoSInitialization
{
rmw_qos_history_policy_t history_policy;
size_t depth;
/// Constructor which takes both a history policy and a depth (even if it would be unused).
QoSInitialization(
rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
bool print_depth_warning = true);
/// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth.
static
QoSInitialization
from_rmw(const rmw_qos_profile_t & rmw_qos);
};
/// Use to initialize the QoS with the keep_all history setting.
struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization
{
KeepAll();
};
/// Use to initialize the QoS with the keep_last history setting and the given depth.
struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
{
explicit KeepLast(size_t depth, bool print_depth_warning = true);
};
/// Encapsulation of Quality of Service settings.
/**
* Quality of Service settings control the behavior of publishers, subscriptions,
* and other entities, and includes things like how data is sent or resent,
* how data is buffered on the publishing and subscribing side, and other things.
* See:
* <a href="https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html">
* https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html
* </a>
*/
class RCLCPP_PUBLIC QoS
{
public:
/// Create a QoS by specifying only the history policy and history depth.
/**
* When using the default initial profile, the defaults will include:
*
* - \link rclcpp::ReliabilityPolicy::Reliable ReliabilityPolicy::Reliable\endlink
* - \link rclcpp::DurabilityPolicy::Volatile DurabilityPolicy::Volatile\endlink
*
* See rmw_qos_profile_default for a full list of default settings.
* If some other rmw_qos_profile_t is passed to initial_profile, then the defaults will derive from
* that profile instead.
*
* \param[in] qos_initialization Specifies history policy and history depth.
* \param[in] initial_profile The rmw_qos_profile_t instance on which to base the default settings.
*/
explicit
QoS(
const QoSInitialization & qos_initialization,
const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default);
/// Conversion constructor to ease construction in the common case of just specifying depth.
/**
* This is a convenience constructor that calls QoS(KeepLast(history_depth)).
*
* \param[in] history_depth How many messages can be queued when publishing
* with a Publisher, or how many messages can be queued before being replaced
* by a Subscription.
*/
// cppcheck-suppress noExplicitConstructor
QoS(size_t history_depth); // NOLINT(runtime/explicit): conversion constructor
/// Return the rmw qos profile.
rmw_qos_profile_t &
get_rmw_qos_profile();
/// Return the rmw qos profile.
const rmw_qos_profile_t &
get_rmw_qos_profile() const;
/// Set the history policy.
QoS &
history(HistoryPolicy history);
/// Set the history policy.
QoS &
history(rmw_qos_history_policy_t history);
/// Set the history to keep last.
QoS &
keep_last(size_t depth);
/// Set the history to keep all.
QoS &
keep_all();
/// Set the reliability setting.
QoS &
reliability(rmw_qos_reliability_policy_t reliability);
/// Set the reliability setting.
QoS &
reliability(ReliabilityPolicy reliability);
/// Set the reliability setting to reliable.
QoS &
reliable();
/// Set the reliability setting to best effort.
QoS &
best_effort();
/// Set the reliability setting to best available.
QoS &
reliability_best_available();
/// Set the durability setting.
QoS &
durability(rmw_qos_durability_policy_t durability);
/// Set the durability setting.
QoS &
durability(DurabilityPolicy durability);
/// Set the durability setting to volatile.
/**
* Note that this cannot be named `volatile` because it is a C++ keyword.
*/
QoS &
durability_volatile();
/// Set the durability setting to transient local.
QoS &
transient_local();
/// Set the durability setting to best available.
QoS &
durability_best_available();
/// Set the deadline setting.
QoS &
deadline(rmw_time_t deadline);
/// Set the deadline setting, rclcpp::Duration.
QoS &
deadline(const rclcpp::Duration & deadline);
/// Set the lifespan setting.
QoS &
lifespan(rmw_time_t lifespan);
/// Set the lifespan setting, rclcpp::Duration.
QoS &
lifespan(const rclcpp::Duration & lifespan);
/// Set the liveliness setting.
QoS &
liveliness(rmw_qos_liveliness_policy_t liveliness);
/// Set the liveliness setting.
QoS &
liveliness(LivelinessPolicy liveliness);
/// Set the liveliness_lease_duration setting.
QoS &
liveliness_lease_duration(rmw_time_t liveliness_lease_duration);
/// Set the liveliness_lease_duration setting, rclcpp::Duration.
QoS &
liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration);
/// Set the avoid_ros_namespace_conventions setting.
QoS &
avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions);
/// Get the history qos policy.
HistoryPolicy
history() const;
/// Get the history depth.
size_t
depth() const;
/// Get the reliability policy.
ReliabilityPolicy
reliability() const;
/// Get the durability policy.
DurabilityPolicy
durability() const;
/// Get the deadline duration setting.
rclcpp::Duration
deadline() const;
/// Get the lifespan duration setting.
rclcpp::Duration
lifespan() const;
/// Get the liveliness policy.
LivelinessPolicy
liveliness() const;
/// Get the liveliness lease duration setting.
rclcpp::Duration
liveliness_lease_duration() const;
/// Get the `avoid ros namespace convention` setting.
bool
avoid_ros_namespace_conventions() const;
private:
rmw_qos_profile_t rmw_qos_profile_;
};
/// Check if two QoS profiles are exactly equal in all policy values.
RCLCPP_PUBLIC
bool operator==(const QoS & left, const QoS & right);
RCLCPP_PUBLIC
bool operator!=(const QoS & left, const QoS & right);
/// Result type for checking QoS compatibility
/**
* \see rclcpp::qos_check_compatible()
*/
struct QoSCheckCompatibleResult
{
/// Compatibility result.
QoSCompatibility compatibility;
/// Reason for a (possible) incompatibility.
/**
* Set if compatiblity is QoSCompatibility::Warning or QoSCompatiblity::Error.
* Not set if the QoS profiles are compatible.
*/
std::string reason;
};
/// Check if two QoS profiles are compatible.
/**
* Two QoS profiles are compatible if a publisher and subcription
* using the QoS policies can communicate with each other.
*
* If any policies have value "system default" or "unknown" then it is possible that
* compatiblity cannot be determined.
* In this case, the value QoSCompatility::Warning is set as part of
* the returned structure.
*
* Example usage:
*
* ```cpp
* rclcpp::QoSCheckCompatibleResult result = rclcpp::qos_check_compatible(
* publisher_qos, subscription_qos);
* if (rclcpp::QoSCompatibility::Error != result.compatibility) {
* // QoS not compatible ...
* // result.reason contains info about the incompatibility
* } else if (rclcpp::QoSCompatibility::Warning != result.compatibility) {
* // QoS may not be compatible ...
* // result.reason contains info about the possible incompatibility
* }
* ```
*
* \param[in] publisher_qos: The QoS profile for a publisher.
* \param[in] subscription_qos: The QoS profile for a subscription.
* \return Struct with compatiblity set to QoSCompatibility::Ok if the QoS profiles are
* compatible, or
* \return Struct with compatibility set to QoSCompatibility::Warning if there is a chance
* the QoS profiles are not compatible, or
* \return Struct with compatibility set to QoSCompatibility::Error if the QoS profiles are
* not compatible.
* \throws rclcpp::exceptions::QoSCheckCompatibilityException if an unexpected error occurs.
*/
RCLCPP_PUBLIC
QoSCheckCompatibleResult
qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos);
/**
* Clock QoS class
* - History: Keep last,
* - Depth: 1,
* - Reliability: Best effort,
* - Durability: Volatile,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC ClockQoS : public QoS
{
public:
explicit
ClockQoS(
const QoSInitialization & qos_initialization = KeepLast(1));
};
/**
* Sensor Data QoS class
* - History: Keep last,
* - Depth: 5,
* - Reliability: Best effort,
* - Durability: Volatile,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC SensorDataQoS : public QoS
{
public:
explicit
SensorDataQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)
));
};
/**
* Parameters QoS class
* - History: Keep last,
* - Depth: 1000,
* - Reliability: Reliable,
* - Durability: Volatile,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - Avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC ParametersQoS : public QoS
{
public:
explicit
ParametersQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_parameters)
));
};
/**
* Services QoS class
* - History: Keep last,
* - Depth: 10,
* - Reliability: Reliable,
* - Durability: Volatile,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - Avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC ServicesQoS : public QoS
{
public:
explicit
ServicesQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_services_default)
));
};
/**
* Parameter events QoS class
* - History: Keep last,
* - Depth: 1000,
* - Reliability: Reliable,
* - Durability: Volatile,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - Avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC ParameterEventsQoS : public QoS
{
public:
explicit
ParameterEventsQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
));
};
/**
* Rosout QoS class
* - History: Keep last,
* - Depth: 1000,
* - Reliability: Reliable,
* - Durability: TRANSIENT_LOCAL,
* - Deadline: Default,
* - Lifespan: {10, 0},
* - Liveliness: System default,
* - Liveliness lease duration: default,
* - Avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC RosoutQoS : public QoS
{
public:
explicit
RosoutQoS(
const QoSInitialization & rosout_qos_initialization = (
QoSInitialization::from_rmw(rcl_qos_profile_rosout_default)
));
};
/**
* System defaults QoS class
* - History: System default,
* - Depth: System default,
* - Reliability: System default,
* - Durability: System default,
* - Deadline: Default,
* - Lifespan: Default,
* - Liveliness: System default,
* - Liveliness lease duration: System default,
* - Avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS
{
public:
explicit
SystemDefaultsQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_system_default)
));
};
/**
* Best available QoS class
*
* Match majority of endpoints currently available while maintaining the highest level of service.
* Policies are chosen at the time of creating a subscription or publisher.
* The middleware is not expected to update policies after creating a subscription or publisher,
* even if one or more policies are incompatible with newly discovered endpoints.
* Therefore, this profile should be used with care since non-deterministic behavior can occur due
* to races with discovery.
*
* - History: Keep last,
* - Depth: 10,
* - Reliability: Best available,
* - Durability: Best available,
* - Deadline: Best available,
* - Lifespan: Default,
* - Liveliness: Best available,
* - Liveliness lease duration: Best available,
* - avoid ros namespace conventions: false
*/
class RCLCPP_PUBLIC BestAvailableQoS : public QoS
{
public:
explicit
BestAvailableQoS(
const QoSInitialization & qos_initialization = (
QoSInitialization::from_rmw(rmw_qos_profile_best_available)
));
};
} // namespace rclcpp
#endif // RCLCPP__QOS_HPP_