ROS2探索（五）intra-process的内部原理

前言

最近一段时间发生不少事情但是一直没有忘记ROS2。
前面介绍了节点基于DDS进行数据传输，Participant收到CacheChange后会将它复制一份到自己的History中，这些Participant不要求在同一个进程或者主机上。然而对于一些涉及到图片和点云的应用中，往往会将不同的处理步骤解耦成若干个节点，比如激光雷达节点只负责发布点云，滤波节点订阅原始点云然后做预处理再发布出去；摄像头节点负责发布图像数据，显示模块订阅后进行界面显示。
ROS2中提供了intra-process机制使得在同一个进程中的节点间减少消息的拷贝开销，保持模块化同时提高了性能。在ros2 foxy源码中有intra-process的demo示例，本文从这些demo入手分析下其内部的实现原理。实验中使用的测试视频来自网上，不接收律师函…

实验

关于intra-process的demo代码在ros2/demos/intra_process_demo文件夹下，文件结构如下：

:~/ros2_foxy/src/ros2/demos/intra_process_demo$ tree
.
├── CHANGELOG.rst
├── CMakeLists.txt
├── include
│ └── image_pipeline
│ ├── camera_node.hpp
│ ├── common.hpp
│ ├── image_view_node.hpp
│ └── watermark_node.hpp
├── package.xml
├── src
│ ├── cyclic_pipeline
│ │ └── cyclic_pipeline.cpp
│ ├── image_pipeline
│ │ ├── camera_node.cpp
│ │ ├── image_pipeline_all_in_one.cpp
│ │ ├── image_pipeline_with_two_image_view.cpp
│ │ ├── image_view_node.cpp
│ │ └── watermark_node.cpp
│ └── two_node_pipeline
│ └── two_node_pipeline.cpp
└── test

我们主要看一下image_pipeline的部分，它包含三个功能节点：camera_node、watermark_node和image_view_node，分别实现了原始图片的发布，添加水印以及图片展示三个功能。
demo展示了三个使用场景：

三个节点各自为一个进程
三个节点都在一个进程中
一个进程中有一个camera_node、一个watermark_node和两个image_view_node

原始的demo代码camera_node使用OpenCV打开摄像头获取图像，这里我没有摄像头所以祭出了我珍藏的视频，代码改动如下：

/**42行构造函数里修改VideoCapturer打开视频文件**///cap_.open(device);cap_.open("/home/wenhui/cxk.mp4");/**loop函数开头添加3行**/double rate = cap_.get(cv::CAP_PROP_FPS); int delay = 1000000/rate;int cnt = 0;/**while循环里添加显示更多的信息**/ss << "CameraNode,pid: " << GETPID() << ", ptr: " << msg.get()<<", frame id: "<<cnt;/**loop函数中while结尾添加sleep控制帧率**/usleep(delay);cnt++;

修改完毕后进行编译，使用–packages-select参数只编译一个包：

colcon build --packages-select intra_process_demo

编译完毕后起三个终端依次分别执行：

终端1：ros2 run intra_process_demo image_view_node
终端2：ros2 run intra_process_demo watermark_node
终端3：ros2 run intra_process_demo camera_node

当终端3启动后出现视频窗口，按空格键可以暂停画面，可以看到：

左上角显示三个节点依次打印的信息，他们的pid不一样，各自节点内图片消息的指针ptr指向位置也不一样，可以看出发生了图像拷贝。
关闭三个终端，新起一个终端执行：

ros2 run intra_process_demo image_pipeline_all_in_one

按空格键暂停画面后得到：

三个节点打印的pid一致，各自的图像指针指向位置也完全一致，没有发生图片的拷贝。
重开一个终端执行：

ros2 run intra_process_demo image_pipeline_with_two_image_view

按空格得到效果：
两个窗口对应两个view_node，两幅图片中区别在于view_node中图像地址不同且frame id也有差别，water-marknode的图像地址与camera_node一致但是两个imageview中的图像地址一个与前面相同但另一个不同，两个viewnode中具体哪一个不同具有随机性。

实验现象总结一下：

intra_process可以实现在同一个进程中不同node间的消息0拷贝
intra_process对于不同进程间的node无法实现0拷贝（废话…那样就是inter_process…）
如果多个消费者同时需要上一层生产者的消息时，同一个消息只能以0拷贝方式传给其中一个消费者，其他的只能拷贝过去

代码分析

上面的实验使我们对intra_process有了大致的轮廓，下面从实验代码为入口扒一下intra_process的运作过程。

demo代码

