动手学 ROS（7）：精讲多线程之 MultiThreadedSpinner

之前的全部示例代码，我们都是用默认的单线程来处理任务，但顺序执行的方式在很多时候是不能保证时效性的，因此我们有必要来看看 ros 里面怎么使用多线程来处理回调任务。

目录

• 单线程的问题：订阅多个 Topic，一个 Spinner thread
• 多线程：订阅多个 Topic，多个 Spinner threads
• 订阅一个 Topic，多个 Spinner threads
• 订阅多个 Topic，每个 Subscriber 一个 Callback queue

1 单线程的问题

现在我们设定一个场景，利用《动手学 ROS（6）：命名空间之 group 与 remap》中的 multi_publisher.launch，分别以 1Hz 的速率向 A/message 和 B/message 发送消息，我们写一个接收端，其中 CallbackA 在处理 A/message 时会 sleep 2s, CallbackB 无间隔，我们来看看执行的效果。

Subscriber 代码如下：

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_b = n.subscribe("B/message", 1, CallbackB);
 ​
     ros::Subscriber sub_a = n.subscribe("A/message", 1, CallbackA);
     ros::spin();
 ​
     return 0;
 }

结果显示，CallbackB 也跟着变成 2s 的调用频率。日志如下（注意打印时间）：

 [ INFO] [1621391346.653984500]:  I heard: [/A/message 1]
 [ INFO] [1621391346.656312400]:  I heard: [/B/message 2]
 [ INFO] [1621391348.656641300]:  I heard: [/A/message 3]
 [ INFO] [1621391348.656849300]:  I heard: [/B/message 4]
 [ INFO] [1621391350.657389800]:  I heard: [/A/message 5]
 [ INFO] [1621391350.657563600]:  I heard: [/B/message 6]

一图胜千言，在只有一个 Spinner thread 的情况下，callback queue 只能顺序执行。

这就说明了单线程的不足，不管有多少个 Subscriber，节点都只能顺序执行回调，这在某些时候是不能忍受的，因此，多线程有了用武之地，我们要做的事情就是增加 spinner thread。

2 多线程：订阅多个 Topic，多个 Spinner threads

我们修改刚刚的 Subcriber, 其实也就改了两行：

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_b = n.subscribe("B/message", 1, CallbackB);
 ​
     ros::Subscriber sub_a = n.subscribe("A/message", 1, CallbackA);
     //改变的地方
     ros::MultiThreadedSpinner spinner(2);
     spinner.spin();
 ​
     return 0;
 }

MultiThreadedSpinner 可初始化线程的数量，这里因为有两个 subscriber，就选择了 2。Callback 依然是 2s 调用一次。测试结果如下：

 [ INFO] [1621390946.100496300]:  I heard: [/B/message 1]
 [ INFO] [1621390947.100450900]:  I heard: [/B/message 2]
 [ INFO] [1621390948.015270200]:  I heard: [/A/message 1]
 [ INFO] [1621390948.099850900]:  I heard: [/B/message 3]
 [ INFO] [1621390949.100118400]:  I heard: [/B/message 4]
 [ INFO] [1621390950.015475900]:  I heard: [/A/message 3]
 [ INFO] [1621390950.100807500]:  I heard: [/B/message 5]
 [ INFO] [1621390951.100232500]:  I heard: [/B/message 6]
 [ INFO] [1621390952.015688200]:  I heard: [/A/message 5]

CallbackB 处理了 B/message 中的所有消息 (1,2,3,4,5,6)，而 CallbackA 还是 2s 调用一次，只处理了 A/message 中编号为 1，3，5 的消息。也就是说，我们有一个空闲的线程在另一个线程被 CallbackA 占用时可以从容地处理 CallbackB。如下图：

3 订阅一个 Topic，多个 Spinner threads

假设只有一个 Topic, 发布端的频率比较高，我们又想尽可能多地响应消息，因此我们可以设置多个 Spinner，但是单纯地像上一小节一样使用 MultiThreadedSpinner 是不行的，ros 作了限制，默认阻止并行处理一个 Callback，我们需要更改 Suscriber 的配置：

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 ​
 void ChatterCallback(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
   std::this_thread::sleep_for(std::chrono::seconds(2));
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   
   ros::NodeHandle n;
   ros::SubscribeOptions ops;
   ops.init<std_msgs::String>("A/message", 1, ChatterCallback);
   ops.allow_concurrent_callbacks = true;
   ros::Subscriber sub1 = n.subscribe(ops);
   ros::MultiThreadedSpinner spinner(2);
   
   spinner.spin();
   return 0;
 }

可以看到，通过设置 SubscribeOptions 的 allow_concurrent_callbacks 为 true，就可以让同一个 Callback 并行执行，结果如下：

 [ INFO] [1621397291.938394800]:  I heard: [/A/message 1]
 [ INFO] [1621397292.938467100]:  I heard: [/A/message 2]
 [ INFO] [1621397293.940122300]:  I heard: [/A/message 3]
 [ INFO] [1621397294.938827600]:  I heard: [/A/message 4]
 [ INFO] [1621397295.940477900]:  I heard: [/A/message 5]
 [ INFO] [1621397296.939098400]:  I heard: [/A/message 6]

虽然 ChatterCallback 中仍然是等待 2s，但实际上却处理了每一秒接收到的消息。如图：

4 订阅多个 Topic，每个 Subscriber 一个 Callback queue

在《ROS Spinning, Threading, Queuing》的最后还提到了一种更加优秀的实践方式：

if there is only one callback queue which is FIFO queue (first in, first out), the callback for the message A cannot be processed until all three callbacks for the message B are processed. This problem can be mitigated by creating multi spinner threads as talked before, but it cannot completely solve the issue which stems from having a single callback queue.
To solve the problem, we can create a separate callback queue and spinner thread which is dedicated to the message A as shown below.

 #include <thread>
 #include <ros/callback_queue.h>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   ros::NodeHandle n;
   ros::Subscriber sub_b = n.subscribe("MessageB", 1, CallbackB);
   
   ros::NodeHandle n_a;
   ros::CallbackQueue callback_queue_a;
   n_a.setCallbackQueue(&callback_queue_a);
   ros::Subscriber sub_a = n_a.subscribe("MessageA", 1, CallbackA);
   std::thread spinner_thread_a([&callback_queue_a]() {
     ros::SingleThreadedSpinner spinner_a;
     spinner_a.spin(&callback_queue_a);
   });
   ros::spin();
   spinner_thread_a.join();
   return 0;
 }

给每一个 subscriber 创建一个单独的 callback queue，这样就解决了即使用了 MultiThreadedSpinner 但所有的 callback 依然在同一个 queue 排除执行的问题，此方法用来解决 subscriber 的优先级问题。笔者在实际中未遇到使用该方法的地方，仅记录在此，保持文章的完整性。

小结

1. MultiThreadedSpinner 可使节点多线程执行任务；
2. 要使一个 Subscriber 多线程执行，需设置 SubscribeOptions 的 allow_concurrent_callbacks 为 true；
3. 如果需要保证某个 Subscriber 优先执行，可配置其独立的 CallbackQueue；
4. 其实还有个 AsyncSpinner 提供和 MultiThreadedSpinner 类似的功能，有机会再介绍。