Robot Operating System——深度解析单线程执行器（SingleThreadedExecutor）执行逻辑

在ROS2中，我们设置的各种回调都是在执行器（Executor）中执行的，所以它是整个系统非常核心的组件。

目前，rclcpp 提供了三种 Executor 类型，它们派生自同一个父类Executor。

本文我们将借用《Robot Operating System——单线程中启动多个Node》中的例子，将单线程执行器在最上层的使用方法总结如下

  // Create an executor that will be responsible for execution of callbacks for a set of nodes.// With this version, all callbacks will be called from within this thread (the main one).rclcpp::executors::SingleThreadedExecutor exec;rclcpp::NodeOptions options;// Add some nodes to the executor which provide work for the executor during its "spin" function.// An example of available work is executing a subscription callback, or a timer callback.auto server = std::make_shared<composition::Server>(options);exec.add_node(server);// spin will block until work comes in, execute work as it becomes available, and keep blocking.// It will only be interrupted by Ctrl-C.exec.spin();

即：

1. 创建SingleThreadedExecutor ；
2. 新增Node;
3. 自旋等待。

创建SingleThreadedExecutor

SingleThreadedExecutor的构造函数基本就是交给基类rclcpp::Executor的构造函数来实现。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::ExecutorOptions & options)
: rclcpp::Executor(options) {}

在Executor的构造函数中，我们着重关注成员变量collector_。

Executor::Executor(const rclcpp::ExecutorOptions & options)
: spinning(false),interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),context_(options.context),notify_waitable_(std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>([this]() {this->entities_need_rebuild_.store(true);})),entities_need_rebuild_(true),collector_(notify_waitable_),wait_set_({}, {}, {}, {}, {}, {}, options.context),current_notify_waitable_(notify_waitable_),impl_(std::make_unique<rclcpp::ExecutorImplementation>())
{shutdown_callback_handle_ = context_->add_on_shutdown_callback([weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {auto strong_gc = weak_gc.lock();if (strong_gc) {strong_gc->trigger();}});notify_waitable_->add_guard_condition(interrupt_guard_condition_);notify_waitable_->add_guard_condition(shutdown_guard_condition_);wait_set_.add_waitable(notify_waitable_);
}

collector_是一个集合，它保存了后续要执行的各个Node指针。

/// Collector used to associate executable entities from nodes and guard conditionsrclcpp::executors::ExecutorEntitiesCollector collector_;

新增Node

add_node

业务逻辑的Node会被添加到上面介绍的成员变量collector_中。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
void
Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
{this->collector_.add_node(node_ptr);try {this->trigger_entity_recollect(notify);} catch (const rclcpp::exceptions::RCLError & ex) {throw std::runtime_error(std::string("Failed to trigger guard condition on node add: ") + ex.what());}
}

然后会调用trigger_entity_recollect方法。

trigger_entity_recollect

这个方法会做两件事：

1. 修改std::atomic_bool类型变量entities_need_rebuild_的值为true，进而让collect_entities()被执行。
2. 如果notify为true，则会通过interrupt_guard_condition_->trigger()唤醒一个处于等待状态的执行器。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
void
Executor::trigger_entity_recollect(bool notify)
{this->entities_need_rebuild_.store(true);if (!spinning.load() && entities_need_rebuild_.exchange(false)) {std::lock_guard<std::mutex> guard(mutex_);this->collect_entities();}if (notify) {interrupt_guard_condition_->trigger();}
}

collect_entities

collect_entities主要做两件事：

1. 过滤过期的Node以及相关回调函数。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
void
Executor::collect_entities()
{// Updating the entity collection and waitset expires any active resultthis->wait_result_.reset();// Get the current list of available waitables from the collector.rclcpp::executors::ExecutorEntitiesCollection collection;this->collector_.update_collections();auto callback_groups = this->collector_.get_all_callback_groups();rclcpp::executors::build_entities_collection(callback_groups, collection);// We must remove expired entities here, so that we don't continue to use older entities.// See https://github.com/ros2/rclcpp/issues/2180 for more information.current_collection_.remove_expired_entities();

2. 更新current_collection_和wait_set_。

  // Update each of the groups of entities in the current collection, adding or removing// from the wait set as necessary.current_collection_.timers.update(collection.timers,[this](auto timer) {wait_set_.add_timer(timer);},[this](auto timer) {wait_set_.remove_timer(timer);});current_collection_.subscriptions.update(collection.subscriptions,[this](auto subscription) {wait_set_.add_subscription(subscription, kDefaultSubscriptionMask);},[this](auto subscription) {wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask);});current_collection_.clients.update(collection.clients,[this](auto client) {wait_set_.add_client(client);},[this](auto client) {wait_set_.remove_client(client);});current_collection_.services.update(collection.services,[this](auto service) {wait_set_.add_service(service);},[this](auto service) {wait_set_.remove_service(service);});current_collection_.guard_conditions.update(collection.guard_conditions,[this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);},[this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);});current_collection_.waitables.update(collection.waitables,[this](auto waitable) {wait_set_.add_waitable(waitable);},[this](auto waitable) {wait_set_.remove_waitable(waitable);});// In the case that an entity already has an expired weak pointer// before being removed from the waitset, additionally prune the waitset.this->wait_set_.prune_deleted_entities();
}

自旋等待

spin()内部核心是一个while循环。它会不停使用get_next_executable取出可以运行的Node的回调，然后让execute_any_executable将其执行。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
void
SingleThreadedExecutor::spin()
{if (spinning.exchange(true)) {throw std::runtime_error("spin() called while already spinning");}RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););// Clear any previous result and rebuild the waitsetthis->wait_result_.reset();this->entities_need_rebuild_ = true;while (rclcpp::ok(this->context_) && spinning.load()) {rclcpp::AnyExecutable any_executable;if (get_next_executable(any_executable)) {execute_any_executable(any_executable);}}
}

