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 conditions
  rclcpp::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 result
  this->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 waitset
  this->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 function
  success = get_next_ready_executable(any_executable);
  // If there are none
  if (!success) {
    // Wait for subscriptions or timers to work on
    wait_for_work(timeout);
    if (!spinning.load()) {
      return false;
    }
    // Try again
    success = 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 result
  this->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_PUBLIC
  bool
  get_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_PUBLIC
  AnyExecutable();

  RCLCPP_PUBLIC
  virtual ~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 items
  rclcpp::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 type
  any_exec.callback_group->can_be_taken_from().store(true);
}

参考资料

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