【ROS】回调的多线程问题

参考：

1. 机械臂学习(15)-ROS多线程
2. 动手学ROS（7）：精讲多线程之MultiThreadedSpinner
3. rosspin、rosspinOnce及多线程订阅
4. ROS发布与订阅处理顺序以及多线程使用

回调（Callback）

ROS中主要有四种类型的回调，分别绑定在不同的ROS对象上：

Callback的特点概括为： 用户实现，ROS调度 。那么，ROS是如何进行调度的呢？

ROS回调机制

ROS默认有维护一个全局回调队列（名为：Global Callback Queue），将已可用的回调插入Callback队列中。再通过Spinner线程获取并执行当前可用的回调。为了说明ROS回调机制，我引入两个ROS节点：一个使用定时器发布多个topic消息；另一个订阅这些topic消息。

1. 发布节点
   一个使用ROS定时器超时回调发布多个topic消息：
   ros::Timer
   来看一个ros::Timer的回调队列，代码如下：

//这段代码主要是实现定时向Topic发布消息
#include "ros/ros.h"
#include 
#include "my_msg/weather_pub.h"
#include "my_msg/air_quality_pub.h"
#include 

int main(int argc, char **argv){
  ros::init(argc, argv, "multi_publisher");
  ros::NodeHandle n;
  /*通知ROS master，本node要发布一个名为“Weather”的话题（topic），
  消息类型为my_msg::weather_pub，发送队列长度为48*/
  ros::Publisher pub_weather = 
    n.advertise<my_msg::weather_pub>("Weather", 48, true);

  /*通知ROS master，本node要发布一个名为“WeatherA”的话题（topic），
  消息类型为my_msg::weather_pub，发送队列长度为48*/
  ros::Publisher pub_weather_a = 
    n.advertise<my_msg::weather_pub>("WeatherA", 48, true);

  /*通知ROS master，本node要发布一个名为“AirQuality”的话题（topic），
  消息类型为my_msg::air_quality_pub，发送队列长度为48*/
  ros::Publisher pub_air_quality = 
    n.advertise<my_msg::air_quality_pub>("AirQuality", 48, true);

  int count = 0;
  //创建一个ros::Timer每0.2秒进行发布，回调函数采用lamda4方法的格式
  ros::Timer timer = n.createTimer(ros::Duration(0.2),
  [&](const ros::TimerEvent&) {

    my_msg::weather_pub msg;

    std::stringstream ss;
    ss << "Sunny " << count;
    msg.weather= ss.str();
    ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
      <<"],weather:"<< msg.weather.c_str());
    pub_weather.publish(msg);

    std::stringstream ssa;
    ssa << "Sunny " << 20+count;
    msg.weather= ssa.str();
    ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
      <<"],weather:"<< msg.weather.c_str());
    pub_weather_a.publish(msg);

    my_msg::air_quality_pub msg_air;
    msg_air.air_quality_index = 128+count;
    ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
      <<"],air quality:"<< msg_air.air_quality_index);
    pub_air_quality.publish(msg_air);

    ++count;
  });
  
  //确保定时器回调被调用
  ros::spin();
  
  return 0;
}

定时器启动后会生成一个Timer线程，根据定时器的参数，当定时器超时后将定时器的回调函数加入Callback队列中。然后再由用户调用的Spinner线程（ros::spin）从Callback队列中依次取出当前已可用的回调并执行。

ros::Timer的队列和回调过程

ros::Publisher
上面例子中，在定时器回调函数中向topic进行发布，ros::Publisher将要发布的消息加入到Publisher队列中，再由专门的Publisher线程发布出去。注意这其中并不涉及Callback队列，这也解释了上篇中提到的：如果一个ROS节点仅进行topic发布是不需要调用spinner的。

ros::Pubblisher的队列和发送过程

2. 订阅节点
   订阅上面发布的topic消息。根据不同情况，进行代码修改。
   ros::Subscriber
   先让ROS节点只订阅一个topic来说明订阅回调的过程。下面是代码：

//订阅一个topic的代码
#include "ros/ros.h"
#include "my_msg/weather_pub.h"

