-
Notifications
You must be signed in to change notification settings - Fork 522
Expand file tree
/
Copy pathexecutor.hpp
More file actions
603 lines (540 loc) · 23.8 KB
/
executor.hpp
File metadata and controls
603 lines (540 loc) · 23.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
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
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
// 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__EXECUTOR_HPP_
#define RCLCPP__EXECUTOR_HPP_
#include <algorithm>
#include <cassert>
#include <chrono>
#include <cstdlib>
#include <iostream>
#include <list>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
#include "rclcpp/executors/executor_notify_waitable.hpp"
#include "rcpputils/scope_exit.hpp"
#include "rclcpp/context.hpp"
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/guard_condition.hpp"
#include "rclcpp/executor_options.hpp"
#include "rclcpp/executors/executor_entities_collection.hpp"
#include "rclcpp/executors/executor_entities_collector.hpp"
#include "rclcpp/future_return_code.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rclcpp/wait_set.hpp"
namespace rclcpp
{
// Forward declaration is used in convenience method signature.
class Node;
class ExecutorImplementation;
/// Coordinate the order and timing of available communication tasks.
/**
* Executor provides spin functions (including spin_node_once and spin_some).
* It coordinates the nodes and callback groups by looking for available work and completing it,
* based on the threading or concurrency scheme provided by the subclass implementation.
* An example of available work is executing a subscription callback, or a timer callback.
* The executor structure allows for a decoupling of the communication graph and the execution
* model.
* See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms.
*/
class Executor
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
/// Default constructor.
/**
* \param[in] options Options used to configure the executor.
*/
RCLCPP_PUBLIC
explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
/// Default destructor.
RCLCPP_PUBLIC
virtual ~Executor();
/// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
// It is up to the implementation of Executor to implement spin.
virtual void
spin() = 0;
/// Add a callback group to an executor.
/**
* An executor can have zero or more callback groups which provide work during `spin` functions.
* When an executor attempts to add a callback group, the executor checks to see if it is already
* associated with another executor, and if it has been, then an exception is thrown.
* Otherwise, the callback group is added to the executor.
*
* Adding a callback group with this method does not associate its node with this executor
* in any way
*
* \param[in] group_ptr a shared ptr that points to a callback group
* \param[in] node_ptr a shared pointer that points to a node base interface
* \param[in] notify True to trigger the interrupt guard condition during this function. If
* the executor is blocked at the rmw layer while waiting for work and it is notified that a new
* callback group was added, it will wake up.
* \throw std::runtime_error if the callback group is associated to an executor
*/
RCLCPP_PUBLIC
virtual void
add_callback_group(
rclcpp::CallbackGroup::SharedPtr group_ptr,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
bool notify = true);
/// Get callback groups that belong to executor.
/**
* This function returns a vector of weak pointers that point to callback groups that were
* associated with the executor.
* The callback groups associated with this executor may have been added with
* `add_callback_group`, or added when a node was added to the executor with `add_node`, or
* automatically added when it created by a node already associated with this executor and the
* automatically_add_to_executor_with_node parameter was true.
*
* \return a vector of weak pointers that point to callback groups that are associated with
* the executor
*/
RCLCPP_PUBLIC
virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
get_all_callback_groups();
/// Get callback groups that belong to executor.
/**
* This function returns a vector of weak pointers that point to callback groups that were
* associated with the executor.
* The callback groups associated with this executor have been added with
* `add_callback_group`.
*
* \return a vector of weak pointers that point to callback groups that are associated with
* the executor
*/
RCLCPP_PUBLIC
virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
get_manually_added_callback_groups();
/// Get callback groups that belong to executor.
/**
* This function returns a vector of weak pointers that point to callback groups that were
* added from a node that is associated with the executor.
* The callback groups are added when a node is added to the executor with `add_node`, or
* automatically if they are created in the future by that node and have the
* automatically_add_to_executor_with_node argument set to true.
*
* \return a vector of weak pointers that point to callback groups from a node associated with
* the executor
*/
RCLCPP_PUBLIC
virtual std::vector<rclcpp::CallbackGroup::WeakPtr>
get_automatically_added_callback_groups_from_nodes();
/// Remove a callback group from the executor.
/**
* The callback group is removed from and disassociated with the executor.
* If the callback group removed was the last callback group from the node
* that is associated with the executor, the interrupt guard condition
* is triggered and node's guard condition is removed from the executor.
*
* This function only removes a callback group that was manually added with
* rclcpp::Executor::add_callback_group.
* To remove callback groups that were added from a node using
* rclcpp::Executor::add_node, use rclcpp::Executor::remove_node instead.
*
* \param[in] group_ptr Shared pointer to the callback group to be added.
* \param[in] notify True to trigger the interrupt guard condition during this function. If
* the executor is blocked at the rmw layer while waiting for work and it is notified that a
* callback group was removed, it will wake up.
* \throw std::runtime_error if node is deleted before callback group
* \throw std::runtime_error if the callback group is not associated with the executor
*/
RCLCPP_PUBLIC
virtual void
remove_callback_group(
rclcpp::CallbackGroup::SharedPtr group_ptr,
bool notify = true);
/// Add a node to the executor.
/**
* Nodes have associated callback groups, and this method adds any of those callback groups
* to this executor which have their automatically_add_to_executor_with_node parameter true.
* The node is also associated with the executor so that future callback groups which are
* created on the node with the automatically_add_to_executor_with_node parameter set to true
* are also automatically associated with this executor.
*
* Callback groups with the automatically_add_to_executor_with_node parameter set to false must
* be manually added to an executor using the rclcpp::Executor::add_callback_group method.
*
* If a node is already associated with an executor, this method throws an exception.
*
* \param[in] node_ptr Shared pointer to the node to be added.
* \param[in] notify True to trigger the interrupt guard condition during this function. If
* the executor is blocked at the rmw layer while waiting for work and it is notified that a new
* node was added, it will wake up.
* \throw std::runtime_error if a node is already associated to an executor
*/
RCLCPP_PUBLIC
virtual void
add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
/// Convenience function which takes Node and forwards NodeBaseInterface.
/**
* \see rclcpp::Executor::add_node
*/
RCLCPP_PUBLIC
virtual void
add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
/// Remove a node from the executor.
/**
* Any callback groups automatically added when this node was added with
* rclcpp::Executor::add_node are automatically removed, and the node is no longer associated
* with this executor.
*
* This also means that future callback groups created by the given node are no longer
* automatically added to this executor.
*
* \param[in] node_ptr Shared pointer to the node to remove.
* \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
* This is useful if the last node was removed from the executor while the executor was blocked
* waiting for work in another thread, because otherwise the executor would never be notified.
* \throw std::runtime_error if the node is not associated with an executor.
* \throw std::runtime_error if the node is not associated with this executor.
*/
RCLCPP_PUBLIC
virtual void
remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
/// Convenience function which takes Node and forwards NodeBaseInterface.
/**
* \see rclcpp::Executor::remove_node
*/
RCLCPP_PUBLIC
virtual void
remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
/// Add a node to executor, execute the next available unit of work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
* \param[in] timeout How long to wait for work to become available. Negative values cause
* spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
* function to be non-blocking.
*/
template<typename RepT = int64_t, typename T = std::milli>
void
spin_node_once(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
{
return spin_node_once_nanoseconds(
node,
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
);
}
/// Convenience function which takes Node and forwards NodeBaseInterface.
template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
void
spin_node_once(
std::shared_ptr<NodeT> node,
std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
{
return spin_node_once_nanoseconds(
node->get_node_base_interface(),
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
);
}
/// Add a node, complete all immediately available work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
*/
RCLCPP_PUBLIC
virtual void
spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
/// Convenience function which takes Node and forwards NodeBaseInterface.
RCLCPP_PUBLIC
virtual void
spin_node_some(std::shared_ptr<rclcpp::Node> node);
/// Collect work once and execute all available work, optionally within a max duration.
/**
* This function can be overridden.
* The default implementation is suitable for a single-threaded model of execution.
* Adding subscriptions, timers, services, etc. with blocking or long running
* callbacks may cause the function exceed the max_duration significantly.
*
* If there is no work to be done when this called, it will return immediately
* because the collecting of available work is non-blocking.
* Before each piece of ready work is executed this function checks if the
* max_duration has been exceeded, and if it has it returns without starting
* the execution of the next piece of work.
*
* If a max_duration of 0 is given, then all of the collected work will be
* executed before the function returns.
*
* \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
*/
RCLCPP_PUBLIC
virtual void
spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
/// Add a node, complete all immediately available work exhaustively, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
*/
RCLCPP_PUBLIC
virtual void
spin_node_all(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::nanoseconds max_duration);
/// Convenience function which takes Node and forwards NodeBaseInterface.
RCLCPP_PUBLIC
virtual void
spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration);
/// Collect and execute work repeatedly within a duration or until no more work is available.
/**
* This function can be overridden. The default implementation is suitable for a
* single-threaded model of execution.
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
* to block (which may have unintended consequences).
* If the time that waitables take to be executed is longer than the period on which new waitables
* become ready, this method will execute work repeatedly until `max_duration` has elapsed.
*
* \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0.
* `0` is potentially block forever until no more work is available.
* \throw std::invalid_argument if max_duration is less than 0.
* Note that spin_all() may take longer than this time as it only returns once max_duration has
* been exceeded.
*/
RCLCPP_PUBLIC
virtual void
spin_all(std::chrono::nanoseconds max_duration);
/// Collect work once and execute the next available work, optionally within a duration.
/**
* This function can be overridden. The default implementation is suitable for
* a single-thread model of execution.
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
* to block (which may have unintended consequences).
* \param[in] timeout The maximum amount of time to spend waiting for work.
* `-1` is potentially block forever waiting for work.
*/
RCLCPP_PUBLIC
virtual void
spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
/**
* \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
* accessed without blocking (though it may still throw an exception).
* \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
* `-1` is block forever, `0` is non-blocking.
* If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
* code.
* \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
*/
template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
FutureReturnCode
spin_until_future_complete(
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return spin_until_future_complete_impl(
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout),
[&future](std::chrono::nanoseconds wait_time) {
return future.wait_for(wait_time);
}
);
}
/// Cancel any running spin* function, causing it to return.
/**
* This function can be called asynchonously from any thread.
* \throws std::runtime_error if there is an issue triggering the guard condition
*/
RCLCPP_PUBLIC
virtual void
cancel();
/// Returns true if the executor is currently spinning.
/**
* This function can be called asynchronously from any thread.
* \return True if the executor is currently spinning.
*/
RCLCPP_PUBLIC
bool
is_spinning();
protected:
/// Constructor that will not initialize any non-trivial members.
/**
* This constructor is intended to be used by any derived executor
* that explicitly does not want to use the default implementation provided
* by this class.
*/
explicit Executor(const std::shared_ptr<rclcpp::Context> & context);
/// Add a node to executor, execute the next available unit of work, and remove the node.
/**
* Implementation of spin_node_once using std::chrono::nanoseconds
* \param[in] node Shared pointer to the node to add.
* \param[in] timeout How long to wait for work to become available. Negative values cause
* spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
* function to be non-blocking.
*/
RCLCPP_PUBLIC
void
spin_node_once_nanoseconds(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::nanoseconds timeout);
/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
/**
* \sa spin_until_future_complete()
* The only difference with spin_until_future_complete() is that the future's
* type is obscured through a std::function which lets you wait on it
* reguardless of type.
*
* \param[in] timeout see spin_until_future_complete() for details
* \param[in] wait_for_future function to wait on the future and get the
* status after waiting
*/
RCLCPP_PUBLIC
virtual FutureReturnCode
spin_until_future_complete_impl(
std::chrono::nanoseconds timeout,
const std::function<std::future_status(std::chrono::nanoseconds wait_time)> & wait_for_future);
/// Collect work and execute available work, optionally within a duration.
/**
* Implementation of spin_some and spin_all.
* The exhaustive flag controls if the function will re-collect available work within the duration.
*
* \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
* \param[in] exhaustive when set to true, continue to collect work and execute (spin_all)
* when set to false, return when all collected work is executed (spin_some)
*/
RCLCPP_PUBLIC
void
spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
/// Find the next available executable and do the work associated with it.
/**
* \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
* service, client).
* \throws std::runtime_error if there is an issue triggering the guard condition
*/
RCLCPP_PUBLIC
void
execute_any_executable(AnyExecutable & any_exec);
/// Run subscription executable.
/**
* Do necessary setup and tear-down as well as executing the subscription.
* \param[in] subscription Subscription to execute
*/
RCLCPP_PUBLIC
static void
execute_subscription(
rclcpp::SubscriptionBase::SharedPtr subscription);
/// Run timer executable.
/**
* Do necessary setup and tear-down as well as executing the timer callback.
* \param[in] timer Timer to execute
*/
RCLCPP_PUBLIC
static void
execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr<void> & data_ptr);
/// Run service server executable.
/**
* Do necessary setup and tear-down as well as executing the service server callback.
* \param[in] service Service to execute
*/
RCLCPP_PUBLIC
static void
execute_service(rclcpp::ServiceBase::SharedPtr service);
/// Run service client executable.
/**
* Do necessary setup and tear-down as well as executing the service client callback.
* \param[in] service Service to execute
*/
RCLCPP_PUBLIC
static void
execute_client(rclcpp::ClientBase::SharedPtr client);
/// Gather all of the waitable entities from associated nodes and callback groups.
RCLCPP_PUBLIC
void
collect_entities();
/// Block until more work becomes avilable or timeout is reached.
/**
* Builds a set of waitable entities, which are passed to the middleware.
* After building wait set, waits on middleware to notify.
* \param[in] timeout duration to wait for new work to become available.
* \throws std::runtime_error if the wait set can be cleared
*/
RCLCPP_PUBLIC
void
wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
/// Check for executable in ready state and populate union structure.
/**
* \param[out] any_executable populated union structure of ready executable
* \return true if an executable was ready and any_executable was populated,
* otherwise false
*/
RCLCPP_PUBLIC
bool
get_next_ready_executable(AnyExecutable & any_executable);
/// Wait for executable in ready state and populate union structure.
/**
* If an executable is ready, it will return immediately, otherwise
* block based on the timeout for work to become ready.
*
* \param[out] any_executable populated union structure of ready executable
* \param[in] timeout duration of time to wait for work, a negative value
* (the defualt behavior), will make this function block indefinitely
* \return true if an executable was ready and any_executable was populated,
* otherwise false
*/
RCLCPP_PUBLIC
bool
get_next_executable(
AnyExecutable & any_executable,
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
/// This function triggers a recollect of all entities that are registered to the executor.
/**
* Calling this function is thread safe.
*
* \param[in] notify if true will execute a trigger that will wake up a waiting executor
*/
RCLCPP_PUBLIC
virtual void
handle_updated_entities(bool notify);
/// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
std::atomic_bool spinning;
/// Guard condition for signaling the rmw layer to wake up for special events.
std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
/// Guard condition for signaling the rmw layer to wake up for system shutdown.
std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
mutable std::mutex mutex_;
/// The context associated with this executor.
std::shared_ptr<rclcpp::Context> context_;
RCLCPP_DISABLE_COPY(Executor)
RCLCPP_PUBLIC
virtual void
spin_once_impl(std::chrono::nanoseconds timeout);
/// Waitable containing guard conditions controlling the executor flow.
/**
* This waitable contains the interrupt and shutdown guard condition, as well
* as the guard condition associated with each node and callback group.
* By default, if any change is detected in the monitored entities, the notify
* waitable will awake the executor and rebuild the collections.
*/
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
std::atomic_bool entities_need_rebuild_;
/// Collector used to associate executable entities from nodes and guard conditions
rclcpp::executors::ExecutorEntitiesCollector collector_;
/// WaitSet to be waited on.
rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
std::optional<rclcpp::WaitResult<rclcpp::WaitSet>> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
/// Hold the current state of the collection being waited on by the waitset
rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
mutex_);
/// Hold the current state of the notify waitable being waited on by the waitset
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
RCPPUTILS_TSA_GUARDED_BY(mutex_);
/// shutdown callback handle registered to Context
rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
/// Pointer to implementation
std::unique_ptr<ExecutorImplementation> impl_;
};
} // namespace rclcpp
#endif // RCLCPP__EXECUTOR_HPP_