那么这个while循环会不会导致CPU一直空转呢？答案是：不是。我们可以看get_next_executable的实现。

get_next_executable

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
bool
Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
{bool success = false;// Check to see if there are any subscriptions or timers needing service// TODO(wjwwood): improve run to run efficiency of this functionsuccess = get_next_ready_executable(any_executable);// If there are noneif (!success) {// Wait for subscriptions or timers to work onwait_for_work(timeout);if (!spinning.load()) {return false;}// Try againsuccess = get_next_ready_executable(any_executable);}return success;
}

它会在底层调用wait_for_work。

wait_for_work

这个方法会一直阻塞到时间超时，或者有回调函数可以被调用。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
void
Executor::wait_for_work(std::chrono::nanoseconds timeout)
{TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());// Clear any previous wait resultthis->wait_result_.reset();{std::lock_guard<std::mutex> guard(mutex_);if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {this->collect_entities();}}this->wait_result_.emplace(wait_set_.wait(timeout));if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) {RCUTILS_LOG_WARN_NAMED("rclcpp","empty wait set received in wait(). This should never happen.");} else {if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) {auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set();if (current_notify_waitable_->is_ready(rcl_wait_set)) {current_notify_waitable_->execute(current_notify_waitable_->take_data());}}}
}

其中主要负责等待的是这句

wait_set_.wait(timeout)

在SingleThreadedExecutor中，由于调用get_next_executable没有传递时间，便采用了默认时间。这样get_next_executable会一直等到有回调可以被执行。这样就避免了CPU空转的问题。

  /// 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_PUBLICboolget_next_executable(AnyExecutable & any_executable,std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));

get_next_ready_executable

get_next_ready_executable会按顺序寻找Timer、Subscription、Service 、Client和Waitable中第一个处于可被回调状态的Node。

Timer

