-
Notifications
You must be signed in to change notification settings - Fork 522
Expand file tree
/
Copy pathcontext.hpp
More file actions
444 lines (391 loc) · 14.8 KB
/
context.hpp
File metadata and controls
444 lines (391 loc) · 14.8 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
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
// Copyright 2014 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__CONTEXT_HPP_
#define RCLCPP__CONTEXT_HPP_
#include <condition_variable>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <typeindex>
#include <typeinfo>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#include <stdexcept>
#include "rcl/context.h"
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
#include "rclcpp/init_options.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace graph_listener
{
class GraphListener;
} // namespace graph_listener
/// Thrown when init is called on an already initialized context.
class ContextAlreadyInitialized : public std::runtime_error
{
public:
ContextAlreadyInitialized()
: std::runtime_error("context is already initialized") {}
};
/// Forward declare WeakContextsWrapper
class WeakContextsWrapper;
class ShutdownCallbackHandle
{
friend class Context;
public:
using ShutdownCallbackType = std::function<void ()>;
private:
std::weak_ptr<ShutdownCallbackType> callback;
};
using OnShutdownCallbackHandle = ShutdownCallbackHandle;
using PreShutdownCallbackHandle = ShutdownCallbackHandle;
/// Context which encapsulates shared state between nodes and other similar entities.
/**
* A context also represents the lifecycle between init and shutdown of rclcpp.
* Nodes may be attached to a particular context by passing to the rclcpp::Node
* constructor a rclcpp::NodeOptions instance in which the Context is set via
* rclcpp::NodeOptions::context.
* Nodes will be automatically removed from the context when destructed.
* Contexts may be shutdown by calling rclcpp::shutdown.
*/
class Context : public std::enable_shared_from_this<Context>
{
public:
RCLCPP_SMART_PTR_DEFINITIONS(Context)
/// Default constructor, after which the Context is still not "initialized".
/**
* Every context which is constructed is added to a global vector of contexts,
* which is used by the signal handler to conditionally shutdown each context
* on SIGINT.
* See the shutdown_on_signal option in the InitOptions class.
*/
RCLCPP_PUBLIC
Context();
RCLCPP_PUBLIC
virtual
~Context();
/// Initialize the context, and the underlying elements like the rcl context.
/**
* This method must be called before passing this context to things like the
* constructor of Node.
* It must also be called before trying to shutdown the context.
*
* Note that this function does not setup any signal handlers, so if you want
* it to be shutdown by the signal handler, then you need to either install
* them manually with rclcpp::install_signal_handlers() or use rclcpp::init().
* In addition to installing the signal handlers, the shutdown_on_signal
* of the InitOptions needs to be `true` for this context to be shutdown by
* the signal handler, otherwise it will be passed over.
*
* After calling this method, shutdown() can be called to invalidate the
* context for derived entities, e.g. nodes, guard conditions, etc.
* However, the underlying rcl context is not finalized until this Context's
* destructor is called or this function is called again.
* Allowing this class to go out of scope and get destructed or calling this
* function a second time while derived entities are still using the context
* is undefined behavior and should be avoided.
* It's a good idea to not reuse context objects and instead create a new one
* each time you need to shutdown and init again.
* This allows derived entities to hold on to shard pointers to the first
* context object until they are done.
*
* This function is thread-safe.
*
* \param[in] argc number of arguments
* \param[in] argv argument array which may contain arguments intended for ROS
* \param[in] init_options initialization options for rclcpp and underlying layers
* \throw ContextAlreadyInitialized if called if init is called more than once
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
* \throws std::runtime_error if the global logging configure mutex is NULL
* \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments
*/
RCLCPP_PUBLIC
virtual
void
init(
int argc,
char const * const * argv,
const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
/// Return true if the context is valid, otherwise false.
/**
* The context is valid if it has been initialized but not shutdown.
*
* This function is thread-safe.
* This function is lock free so long as pointers and uint64_t atomics are
* lock free.
*
* \return true if valid, otherwise false
*/
RCLCPP_PUBLIC
bool
is_valid() const;
/// Return the init options used during init.
RCLCPP_PUBLIC
const rclcpp::InitOptions &
get_init_options() const;
/// Return a copy of the init options used during init.
RCLCPP_PUBLIC
rclcpp::InitOptions
get_init_options();
/// Return actual domain id.
RCLCPP_PUBLIC
size_t
get_domain_id() const;
/// Return the shutdown reason, or empty string if not shutdown.
/**
* This function is thread-safe.
*/
RCLCPP_PUBLIC
std::string
shutdown_reason() const;
/// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
/**
* Several things happen when the context is shutdown, in this order:
*
* - acquires a lock to prevent race conditions with init, on_shutdown, etc.
* - if the context is not initialized, return false
* - rcl_shutdown() is called on the internal rcl_context_t instance
* - the shutdown reason is set
* - each on_shutdown callback is called, in the order that they were added
* - interrupt blocking sleep_for() calls, so they return early due to shutdown
* - interrupt blocking executors and wait sets
*
* The underlying rcl context is not finalized by this function.
*
* This function is thread-safe.
*
* Note that if you override this method, but leave shutdown to be called in
* the destruction of this base class, it will not call the overridden
* version from your base class.
* So you need to ensure you call your class's shutdown() in its destructor.
*
* \param[in] reason the description of why shutdown happened
* \return true if shutdown was successful, false if context was already shutdown
* \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
*/
RCLCPP_PUBLIC
virtual
bool
shutdown(const std::string & reason);
using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
/// Add a on_shutdown callback to be called when shutdown is called for this context.
/**
* These callbacks will be called in the order they are added as the second
* to last step in shutdown().
*
* When shutdown occurs due to the signal handler, these callbacks are run
* asynchronously in the dedicated singal handling thread.
*
* Also, shutdown() may be called from the destructor of this function.
* Therefore, it is not safe to throw exceptions from these callbacks.
* Instead, log errors or use some other mechanism to indicate an error has
* occurred.
*
* On shutdown callbacks may be registered before init and after shutdown,
* and persist on repeated init's.
*
* \param[in] callback the on shutdown callback to be registered
* \return the callback passed, for convenience when storing a passed lambda
*/
RCLCPP_PUBLIC
virtual
OnShutdownCallback
on_shutdown(const OnShutdownCallback & callback);
/// Add a on_shutdown callback to be called when shutdown is called for this context.
/**
* These callbacks will be called in the order they are added as the second
* to last step in shutdown().
*
* When shutdown occurs due to the signal handler, these callbacks are run
* asynchronously in the dedicated signal handling thread.
*
* Also, shutdown() may be called from the destructor of this function.
* Therefore, it is not safe to throw exceptions from these callbacks.
* Instead, log errors or use some other mechanism to indicate an error has
* occurred.
*
* On shutdown callbacks may be registered before init and after shutdown,
* and persist on repeated init's.
*
* \param[in] callback the on_shutdown callback to be registered
* \return the created callback handle
*/
RCLCPP_PUBLIC
virtual
OnShutdownCallbackHandle
add_on_shutdown_callback(const OnShutdownCallback & callback);
/// Remove an registered on_shutdown callbacks.
/**
* \param[in] callback_handle the on_shutdown callback handle to be removed.
* \return true if the callback is found and removed, otherwise false.
*/
RCLCPP_PUBLIC
virtual
bool
remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
/// Add a pre_shutdown callback to be called before shutdown is called for this context.
/**
* These callbacks will be called in the order they are added.
*
* When shutdown occurs due to the signal handler, these callbacks are run
* asynchronously in the dedicated signal handling thread.
*
* \param[in] callback the pre_shutdown callback to be registered
* \return the created callback handle
*/
RCLCPP_PUBLIC
virtual
PreShutdownCallbackHandle
add_pre_shutdown_callback(const PreShutdownCallback & callback);
/// Remove an registered pre_shutdown callback.
/**
* \param[in] callback_handle the pre_shutdown callback handle to be removed.
* \return true if the callback is found and removed, otherwise false.
*/
RCLCPP_PUBLIC
virtual
bool
remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
/// Return the shutdown callbacks.
/**
* Returned callbacks are a copy of the registered callbacks.
*/
RCLCPP_PUBLIC
std::vector<OnShutdownCallback>
get_on_shutdown_callbacks() const;
/// Return the pre-shutdown callbacks.
/**
* Returned callbacks are a copy of the registered callbacks.
*/
RCLCPP_PUBLIC
std::vector<PreShutdownCallback>
get_pre_shutdown_callbacks() const;
/// Return the internal rcl context.
RCLCPP_PUBLIC
std::shared_ptr<rcl_context_t>
get_rcl_context();
RCLCPP_PUBLIC
std::shared_ptr<rclcpp::graph_listener::GraphListener>
get_graph_listener();
/// Sleep for a given period of time or until shutdown() is called.
/**
* This function can be interrupted early if:
*
* - this context is shutdown()
* - this context is destructed (resulting in shutdown)
* - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown)
* - interrupt_all_sleep_for() is called
*
* \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
* \return true if the condition variable did not timeout, i.e. you were interrupted.
*/
RCLCPP_PUBLIC
bool
sleep_for(const std::chrono::nanoseconds & nanoseconds);
/// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
RCLCPP_PUBLIC
void
interrupt_all_sleep_for();
/// Return a singleton instance for the SubContext type, constructing one if necessary.
template<typename SubContext, typename ... Args>
std::shared_ptr<SubContext>
get_sub_context(Args && ... args)
{
std::lock_guard<std::recursive_mutex> lock(sub_contexts_mutex_);
std::type_index type_i(typeid(SubContext));
std::shared_ptr<SubContext> sub_context;
auto it = sub_contexts_.find(type_i);
if (it == sub_contexts_.end()) {
// It doesn't exist yet, make it
sub_context = std::shared_ptr<SubContext>(
new SubContext(std::forward<Args>(args) ...),
[](SubContext * sub_context_ptr) {
delete sub_context_ptr;
});
sub_contexts_[type_i] = sub_context;
} else {
// It exists, get it out and cast it.
sub_context = std::static_pointer_cast<SubContext>(it->second);
}
return sub_context;
}
protected:
// Called by constructor and destructor to clean up by finalizing the
// shutdown rcl context and preparing for a new init cycle.
RCLCPP_PUBLIC
void
clean_up();
private:
RCLCPP_DISABLE_COPY(Context)
// This mutex is recursive so that the destructor can ensure atomicity
// between is_initialized and shutdown.
mutable std::recursive_mutex init_mutex_;
std::shared_ptr<rcl_context_t> rcl_context_;
rclcpp::InitOptions init_options_;
std::string shutdown_reason_;
// Keep shared ownership of the global logging mutex.
std::shared_ptr<std::recursive_mutex> logging_mutex_;
std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
// This mutex is recursive so that the constructor of a sub context may
// attempt to acquire another sub context.
std::recursive_mutex sub_contexts_mutex_;
std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
mutable std::recursive_mutex on_shutdown_callbacks_mutex_;
std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
mutable std::recursive_mutex pre_shutdown_callbacks_mutex_;
/// Condition variable for timed sleep (see sleep_for).
std::condition_variable interrupt_condition_variable_;
/// Mutex for protecting the global condition variable.
std::mutex interrupt_mutex_;
/// Graph Listener which waits on graph changes for the node and is shared across nodes.
std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
/// Keep shared ownership of global vector of weak contexts
std::shared_ptr<WeakContextsWrapper> weak_contexts_;
enum class ShutdownType
{
pre_shutdown,
on_shutdown
};
using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
template<ShutdownType shutdown_type>
RCLCPP_LOCAL
ShutdownCallbackHandle
add_shutdown_callback(
const ShutdownCallback & callback);
template<ShutdownType shutdown_type>
RCLCPP_LOCAL
bool
remove_shutdown_callback(
const ShutdownCallbackHandle & callback_handle);
template<ShutdownType shutdown_type>
RCLCPP_LOCAL
std::vector<rclcpp::Context::ShutdownCallback>
get_shutdown_callback() const;
};
/// Return a copy of the list of context shared pointers.
/**
* This function is thread-safe.
*/
RCLCPP_PUBLIC
std::vector<Context::SharedPtr>
get_contexts();
} // namespace rclcpp
#endif // RCLCPP__CONTEXT_HPP_