ROS 搞懂多话题回调机制以及消息队列

ROS 消息队列的运行机制

下面只是自己的理解，如果有误望大家指正！ 大部分内容参考于 https://blog.csdn.net/qq_42700518/article/details/104453752
ROS中，publisher和subscriber都有一个消息队列用于数据收发时候的缓存，那么它们有什么异同呢？
ROS话题的通信是异步的，也就是publisher只管不停的发但不管是否被接收，publisher的消息队列就是publisher发布信息的地方，publisher向消息队列发布信息然后通知subscriber过来读取， 而并不是publisher直接push给subscriber，publisher不停将消息发布到消息队列上，直到队列占满，然后新到的消息把最老的消息挤出队列，每个subscriber会通过一个位置指针使其在正确的消息队列位置上进行读取，当subscriber读取的及时(只需要该subscriber和publisher同时启动)，该subscriber读取的就是最新发布的消息，但是如果subscriber的启动有延时，那么subscriber读取的就是历史信息了，因此，publisher消息队列的作用就是，缓存一定数量的历史信息，让不能及时订阅的subscriber能读取到之前发布的信息。如果没这个需求或是想让subscriber读取到最新的消息，那么让消息队列长度设为1 。
subscriber读取到数据后就会将它放置与自己的消息队列中，等待回调函数读取，这个消息队列的作用是，将回调函数来不及处理的信息及时的缓存，这样不至于因为回调函数执行或调用过慢，导致数据的丢失。
多话题回调机制
ros::spinOnce()和spin()

spinOnce()的主要用法是：

ros::Rate r(rate);
while (ros::ok())
{
ros::spinOnce();
r.sleep();
}

当一个节点订阅了多个话题时，每调用一次ros::spinOnce()，每个接收到消息的subscriber都去执行它的回调函数，那么问题来了，每个subscriber执行回调函数的次数是多少呢？难道每次每个subscriber就调用一次回调函数? 还有多个subscriber的回调函数的执行顺序是什么样呢?
上一节讲到了，subscriber的消息队列用于缓存来不及处理的消息，那么每次调用ros::spinOnce()时，如果各个subscriber只是执行一次回调函数,那么消息队列估计很长时间都不会清0, 所以, 应该说每次调用ros::spinOnce()都会执行与消息队列中缓存的信息数量相同次数的回调函数，只要回调函数执行够快的话，就能清空队列。
另外, 当多个subscriber都需要调用回调函数时，则按顺序依次执行各个subscriber的回调函数。
如下例(非完整)：

订阅部分

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/String.h>
#include <cmath>
using namespace  std;
/*****************************订阅部分*********************************/
  void chatterCallback1(const std_msgs::String::ConstPtr& msg)
{
 int i=0;
 ROS_INFO("I heard: [%s]", msg->data.c_str());
//  sleep(1);
}

void chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
 ROS_INFO("I heard: [%s]", msg->data.c_str());
 sleep(1);
}
int main(int argc, char** argv )
{
    ros::init(argc, argv, "rosub");
    ros::NodeHandle nhsub;
    ros::Subscriber  chatter1_sub = nhsub.subscribe("/chatter1", 5, chatterCallback1 );
    ros::Subscriber  chatter2_sub = nhsub.subscribe("/chatter2", 3, chatterCallback2 );

//    ros::Rate r(0.1);
//    while (ros::ok())
//    {
    ros::spin();
//    r.sleep();
//    }
}

发布部分

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/String.h>
#include <cmath>
using namespace  std;
void send1(ros::Publisher & chatter1_pub)
{
   static int t=0;
   std_msgs::String msg;
   std::stringstream ss;
   ss<<"chatter1 "<<t<<endl;
   msg.data = ss.str();
   chatter1_pub.publish(msg);
   t++;
}


void send2(ros::Publisher & chatter2_pub)
{
   static int t=0;
   std_msgs::String msg;
   std::stringstream ss;
   ss<<"chatter2 "<<t<<endl;
   msg.data = ss.str();
   chatter2_pub.publish(msg);
   t++;
}
int main( int argc, char** argv )
{
  ros::init(argc, argv, "rostest");
  ros::NodeHandle nh;
  ros::Publisher chatter1_pub = nh.advertise<std_msgs::String>("/chatter1", 1);
  ros::Publisher chatter2_pub = nh.advertise<std_msgs::String>("/chatter2", 1);
  ros::Rate r(10);
  while (ros::ok()) {
      send1(chatter1_pub);
      send2(chatter2_pub);
      ros::spinOnce();
      r.sleep();
  }
}

发布器以10HZ的速度发布信息到chatter1，chatter2话题。
接收器，以0.1HZ的速度调用ros::spinOnce()，订阅的消息队列长度分别为5和3，那么以0.1hz即10s的间隔调用ros::spinOnce()时，两个Topic的消息队列都满了，这时一共会调用8次回调函数，且前6次为交替调用 。 有一点要注意，在回调函数里执行的时间也是会算在r.sleep()中的，如上例中，执行chatterCallback2()需要一秒，那么spinOnce()后一共要花3秒在回调函数中，然后的r.sleep()就只会执行7秒的延时，那么可想而知，如果在回调函数中执行的时间大于r.sleep()需要延时的时间，那么回调函数就一直执行下去。
在这里插入图片描述
上图是执行一次spinOnce()所调用的所有回调函数的输出，可见chatter2在执行回调函数里的1S延时操作时，消息队列的数据也更新了，也就是说，执行回调函数时，消息队列并不会被锁定住。
另外，在chatter2回调函数延时的时候，chatter1的回调函数也是处于等待当中，就是说这些回调函数都是串行执行的。
而ros::spin()可以看作r.sleep()=0的ros::spinOnce()，即
while (ros::ok())
{undefined
ros::spinOnce();
}
其他方面都可当作spinOnce()来理解。
ros::MultiThreadedSpinner

MultiThreadedSpinner的用法是：

ros::MultiThreadedSpinner spinner(4); // 数字表示调用的线程数
spinner.spin(); // spin() will not return until the node has been shutdown
通过MultiThreadedSpinner调用的回调函数就不再是串行执行的了，而是多线程 并行执行！

总结：
1、spinOnce()/spin()调用时是串行执行所有的回调函数，并且是各个话题按顺序执行的，执行的次数与调用spinOnce()时消息队列的元素个数相同。
2、MultiThreadedSpinner调用的回调函数就不再是串行执行的了，而是多线程 并行执行。