首先看下camera_node的构造函数：

  CameraNode(const std::string & output, const std::string & node_name = "camera_node",bool watermark = true, int device = 0, int width = 320, int height = 240): Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)),canceled_(false), watermark_(watermark){...}

可以看到CameraNode继承自Node且设置了use_intra_process_comms为true，这样可以使用intra_process机制。

Camera_node的核心部分：

   while (rclcpp::ok() && !canceled_.load()) {// Capture a frame from OpenCV.cap_ >> frame_;if (frame_.empty()) {continue;}// Create a new unique_ptr to an Image message for storage.sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());if (watermark_) {std::stringstream ss;// Put this process's id and the msg's pointer address on the image.ss << "CameraNode,pid: " << GETPID() << ", ptr: " << msg.get()<<", frame id: "<<cnt;draw_on_image(frame_, ss.str(), 20);}// Pack the OpenCV image into the ROS image.set_now(msg->header.stamp);msg->header.frame_id = "camera_frame";msg->height = frame_.rows;msg->width = frame_.cols;msg->encoding = mat_type2encoding(frame_.type());msg->is_bigendian = false;msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);msg->data.assign(frame_.datastart, frame_.dataend);pub_->publish(std::move(msg));  // Publish.cnt++;std::this_thread::sleep_for(std::chrono::milliseconds(delay));}

while循环中通过cv::VideoCapture读取视频帧并构造sensor_msgs::msg::Image消息msg发布出去。注意这里的msg是一个智能指针sensor_msgs::msg::Image::UniquePtr，它的原型是STL的std::unique_ptr:

using UniquePtrWithDeleter = std::unique_ptr<sensor_msgs::msg::Image_, Deleter>;
using UniquePtr = UniquePtrWithDeleter<>;

watermark_node的关键代码如下：

  WatermarkNode(const std::string & input, const std::string & output, const std::string & text,const std::string & node_name = "watermark_node"): Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)){rclcpp::SensorDataQoS qos;// Create a publisher on the input topic.pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, qos);std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;// Create a subscription on the output topic.sub_ = this->create_subscription<sensor_msgs::msg::Image>(input,qos,[captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) {auto pub_ptr = captured_pub.lock();if (!pub_ptr) {return;}// Create a cv::Mat from the image message (without copying).cv::Mat cv_mat(msg->height, msg->width,encoding2mat_type(msg->encoding),msg->data.data());// Annotate the image with the pid, pointer address, and the watermark text.std::stringstream ss;ss << "WatermarkNode,pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;draw_on_image(cv_mat, ss.str(), 40);pub_ptr->publish(std::move(msg));  // Publish it along.});}

watermark_node同样设置了use_intra_process_comms，接着创建了sub_用来订阅上面camera_node发布的图片，订阅的回调函数是一个lambda函数，其绑定了captured_pub和text，回调函数的参数类型为sensor_msgs::msg::Image::UniquePtr，经过draw_on_image处理后的图片UniquePtr通过std::move转成右值并pub出去。
最后看下ImageViewNode：

  explicit ImageViewNode(const std::string & input, const std::string & node_name = "image_view_node",bool watermark = true): Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)){// Create a subscription on the input topic.sub_ = this->create_subscription<sensor_msgs::msg::Image>(input,rclcpp::SensorDataQoS(),[node_name, watermark](const sensor_msgs::msg::Image::SharedPtr msg) {// Create a cv::Mat from the image message (without copying).cv::Mat cv_mat(msg->height, msg->width,encoding2mat_type(msg->encoding),msg->data.data());if (watermark) {// Annotate with the pid and pointer address.std::stringstream ss;ss << "ImageViewNode,pid: " << GETPID() << ", ptr: " << msg.get();draw_on_image(cv_mat, ss.str(), 60);}// Show the image.cv::Mat c_mat = cv_mat;cv::imshow(node_name.c_str(), c_mat);char key = cv::waitKey(1);    // Look for key presses.if (key == 27 /* ESC */ || key == 'q') {rclcpp::shutdown();}...});}

use_intra_process_comms设置为true，create_subscription创建订阅关系并传入回调lambda函数。
关键的来了： 回调lambda中参数类型是const sensor_msgs::msg::Image::SharedPtr，它和watermark_node的回调参数类型不同。前面看到watermark_node中pub的数据是UniquePtr（通过std::move），而这里被转成了SharedPtr。

到这里Demo中主要的部分了解了，可以看出三个功能node都一开始设置了use_intra_process_comms，然后消息node订阅和发布的msg通过UniquePtr或者SharedPtr进行管理。因此我们先看一下use_intra_process_comms做了什么。

IntraProcessManager引入

我们以watermark_node为例，构造函数中设置实例化了Node基类且传入use_intra_process_comms为true的option，这样就设置NodeBase::use_intra_process_default_成员变量为true；后续可以通过rclcpp::detail::resolve_use_intra_process函数来判断某个node是否启用intra-process。
接下来从publisher和subscriber两个方向观察use_intra_process_default_带来了哪些不同。
在ROS2探索（一）Publisher-Subscriber的内部过程里面publisher在创建的过程中调用了post_init_setup函数，当时没有具体介绍其作用，它的实现如下：

virtual void post_init_setup(rclcpp::node_interfaces::NodeBaseInterface * node_base,const std::string & topic,const rclcpp::QoS & qos,const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options){// Topic is unused for now.(void)topic;(void)options;// If needed, setup intra process communication.if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {auto context = node_base->get_context();// Get the intra process manager instance for this context.auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();// Register the publisher with the intra process manager....uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());this->setup_intra_process(intra_process_publisher_id,ipm);}}

可以看到一个启用intra-process的publisher需要IntraProcessManager这个东西，IntraProcessManager可以通过add_publisher来添加相关的publisher，添加后IntraProcessManager会返回一个uint64_t类型的标识ID；然后通过setup_intra_process将创建好的publisher对象与IntraProcessManager进行关联。
对于subscriber这边，回顾下Subscription的构造函数，摘要如下：

    Subscription(...): SubscriptionBase(node_base,type_support_handle,topic_name,options.template to_rcl_subscription_options<CallbackMessageT>(qos),rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),any_callback_(callback),options_(options),message_memory_strategy_(message_memory_strategy){...// Setup intra process publishing if requested.if (rclcpp::detail::resolve_use_intra_process(options, *node_base)){using rclcpp::detail::resolve_intra_process_buffer_type;// Check if the QoS is compatible with intra-process....// First create a SubscriptionIntraProcess which will be given to the intra-process manager.auto context = node_base->get_context();using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<CallbackMessageT,AllocatorT,typename MessageUniquePtr::deleter_type>;auto subscription_intra_process = std::make_shared<SubscriptionIntraProcessT>(callback,options.get_allocator(),context,this->get_topic_name(), // important to get like this, as it has the fully-qualified nameqos_profile,resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));...// Add it to the intra process manager.using rclcpp::experimental::IntraProcessManager;auto ipm = context->get_sub_context<IntraProcessManager>();uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process);this->setup_intra_process(intra_process_subscription_id, ipm);}...}

构造函数中通过resolve_use_intra_process辅助函数检查是否开启intra-process，如果开启则获取IntraProcessManager然后add_subscription，注意这里使用了SubscriptionIntraProcess类型作为AnyExecutable内容来供Executor执行，SubscriptionIntraProcess与普通的Subscription不同，它继承自rclcpp::Waitable

class SubscriptionIntraProcessBase : public rclcpp::Waitable

最后一步和publisher一样也是使用setup_intra_process关联该subscription与IntraProcessManager。

总结一下，当node被设置为use_intra_process后，该node创建subsciber或者pubscriber时会使用IntraProcessManager，IntraProcessManager初始化的步骤就是：

get，获取IntraProcessManager实例
add，向IntraProcessManager添加publisher或者subscription，注意订阅使用的是waitable类型的SubscriptionIntraProcess
set，使用setup_intra_process将publisher或者SubscriptionIntraProcess与IntraProcessManager关联

IntraProcessManager做了什么

前面引入了IntraProcessManager这个东西，然后看到如果开启了intra-process那么publisher和subscription创建后需要与IntraProcessManager进行关联。接下来分析一下IntraProcessManager在消息传送中具体做了哪些工作。
首先看publish函数，demo代码中使用的是unique_ptr参数，实现如下：

  virtual voidpublish(std::unique_ptr<MessageT, MessageDeleter> msg){if (!intra_process_is_enabled_) {this->do_inter_process_publish(*msg);return;}//bool inter_process_publish_needed =get_subscription_count() > get_intra_process_subscription_count();if (inter_process_publish_needed) {auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));this->do_inter_process_publish(*shared_msg);} else {this->do_intra_process_publish(std::move(msg));}}

接着publisher的do_intra_process_publish中调用了IntraProcessManager的do_intra_process_publish接口：

  voiddo_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg){...ipm->template do_intra_process_publish<MessageT, AllocatorT>(intra_process_publisher_id_,std::move(msg),message_allocator_);}

现在来看IntraProcessManager的do_intra_process_publish实现：

  template<typename MessageT,typename Alloc = std::allocator<void>,typename Deleter = std::default_delete<MessageT>>void do_intra_process_publish(uint64_t intra_process_publisher_id,std::unique_ptr<MessageT, Deleter> message,std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator);

首先IntraProcessManager检查publisher是否有对应的subscription：

    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);if (publisher_it == pub_to_subs_.end()) {// Publisher is either invalid or no longer exists.RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Calling do_intra_process_publish for invalid or no longer existing publisher id");return;}

这里体现了IntraProcessManager的一个职责就是维护订阅和发布关系，主要通过保存三个unordered_map来实现：

 struct SplittedSubscriptions{std::vector<uint64_t> take_shared_subscriptions;std::vector<uint64_t> take_ownership_subscriptions;};using SubscriptionMap =std::unordered_map<uint64_t, SubscriptionInfo>;using PublisherMap =std::unordered_map<uint64_t, PublisherInfo>;using PublisherToSubscriptionIdsMap =std::unordered_map<uint64_t, SplittedSubscriptions>;PublisherToSubscriptionIdsMap pub_to_subs_;SubscriptionMap subscriptions_;PublisherMap publishers_;

pub_to_subs_储存了一个publisher被哪些subscription订阅，SplittedSubscriptions包含两个数组，分别表示可共享订阅和独占订阅。每个publisher和subscription由IntraProcessManager分配一个uint64_t唯一标识。

接下来，IntraProcessManager将publisher发布的消息分别传送到SubscriptionIntraProcess的buffer_成员中。上面看到subscription由共享和独占两种，IntraProcessManager做了三种处理方法：

publisher所有subscription都是共享的。直接将消息从unique_ptr提升为shared_ptr，用add_shared_msg_to_buffers分发数据
subscription都是独占的，或者只有一个是共享的。等价于所有subscription都是独占，用add_owned_msg_to_buffers分发数据
既有独占又有共享且数量都不止一个。先将消息拷贝，分发给共享subscription，然后再分发给独享subscription。

上面演示的demo中所有的subscription都是独占的，我当时感到非常奇怪，明明view_node采用了sharedPtr参数回调呀。。。后来仔细看了才发现在创建subscription时，resolve_intra_process_buffer_type里如果IntraProcessBufferType是CallbackDefault，则根据回调函数的参数类型进行解析。但是AnySubscription里面的use_take_shared_method判断依据是：
return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_;
但view_node的回调函数参数类型是SharedPtrCallback，不是ConstSharedPtrCallback，也不是ConstSharedPtrWithInfoCallback！
综上，view_node所创建的subscription是独占的（take_ownership）

add_shared_msg_to_buffers的实现如下：

  template<typename MessageT>voidadd_shared_msg_to_buffers(std::shared_ptr<const MessageT> message,std::vector<uint64_t> subscription_ids){for (auto id : subscription_ids) {auto subscription_it = subscriptions_.find(id);if (subscription_it == subscriptions_.end()) {throw std::runtime_error("subscription has unexpectedly gone out of scope");}auto subscription_base = subscription_it->second.subscription;auto subscription = std::static_pointer_cast<rclcpp::experimental::SubscriptionIntraProcess<MessageT>>(subscription_base);subscription->provide_intra_process_message(message);}}

比较简单，就是遍历subscription然后调用subscription的provide_intra_process_message接口。
provide_intra_process_message则将数据传到相应的buffer里，并设置waitable监听的ready条件供executor查询。
add_owned_msg_to_buffers与add_shared_msg_to_buffers类似，不过对于前面n-1个subscription做数据拷贝，最后一个不拷贝：

      if (std::next(it) == subscription_ids.end()) {// If this is the last subscription, give up ownershipsubscription->provide_intra_process_message(std::move(message));} else {// Copy the message since we have additional subscriptions to serveMessageUniquePtr copy_message;Deleter deleter = message.get_deleter();auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);MessageAllocTraits::construct(*allocator.get(), ptr, *message);copy_message = MessageUniquePtr(ptr, deleter);subscription->provide_intra_process_message(std::move(copy_message));}

总结一下，IntraProcessManager做了下面的几件事:

为需要intra-process通信的publisher和subscription分配标识符
维护上面publisher和subscription之间的通信映射关系
根据subscription的回调类型决定如何分发消息，一对一不拷贝，一对多会自动拷贝n-1个

Intra-process回调的执行过程

目光转到subscription这边。ROS2探索（二）executor 中介绍了executor处理订阅的过程，其核心是Executor::execute_any_executable函数，intra-process的订阅是一个SubscriptionIntraProcess（即waitable），每当SubscriptionIntraProcess的buffer被塞入数据，它就会变成ready，exetutor就会调用：

  if (any_exec.waitable){any_exec.waitable->execute();}

execute使用的是SubscriptionIntraProcess的成员函数：

  void execute(){execute_impl<CallbackMessageT>();}template<class T>typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::typeexecute_impl(){rmw_message_info_t msg_info;msg_info.publisher_gid = {0, {0}};msg_info.from_intra_process = true;if (any_callback_.use_take_shared_method()) {ConstMessageSharedPtr msg = buffer_->consume_shared();any_callback_.dispatch_intra_process(msg, msg_info);} else {MessageUniquePtr msg = buffer_->consume_unique();any_callback_.dispatch_intra_process(std::move(msg), msg_info);}}

execute_impl中由于我们的图片源消息是一个unique_ptr，所以进入了

src/ros2/rclcpp/rclcpp/include/rclcpp/any_subscription_callback.hpp

里的

void dispatch_intra_process(MessageUniquePtr message, const rclcpp::MessageInfo &message_info){TRACEPOINT(callback_start, (const void *)this, true);if (shared_ptr_callback_) //case A{typename std::shared_ptr<MessageT> shared_message = std::move(message);shared_ptr_callback_(shared_message);}else if (shared_ptr_with_info_callback_){typename std::shared_ptr<MessageT> shared_message = std::move(message);shared_ptr_with_info_callback_(shared_message, message_info);}else if (unique_ptr_callback_)//case B{unique_ptr_callback_(std::move(message));}else if (unique_ptr_with_info_callback_){unique_ptr_with_info_callback_(std::move(message), message_info);}else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_){throw std::runtime_error("unexpected dispatch_intra_process unique message call"" with const shared_ptr callback");}else{throw std::runtime_error("unexpected message without any callback set");}TRACEPOINT(callback_end, (const void *)this);}

这个函数我们主要关注第一个和第三个分支，即caseA 和caseB。caseA表示回调函数接受的参数是一个shared_ptr，对应的是view_node，因此将源消息通过std::move转成了std::shared_ptr传给用户的回调函数。
第三个分支caseB对应watermark_node，回调函数输入的参数是unique_ptr，直接用move操作转右值传给回调函数执行。这里注意的是AnySubscriptionCallback有6个成员变量保存用户的回调函数，但同时只能有一个被设置：

    SharedPtrCallback shared_ptr_callback_;SharedPtrWithInfoCallback shared_ptr_with_info_callback_;ConstSharedPtrCallback const_shared_ptr_callback_;ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback_;UniquePtrCallback unique_ptr_callback_;UniquePtrWithInfoCallback unique_ptr_with_info_callback_;

设置回调函数采用的set成员函数，其在src/ros2/rclcpp/rclcpp/include/rclcpp/subscription_factory.hpp中创建subscription中被调用：

any_subscription_callback.set(std::forward(callback));

std::forward是一个“万能转换”，目标类型是CallbackT，而set提供了6种条件模板供编译时进行检查：

    template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,SharedPtrCallback>::value>::type * = nullptr>void set(CallbackT callback){shared_ptr_callback_ = callback;}template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,SharedPtrWithInfoCallback>::value>::type * = nullptr>void set(CallbackT callback){shared_ptr_with_info_callback_ = callback;}template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,ConstSharedPtrCallback>::value>::type * = nullptr>void set(CallbackT callback){const_shared_ptr_callback_ = callback;}template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,ConstSharedPtrWithInfoCallback>::value>::type * = nullptr>void set(CallbackT callback){const_shared_ptr_with_info_callback_ = callback;}template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,UniquePtrCallback>::value>::type * = nullptr>void set(CallbackT callback){unique_ptr_callback_ = callback;}template <typename CallbackT,typename std::enable_if<rclcpp::function_traits::same_arguments<CallbackT,UniquePtrWithInfoCallback>::value>::type * = nullptr>void set(CallbackT callback){unique_ptr_with_info_callback_ = callback;}

结语

Intra-process机制主要通过IntraProcessManager实现，需要将publisher和subscription注册到IntraProcessManager中并开启node的intra-process属性。IntraProcessManager维护了需要intra-process的publisher与subscription的对应关系，使用unique_ptr进行数据转移避免拷贝操作；而当多个subscription需要同一个publisher数据时，IntraProcessManager虽然会有拷贝但会保持最后一个subscription不拷贝。executor将需要intra-process的subscription看做waitable对象，执行与前面文章中不同的操作，这点需要注意。
最后明确一下，在Foxy版本中，IntraProcessManager连接了Publisher和Subscription，IntraProcessManager负责分发数据并通知该Subscription为ready，不再走RWM层，https://design.ros2.org/articles/intraprocess_communications.html这篇文章中开头的图片在Foxy里面已经不符啦~