ROS2探索（二）executor

前言

在ROS2探索（一）Publisher-Subscriber的内部过程 我们接触了executor这个新东西，发现spin函数其实调用的就是executor的接口，一个executor可以通过add_node来添加多个节点，然后不断循环执行准备好了的AnyExecutable对象。
本文围绕ros2源码中的executors示例，学习下多线程executor，进一步分析executor处理subscription的流程。

正文

打开ros2/examples/rclcpp/executors/下的multithreaded_executor.cpp，直奔main函数：

int main(int argc, char * argv[])
{
     
  rclcpp::init(argc, argv);
  rclcpp::executors::MultiThreadedExecutor executor;
  auto pubnode = std::make_shared<PublisherNode>();
  auto subnode = std::make_shared<DualThreadedNode>();
  executor.add_node(pubnode);
  executor.add_node(subnode);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}

这里构造了两个节点，PublisherNode和DualThreadedNode，其中DualThreadedNode从名字来看拥有两个线程用来监听话题消息。多线程的节点必须要用MultiThreadedExecutor，这是和之前的不一样的地方。
接下来示例给出了一个进程拥有多个节点的例子，只需要调用add_node添加这些节点即可，添加完了开始spin，不过这里不用给定节点指针对象，因为直接调用的executor.spin：

  executor.add_node(pubnode);
  executor.add_node(subnode);
  executor.spin();

PublisherNode没什么好说的，和前一篇文章中基本一样。直接看DualThreadedNode：

  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_;
  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription1_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription2_;

DualThreadedNode拥有两个Subscription成员用于监听同一个话题以及Subscription对应的回调组（CallbackGroup）。
回调组通过Node::create_callback_group进行创建：

callback_group_subscriber1_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_subscriber2_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

参数MutuallyExclusive表示此组的回调函数是互斥的，一次只能同时执行一个。如果配置成CallbackGroupType::Reentrant则同时可以执行多个。
接着创建SubscriptionOptions并设置所属回调组：

    auto sub1_opt = rclcpp::SubscriptionOptions();
    sub1_opt.callback_group = callback_group_subscriber1_;
    auto sub2_opt = rclcpp::SubscriptionOptions()
    sub2_opt.callback_group = callback_group_subscriber2_;

然后用这些Options创建Subscription，这里没有用默认参数而是传入了带回调组的SubscriptionOptions：

   subscription1_ = this->create_subscription<std_msgs::msg::String>(
      "topic",
      rclcpp::QoS(10),
      std::bind(&DualThreadedNode::subscriber1_cb,std::placeholders::_1),
      sub1_opt);

回调函数与之前的lambda表达式不同，这里通过std::bind绑定了成员函数作为作为回调函数，占位符表示回调函数待定的输入。
我们现在重点来看一下rclcpp::executors::MultiThreadedExecutor对象。首先多线程executor的spin函数如下：

void MultiThreadedExecutor::spin()
{
     
...
  std::vector<std::thread> threads;
  size_t thread_id = 0;
  {
     
    std::lock_guard<std::mutex> wait_lock(wait_mutex_);
    for (; thread_id < number_of_threads_ - 1; ++thread_id)
    {
     
      auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
      threads.emplace_back(func);
    }
  }
  run(thread_id);
  for (auto &thread : threads)
  {
     
    thread.join();
  }
}

