ROS 2 rclcpp + rcl - humble
humble
ROS 2 C++ Client Library with ROS Client Library
|
Coordinate the order and timing of available communication tasks. More...
#include <rclcpp/executor.hpp>
Public Member Functions | |
RCLCPP_PUBLIC | Executor (const rclcpp::ExecutorOptions &options=rclcpp::ExecutorOptions()) |
Default constructor. More... | |
virtual RCLCPP_PUBLIC | ~Executor () |
Default destructor. | |
virtual void | spin ()=0 |
Do work periodically as it becomes available to us. Blocking call, may block indefinitely. | |
virtual RCLCPP_PUBLIC void | add_callback_group (rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify=true) |
Add a callback group to an executor. More... | |
virtual RCLCPP_PUBLIC std::vector< rclcpp::CallbackGroup::WeakPtr > | get_all_callback_groups () |
Get callback groups that belong to executor. More... | |
virtual RCLCPP_PUBLIC std::vector< rclcpp::CallbackGroup::WeakPtr > | get_manually_added_callback_groups () |
Get callback groups that belong to executor. More... | |
virtual RCLCPP_PUBLIC std::vector< rclcpp::CallbackGroup::WeakPtr > | get_automatically_added_callback_groups_from_nodes () |
Get callback groups that belong to executor. More... | |
virtual RCLCPP_PUBLIC void | remove_callback_group (rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify=true) |
Remove a callback group from the executor. More... | |
virtual RCLCPP_PUBLIC void | add_node (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify=true) |
Add a node to the executor. More... | |
virtual RCLCPP_PUBLIC void | add_node (std::shared_ptr< rclcpp::Node > node_ptr, bool notify=true) |
Convenience function which takes Node and forwards NodeBaseInterface. More... | |
virtual RCLCPP_PUBLIC void | remove_node (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify=true) |
Remove a node from the executor. More... | |
virtual RCLCPP_PUBLIC void | remove_node (std::shared_ptr< rclcpp::Node > node_ptr, bool notify=true) |
Convenience function which takes Node and forwards NodeBaseInterface. More... | |
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)) |
Add a node to executor, execute the next available unit of work, and remove the node. More... | |
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)) |
Convenience function which takes Node and forwards NodeBaseInterface. | |
RCLCPP_PUBLIC void | spin_node_some (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) |
Add a node, complete all immediately available work, and remove the node. More... | |
RCLCPP_PUBLIC void | spin_node_some (std::shared_ptr< rclcpp::Node > node) |
Convenience function which takes Node and forwards NodeBaseInterface. | |
virtual RCLCPP_PUBLIC void | spin_some (std::chrono::nanoseconds max_duration=std::chrono::nanoseconds(0)) |
Collect work once and execute all available work, optionally within a duration. More... | |
virtual RCLCPP_PUBLIC void | spin_all (std::chrono::nanoseconds max_duration) |
Collect and execute work repeatedly within a duration or until no more work is available. More... | |
virtual RCLCPP_PUBLIC void | spin_once (std::chrono::nanoseconds timeout=std::chrono::nanoseconds(-1)) |
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)) |
Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. More... | |
RCLCPP_PUBLIC void | cancel () |
Cancel any running spin* function, causing it to return. More... | |
RCLCPP_PUBLIC void | set_memory_strategy (memory_strategy::MemoryStrategy::SharedPtr memory_strategy) |
Support dynamic switching of the memory strategy. More... | |
RCLCPP_PUBLIC bool | is_spinning () |
Returns true if the executor is currently spinning. More... | |
Protected Types | |
typedef std::map< rclcpp::CallbackGroup::WeakPtr, const rclcpp::GuardCondition *, std::owner_less< rclcpp::CallbackGroup::WeakPtr > > | WeakCallbackGroupsToGuardConditionsMap |
Protected Member Functions | |
RCLCPP_PUBLIC void | spin_node_once_nanoseconds (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout) |
RCLCPP_PUBLIC void | spin_some_impl (std::chrono::nanoseconds max_duration, bool exhaustive) |
RCLCPP_PUBLIC void | execute_any_executable (AnyExecutable &any_exec) |
Find the next available executable and do the work associated with it. More... | |
RCLCPP_PUBLIC void | wait_for_work (std::chrono::nanoseconds timeout=std::chrono::nanoseconds(-1)) |
RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr | get_node_by_group (const WeakCallbackGroupsToNodesMap &weak_groups_to_nodes, rclcpp::CallbackGroup::SharedPtr group) |
RCLCPP_PUBLIC bool | has_node (const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const WeakCallbackGroupsToNodesMap &weak_groups_to_nodes) const |
Return true if the node has been added to this executor. More... | |
RCLCPP_PUBLIC rclcpp::CallbackGroup::SharedPtr | get_group_by_timer (rclcpp::TimerBase::SharedPtr timer) |
virtual RCLCPP_PUBLIC void | add_callback_group_to_map (rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, WeakCallbackGroupsToNodesMap &weak_groups_to_nodes, bool notify=true) RCPPUTILS_TSA_REQUIRES(mutex_) |
Add a callback group to an executor. More... | |
virtual RCLCPP_PUBLIC void | remove_callback_group_from_map (rclcpp::CallbackGroup::SharedPtr group_ptr, WeakCallbackGroupsToNodesMap &weak_groups_to_nodes, bool notify=true) RCPPUTILS_TSA_REQUIRES(mutex_) |
Remove a callback group from the executor. More... | |
RCLCPP_PUBLIC bool | get_next_ready_executable (AnyExecutable &any_executable) |
RCLCPP_PUBLIC bool | get_next_ready_executable_from_map (AnyExecutable &any_executable, const WeakCallbackGroupsToNodesMap &weak_groups_to_nodes) |
RCLCPP_PUBLIC bool | get_next_executable (AnyExecutable &any_executable, std::chrono::nanoseconds timeout=std::chrono::nanoseconds(-1)) |
virtual RCLCPP_PUBLIC void | add_callback_groups_from_nodes_associated_to_executor () RCPPUTILS_TSA_REQUIRES(mutex_) |
Add all callback groups that can be automatically added from associated nodes. More... | |
memory_strategy::MemoryStrategy::SharedPtr memory_strategy_ | RCPPUTILS_TSA_PT_GUARDED_BY (mutex_) |
The memory strategy: an interface for handling user-defined memory allocation strategies. | |
virtual RCLCPP_PUBLIC void | spin_once_impl (std::chrono::nanoseconds timeout) |
WeakCallbackGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ | RCPPUTILS_TSA_GUARDED_BY (mutex_) |
maps callback groups to guard conditions | |
WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_ | RCPPUTILS_TSA_GUARDED_BY (mutex_) |
maps callback groups associated to nodes | |
WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_ | RCPPUTILS_TSA_GUARDED_BY (mutex_) |
maps callback groups to nodes associated with executor | |
WeakCallbackGroupsToNodesMap weak_groups_to_nodes_ | RCPPUTILS_TSA_GUARDED_BY (mutex_) |
maps all callback groups to nodes | |
std::list< rclcpp::node_interfaces::NodeBaseInterface::WeakPtr > weak_nodes_ | RCPPUTILS_TSA_GUARDED_BY (mutex_) |
nodes that are associated with the executor | |
Protected Attributes | |
std::atomic_bool | spinning |
Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. | |
rclcpp::GuardCondition | interrupt_guard_condition_ |
Guard condition for signaling the rmw layer to wake up for special events. | |
std::shared_ptr< rclcpp::GuardCondition > | shutdown_guard_condition_ |
rcl_wait_set_t | wait_set_ = rcl_get_zero_initialized_wait_set() |
Wait set for managing entities that the rmw layer waits on. | |
std::mutex | mutex_ |
std::shared_ptr< rclcpp::Context > | context_ |
The context associated with this executor. | |
rclcpp::OnShutdownCallbackHandle | shutdown_callback_handle_ |
shutdown callback handle registered to Context | |
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.
Definition at line 65 of file executor.hpp.
|
explicit |
Default constructor.
[in] | options | Options used to configure the executor. |
Definition at line 46 of file executor.cpp.
References context_, interrupt_guard_condition_, RCL_RET_OK, rcl_wait_set_init(), shutdown_callback_handle_, and wait_set_.
|
virtual |
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
[in] | group_ptr | a shared ptr that points to a callback group |
[in] | node_ptr | a shared pointer that points to a node base interface |
[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. |
std::runtime_error | if the callback group is associated to an executor |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 239 of file executor.cpp.
References add_callback_group_to_map().
|
protectedvirtual |
Add a callback group to an executor.
Definition at line 196 of file executor.cpp.
References interrupt_guard_condition_, and rclcpp::GuardCondition::trigger().
Referenced by add_callback_group().
|
protectedvirtual |
Add all callback groups that can be automatically added from associated nodes.
The executor, before collecting entities, verifies if any callback group from nodes associated with the executor, which is not already associated to an executor, can be automatically added to this executor. This takes care of any callback group that has been added to a node but not explicitly added to the executor. It is important to note that in order for the callback groups to be automatically added to an executor through this function, the node of the callback groups needs to have been added through the add_node
method.
Definition at line 172 of file executor.cpp.
Referenced by wait_for_work().
|
virtual |
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.
[in] | node_ptr | Shared pointer to the node to be added. |
[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. |
std::runtime_error | if a node is already associated to an executor |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 253 of file executor.cpp.
Referenced by add_node(), rclcpp::spin(), and spin_node_some().
|
virtual |
Convenience function which takes Node and forwards NodeBaseInterface.
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 336 of file executor.cpp.
References add_node().
void Executor::cancel | ( | ) |
Cancel any running spin* function, causing it to return.
This function can be called asynchonously from any thread.
std::runtime_error | if there is an issue triggering the guard condition |
Definition at line 487 of file executor.cpp.
References interrupt_guard_condition_, spinning, and rclcpp::GuardCondition::trigger().
|
protected |
Find the next available executable and do the work associated with it.
[in] | any_exec | Union structure that can hold any executable type (timer, subscription, service, client). |
std::runtime_error | if there is an issue triggering the guard condition |
Definition at line 509 of file executor.cpp.
References interrupt_guard_condition_, spinning, and rclcpp::GuardCondition::trigger().
Referenced by rclcpp::executors::SingleThreadedExecutor::spin().
|
virtual |
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.
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 136 of file executor.cpp.
|
virtual |
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.
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 161 of file executor.cpp.
|
virtual |
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
.
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 150 of file executor.cpp.
|
protected |
Return true if the node has been added to this executor.
[in] | node_ptr | a shared pointer that points to a node base interface |
[in] | weak_groups_to_nodes | map to nodes to lookup |
Definition at line 929 of file executor.cpp.
Referenced by remove_callback_group_from_map().
bool Executor::is_spinning | ( | ) |
Returns true if the executor is currently spinning.
This function can be called asynchronously from any thread.
Definition at line 944 of file executor.cpp.
References spinning.
|
virtual |
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.
[in] | group_ptr | Shared pointer to the callback group to be added. |
[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. |
std::runtime_error | if node is deleted before callback group |
std::runtime_error | if the callback group is not associated with the executor |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 324 of file executor.cpp.
References remove_callback_group_from_map().
|
protectedvirtual |
Remove a callback group from the executor.
Definition at line 281 of file executor.cpp.
References has_node(), interrupt_guard_condition_, and rclcpp::GuardCondition::trigger().
Referenced by remove_callback_group(), and remove_node().
|
virtual |
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.
[in] | node_ptr | Shared pointer to the node to remove. |
[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. |
std::runtime_error | if the node is not associated with an executor. |
std::runtime_error | if the node is not associated with this executor. |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 342 of file executor.cpp.
References remove_callback_group_from_map().
Referenced by remove_node(), rclcpp::spin(), and spin_node_some().
|
virtual |
Convenience function which takes Node and forwards NodeBaseInterface.
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 386 of file executor.cpp.
References remove_node().
void Executor::set_memory_strategy | ( | memory_strategy::MemoryStrategy::SharedPtr | memory_strategy | ) |
Support dynamic switching of the memory strategy.
Switching the memory strategy while the executor is spinning in another threading could have unintended consequences.
[in] | memory_strategy | Shared pointer to the memory strategy to set. |
std::runtime_error | if memory_strategy is null |
Definition at line 499 of file executor.cpp.
|
virtual |
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.
[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. |
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. |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 421 of file executor.cpp.
|
inline |
Add a node to executor, execute the next available unit of work, and remove the node.
[in] | node | Shared pointer to the node to add. |
[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. |
Definition at line 248 of file executor.hpp.
void Executor::spin_node_some | ( | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr | node | ) |
Add a node, complete all immediately available work, and remove the node.
[in] | node | Shared pointer to the node to add. |
Definition at line 403 of file executor.cpp.
References add_node(), remove_node(), and spin_some().
Referenced by spin_node_some(), and rclcpp::spin_some().
|
virtual |
Collect work once and execute all available work, optionally within a 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 callbacks will cause this function to block (which may have unintended consequences).
[in] | max_duration | The maximum amount of time to spend executing work, or 0 for no limit. Note that spin_some() may take longer than this time as it only returns once max_duration has been exceeded. |
Reimplemented in rclcpp::executors::StaticSingleThreadedExecutor.
Definition at line 416 of file executor.cpp.
Referenced by spin_node_some().
|
inline |
Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
[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). |
[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. |
SUCCESS
, INTERRUPTED
, or TIMEOUT
. Definition at line 334 of file executor.hpp.
|
protected |
std::runtime_error | if the wait set can be cleared |
Definition at line 690 of file executor.cpp.
References add_callback_groups_from_nodes_associated_to_executor(), RCL_RET_OK, rcl_wait_set_clear(), rcl_wait_set_resize(), and wait_set_.