//回调函数，注意参数是const类型的boost::shared_ptr指针
void weatherCallback(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO("The 24 hours Weather: [%s]", msg->weather.c_str());
}
int main(int argc, char **argv){
  ros::init(argc, argv, "subscriber");
  ros::NodeHandle n;
  /*通知ROS master，本node要订阅名为“Weather”的话题（topic），
  并指定回调函数weatherCallback*/
  ros::Subscriber sub = n.subscribe("Weather", 48, weatherCallback);
  ros::spin();
}

订阅创建后，涉及到两个线程和两个队列：

• ROS提供的Receiver线程将收到的消息加入到Subscriber队列，并触发使订阅回调函数加入Callback队列。注意：每个ros::Subscriber有自己的Subscriber队列，而Callback队列默认是全局的。

• 用户调用的Spinner线程（也就是ros::spin）从Callback队列中取出已可用的回调并执行。
  

  ros::Subscriber的队列和回调过程
  

在实际项目中，如果订阅回调中有耗时操作，那么可以用户可以启用多个Spinner线程并发从Callback队列中取出已可用的回调并执行，这样可以加快Callback队列被执行的速度。

ROS Spinner

ROS提供两种单线程回调的spin方法和两种多线程回调的Spin类，分别是：

单线程回调spin方法：

• ros::spin()——相当于while(true)的大循环，不断遍历执行Callback队列中的可用回调
• ros::spinOne()——相当于马上执行一次Callback队列中的可用回调

多线程回调spin类：

• ros::MultiThreadedSpinner——相当于while(true)大循环，启动指定数量的Spinner线程并发执行Callback队列中的可用回调。可指定Callback队列。
• ros::AsyncSpinner——异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。可指定Callback队列。

简单总结，如果程序简单用ros::spin()就够了；如果程序复杂推荐使用ros::AsyncSpinner类。他们的详细用法和区别在ROS官方教程中已经写得比较清楚。

下面这段代码展示如何使用ros::AsyncSpinner启用多个Spinner线程：

//一个topic多个线程来执行的代码
#include "ros/ros.h"
#include "boost/thread.hpp"
#include "my_msg/weather_pub.h"

//回调函数，注意参数是const类型的boost::shared_ptr指针
void weatherCallback(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],after looping 24 hours weather:"<< msg->weather.c_str());
}

int main(int argc, char **argv){
  ros::init(argc, argv, "multi_subscriber");
  ros::NodeHandle n;
  /*通知ROS master，本node要订阅名为“Weather”的话题（topic），
  并指定回调函数weatherCallback*/
  ros::Subscriber sub = n.subscribe("Weather", 48, weatherCallback);

  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"]This is main thread.");

  //声明spinner对象，参数2表示并发线程数，默认处理全局Callback队列
  ros::AsyncSpinner spinner(2);
  //启动两个spinner线程并发执行可用回调  
  spinner.start();
  ros::waitForShutdown();
}

//这是执行结果，可以看到主线程
[ INFO] [1637131602.089381910]: Thread[7f9a1ad24780]This is main thread.
[ INFO] [1637131602.375058712]: Thread[7f9a11bb6700],after looping 24 hours weather:Sunny 679
[ INFO] [1637131602.488504089]: Thread[7f9a11bb6700],after looping 24 hours weather:Sunny 680
[ INFO] [1637131602.688845441]: Thread[7f9a123b7700],after looping 24 hours weather:Sunny 681
[ INFO] [1637131602.888828136]: Thread[7f9a123b7700],after looping 24 hours weather:Sunny 682

从执行结果中可以看到，进程中包括三个线程：主线程、Spinner线程1、Spinner线程2。

下图展示了相关的线程和队列处理过程 ：

使用多线程Spinner同时并发处理回调过程

实际项目中一个节点往往要订阅多个topic，在使用默认全局Callback队列时，如果某些topic发布频率高回调处理又耗时的话，容易影响其他topic消息的处理。
假定发布端现在分别以1Hz的速率向A/message和B/message发送消息：

#include 
 #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]

下图中TopicB的消息居多可能影响TopicA的处理。

多个ros::Subscriber在全局回调队列中排队的过程

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

订阅多个Topic，多个Spinner threads：

 #include 
 #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。如下图：