spin函数中首先根据number_of_threads_创建该数量个std::thread，每个thread绑定run成员函数。run函数与单线程的spin类似，其中增加了线程安全和线程定时器操作。
number_of_threads由用户指定，默认为std::thread::hardware_concurrency()即cpu的核心数，我的输出是8个线程：
当使用8个线程时，ros2 run examples_rclcpp_multithreaded_executor multithreaded_executor执行后输出如下：
可以看出节点线程ID不固定，如果将MultiThreadedExecutor的线程数目改为1` ：

rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(),1);

得到的输出如下：发现此时三个节点共用了同一个线程。
至此得出以下结论：

• executor有MultiThreadedExecutor和SingleThreadedExecutor两种，MultiThreadedExecutor默认开启CPU核心数个线程
• 每个线程独立地检查executor中已添加节点是否有AnyExecutable，其既可以是publish也可以是subscription
  分析完多线程executor的线程创建，我们来看一下executor是执行订阅消息回调的过程。
  首先Executor开始spin后每个线程不断循环三个步骤，即下图所示：

其中get_next_executable()调用get_next_subscription()来检查当前是否收到消息

		void get_next_subscription(rclcpp::AnyExecutable &any_exec,
                                   const WeakNodeList &weak_nodes) override
        {
     
          auto it = subscription_handles_.begin();
          while (it != subscription_handles_.end())
          {
     
            auto subscription = get_subscription_by_handle(*it, weak_nodes);
            if (subscription)
            {
     
              // Find the group for this handle and see if it can be serviced
              auto group = get_group_by_subscription(subscription, weak_nodes);
              if (!group)
              {
     
                // Group was not found, meaning the subscription is not valid...
                // Remove it from the ready list and continue looking
                it = subscription_handles_.erase(it);
                continue;
              }
              if (!group->can_be_taken_from().load())
              {
     
                // Group is mutually exclusive and is being used, so skip it for now
                // Leave it to be checked next time, but continue searching
                ++it;
                continue;
              }
              // Otherwise it is safe to set and return the any_exec
              any_exec.subscription = subscription;
              any_exec.callback_group = group;
              any_exec.node_base = get_node_by_group(group, weak_nodes);
              subscription_handles_.erase(it);
              return;
            }
            // Else, the subscription is no longer valid, remove it and continue
            it = subscription_handles_.erase(it);
          }
        }

get_next_subscription遍历当前节点的所有rcl_subscription_t句柄，对每个rcl_subscription_t句柄检查其所在回调组是否为MutuallyExclusive，如果是MutuallyExclusive互斥组那么还得确保当前回调没有其他线程正在调用。
得到一个包含subscription的AnyExecutable对象之后，我们执行execute_any_executable中的execute_subscription函数，这个函数很长而且不太容易理解，挑出了关键部分如下：

void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
{
     
  rclcpp::MessageInfo message_info;
  message_info.get_rmw_message_info().from_intra_process = false;

  if (subscription->is_serialized()) //case 1
  {
     
  	// This is the case where a copy of the serialized message is taken from
    // the middleware via inter-process communication.
    ...
  }
  else if (subscription->can_loan_messages()) //case 2
  {
     
    // This is the case where a loaned message is taken from the middleware via
    // inter-process communication, given to the user for their callback,
    // and then returned.
    ...
  }
  else //case 3
  {
     
    // This case is taking a copy of the message data from the middleware via
    // inter-process communication.
    std::shared_ptr<void> message = subscription->create_message();
    take_and_do_error_handling(
        "taking a message from topic",
        subscription->get_topic_name(),
        [&]() {
      return subscription->take_type_erased(message.get(), message_info); },
        [&]() {
      subscription->handle_message(message, message_info); });
    subscription->return_message(message);
  }
}

execute_subscription有三个分支，他们都是处理经过中间件的消息数据，主要的区别在于：

• case1使用的是序列化的数据，这种消息的header和消息payload是手工设置的，好比TCP定制报文
• case2是向中间件借用消息（loan message），采用的共享内存来避免拷贝，性能不错但需要DDS中间件的支持。这部分先关注，后面文章再谈，先挖好坑。
• case3最简单，直接将消息拷贝一份传给回调函数

我们现在只分析下case3，case3里第一步开辟消息内存：

std::shared_ptr<void> message = subscription->create_message();

然后Subscription对象操作rcl句柄：

rcl_ret_t ret = rcl_take(
    this->get_subscription_handle().get(),
    message_out,
    &message_info_out.get_rmw_message_info(),
    nullptr  // rmw_subscription_allocation_t is unused here
  );

rcl句柄再往下获取rmw句柄的消息：

rmw_ret_t ret = rmw_take_with_info(
    subscription->impl->rmw_handle, ros_message, &taken, message_info_local, allocation);

如果rmw_take_with_info返回RMW_RET_OK的话表示rmw有消息，则进行handle_message步骤。
handle_message首先根据模板参数CallbackMessageT将返回数据转换成对应类型：

auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);

然后将带类型的消息数据分发给回调函数进行处理：

any_callback_.dispatch(typed_message, message_info);

完整的handle_message代码如下：

    void handle_message(std::shared_ptr<void> &message,
                        const rclcpp::MessageInfo &message_info) override
    {
     
      if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid))
      {
     
        // In this case, the message will be delivered via intra process and
        // we should ignore this copy of the message.
        return;
      }
      auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
      any_callback_.dispatch(typed_message, message_info);

      if (subscription_topic_statistics_)
      {
     
        const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(
            std::chrono::system_clock::now());
        const auto time = rclcpp::Time(nanos.time_since_epoch().count());
        subscription_topic_statistics_->handle_message(*typed_message, time);
      }
    }

下面画图总结一下executor的使用与Subscription处理的流程：

总结

本文到目前了解了ROS2的以下内容：

• executor多线程的实现
• 多线程executor默认开启cpu核心数个线程
• CallbackGroup实现了对回调函数的互斥与重入调用
• execute_subscription对于中间件消息有三种处理类型：serialized message、loaded message以及普通的拷贝消息
• executor处理subscription回调的流程
  下一篇将分析ros2中的service实现。