bool valid_executable = false;if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) {return false;}if (!valid_executable) {size_t current_timer_index = 0;while (true) {auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index);if (nullptr == timer) {break;}current_timer_index = timer_index;auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());if (entity_iter != current_collection_.timers.end()) {auto callback_group = entity_iter->second.callback_group.lock();if (!callback_group || !callback_group->can_be_taken_from()) {current_timer_index++;continue;}// At this point the timer is either ready for execution or was perhaps// it was canceled, based on the result of call(), but either way it// should not be checked again from peek_next_ready_timer(), so clear// it from the wait result.wait_result_->clear_timer_with_index(current_timer_index);// Check that the timer should be called still, i.e. it wasn't canceled.any_executable.data = timer->call();if (!any_executable.data) {current_timer_index++;continue;}any_executable.timer = timer;any_executable.callback_group = callback_group;valid_executable = true;break;}current_timer_index++;}}

Subscription

  if (!valid_executable) {while (auto subscription = wait_result_->next_ready_subscription()) {auto entity_iter = current_collection_.subscriptions.find(subscription->get_subscription_handle().get());if (entity_iter != current_collection_.subscriptions.end()) {auto callback_group = entity_iter->second.callback_group.lock();if (!callback_group || !callback_group->can_be_taken_from()) {continue;}any_executable.subscription = subscription;any_executable.callback_group = callback_group;valid_executable = true;break;}}}

Service

  if (!valid_executable) {while (auto service = wait_result_->next_ready_service()) {auto entity_iter = current_collection_.services.find(service->get_service_handle().get());if (entity_iter != current_collection_.services.end()) {auto callback_group = entity_iter->second.callback_group.lock();if (!callback_group || !callback_group->can_be_taken_from()) {continue;}any_executable.service = service;any_executable.callback_group = callback_group;valid_executable = true;break;}}}

Client

  if (!valid_executable) {while (auto client = wait_result_->next_ready_client()) {auto entity_iter = current_collection_.clients.find(client->get_client_handle().get());if (entity_iter != current_collection_.clients.end()) {auto callback_group = entity_iter->second.callback_group.lock();if (!callback_group || !callback_group->can_be_taken_from()) {continue;}any_executable.client = client;any_executable.callback_group = callback_group;valid_executable = true;break;}}}

Waitable

  if (!valid_executable) {while (auto waitable = wait_result_->next_ready_waitable()) {auto entity_iter = current_collection_.waitables.find(waitable.get());if (entity_iter != current_collection_.waitables.end()) {auto callback_group = entity_iter->second.callback_group.lock();if (!callback_group || !callback_group->can_be_taken_from()) {continue;}any_executable.waitable = waitable;any_executable.callback_group = callback_group;any_executable.data = waitable->take_data();valid_executable = true;break;}}}

AnyExecutable

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/include/rclcpp/any_executable.hpp
struct AnyExecutable
{RCLCPP_PUBLICAnyExecutable();RCLCPP_PUBLICvirtual ~AnyExecutable();// Only one of the following pointers will be set.rclcpp::SubscriptionBase::SharedPtr subscription;rclcpp::TimerBase::SharedPtr timer;rclcpp::ServiceBase::SharedPtr service;rclcpp::ClientBase::SharedPtr client;rclcpp::Waitable::SharedPtr waitable;// These are used to keep the scope on the containing itemsrclcpp::CallbackGroup::SharedPtr callback_group {nullptr};rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr};std::shared_ptr<void> data {nullptr};
};

execute_any_executable

找到可以执行的Node后，便可以调用execute_any_executable让其执行。

在execute_any_executable内部，我们看到它也是区分Timer、Subscription、Service 、Client和Waitable类型来执行的。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/src/rclcpp/executor.cpp
void
Executor::execute_any_executable(AnyExecutable & any_exec)
{if (!spinning.load()) {return;}assert((void("cannot execute an AnyExecutable without a valid callback group"),any_exec.callback_group));if (any_exec.timer) {TRACETOOLS_TRACEPOINT(rclcpp_executor_execute,static_cast<const void *>(any_exec.timer->get_timer_handle().get()));execute_timer(any_exec.timer, any_exec.data);}if (any_exec.subscription) {TRACETOOLS_TRACEPOINT(rclcpp_executor_execute,static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));execute_subscription(any_exec.subscription);}if (any_exec.service) {execute_service(any_exec.service);}if (any_exec.client) {execute_client(any_exec.client);}if (any_exec.waitable) {const std::shared_ptr<void> & const_data = any_exec.data;any_exec.waitable->execute(const_data);}// Reset the callback_group, regardless of typeany_exec.callback_group->can_be_taken_from().store(true);
}

参考资料

• https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Executors.html#executors