订阅一个Topic，多个Spinner threads

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

#include 
 #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，但实际上却处理了每一秒接收到的消息。如图：

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

#include 
 #include 
 #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的优先级问题。

即：这种情况下，ROS提供了机制，可以为每个ros::Subscriber指定Callback队列，再分别指定Spinner线程仅处理指定Callback队列的回调。这样确保每个订阅回调相互独立不影响。下面的代码展示如何进行上述操作：

//为每个subscriber指定队列
#include "ros/ros.h"
#include "boost/thread.hpp"
#include "my_msg/weather_pub.h"
#include "my_msg/air_quality_pub.h"
#include 

//回调函数，注意参数是const类型的boost::shared_ptr指针
void weatherCallback(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],before loop 24 hours weather:"<< msg->weather.c_str()); 
  //死循环
  while(true){}

  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],24 hours weather:"<< msg->weather.c_str()); 
}

void weatherCallback_A(const my_msg::weather_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],A 24 hours weather:"<< msg->weather.c_str()); 
}

//回调函数，注意参数是const类型的boost::shared_ptr指针
void airQualityCallback(const my_msg::air_quality_pubConstPtr& msg)
{
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()
    <<"],24 hours air quality:"<< msg->air_quality_index); 
}

int main(int argc, char **argv){
  ros::init(argc, argv, "multi_subscriber");
  ros::NodeHandle n;
  /*通知ROS master，本node要订阅名为“Weather”的话题（topic），
  并指定回调函数weatherCallback*/
  ros::Subscriber sub = n.subscribe("Weather", 48, weatherCallback);
  ros::Subscriber sub_a = n.subscribe("WeatherA", 48, weatherCallback_A);

  //需要单独声明一个ros::NodeHandle
  ros::NodeHandle n_1;
  //为这个ros::Nodehandle指定单独的Callback队列
  ros::CallbackQueue my_queue;
  n_1.setCallbackQueue(&my_queue);
  /*通知ROS master，本node要订阅名为“AirQuality”的话题（topic），
  并指定回调函数airQualityCallback*/
  ros::Subscriber air_sub = n_1.subscribe("AirQuality", 48, airQualityCallback);
  
  ROS_INFO_STREAM("Thread["<< boost::this_thread::get_id()<<"]This is main thread.");
  
  //启动两个线程处理全局Callback队列 
  ros::AsyncSpinner spinner(2);
  spinner.start();

  //启动一个线程处理AirQuality单独的队列
  ros::AsyncSpinner spinner_1(1, &my_queue);
  spinner_1.start();
  
  ros::waitForShutdown(); 
}

// 执行结果
[ INFO] [1637132247.535142399]: Thread[7f73e4384780]This is main thread.
[ INFO] [1637132247.743935399]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3926
[ INFO] [1637132247.744032493]: Thread[7f73d6ffd700],before loop 24 hours weather:Sunny 3906
[ INFO] [1637132247.744203496]: Thread[7f73d67fc700],24 hours air quality:4034
[ INFO] [1637132247.888403207]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3927
[ INFO] [1637132247.888433359]: Thread[7f73d67fc700],24 hours air quality:4035
[ INFO] [1637132248.088418911]: Thread[7f73d67fc700],24 hours air quality:4036
[ INFO] [1637132248.088461907]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3928
[ INFO] [1637132248.288417795]: Thread[7f73d67fc700],24 hours air quality:4037
[ INFO] [1637132248.288448289]: Thread[7f73d77fe700],A 24 hours weather:Sunny 3929

从执行结果中可以看到，进程中包括四个线程：主线程、全局队列Spinner线程1、全局队列Spinner线程2，以及本地队列Spinner线程3。尽管Spinner线程1被回调函数中的死循环卡住，但并不影响其他topic的回调处理。
下图展示了相关的线程和队列处理过程 ：

多个ros::Subscriber各自独立使用回调队列的过程

小结

• MultiThreadedSpinner可使节点多线程执行任务；
• 要使一个Subscriber多线程执行，需设置SubscribeOptions的allow_concurrent_callbacks为true；
• 如果需要保证某个Subscriber优先执行，可配置其独立的CallbackQueue；