关于 “从零开始做自动驾驶定位(三)”中ROS缓冲区机制的理解

缓存区

        ROS中发布和订阅均有消息缓存区的概念，消息缓存区大小 buff_size 为  queue_size* 单个message所占缓存区大小

buff_size = queue_size * 单个message所占缓存区大小

发布：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <ros/console.h>
#include <sstream>
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter1", 10);
  ros::Publisher chatter_pub2 = n.advertise<std_msgs::String>("chatter2", 10);

  ros::Rate loop_rate(1);/*每秒发一次数据*/
  int count = 0;

  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    std::stringstream ss2;
    ss << "chatter1: " << count;
    ss2 << "chatter2: " << count;
    msg.data = ss.str();
    chatter_pub.publish(msg);
    msg.data = ss2.str();
    chatter_pub2.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
    
    ++count;
  }
}

订阅：

#include "std_msgs/String.h"
#include <ros/ros.h>

using namespace std;
 
int ccount = 0;

void chatterCallback1(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("[%s]", msg->data.c_str());
}

void chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("[%s]", msg->data.c_str());
  ccount++;

  if(ccount%2 == 0){
    ROS_INFO_STREAM("------------sleep-------------");
    sleep(5);    
  }
}
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ROS_INFO("sub ------");
  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("chatter1", 10, chatterCallback1);
  ros::Subscriber sub2 = n.subscribe("chatter2", 10, chatterCallback2);

  sleep(5);//启动后等待5秒
  printf("after 5 s\n");

  while(ros::ok()){
    ros::spinOnce();//执行一次回调，你认为是连续5个消息，实际因为每次处理时间为5秒，后面几次的消息被挤出了缓冲区，读到的不是最开始的数据。
    ROS_INFO_STREAM("---ccount: " << ccount);
    printf("spinOnce\n");
  }
  return 0;
}

        先运行 pub 一段时间后再运行 sub。得到以下输出结果： 

[ INFO] [1723126456.520029189]: sub ------
after 5 s
[ INFO] [1723126461.523491833]: [chatter1: 7]
[ INFO] [1723126461.524979833]: [chatter2: 7]
[ INFO] [1723126461.525021825]: [chatter1: 8]
[ INFO] [1723126461.525036585]: [chatter2: 8]
[ INFO] [1723126461.525055258]: ------------sleep-------------
[ INFO] [1723126466.525285922]: [chatter1: 9]
[ INFO] [1723126466.525345422]: [chatter2: 9]
[ INFO] [1723126466.525360130]: [chatter1: 10]
[ INFO] [1723126466.525374453]: [chatter2: 10]
[ INFO] [1723126466.525388751]: ------------sleep-------------
[ INFO] [1723126471.525645344]: [chatter1: 12]
[ INFO] [1723126471.525704813]: [chatter2: 12]
[ INFO] [1723126471.525724781]: ---ccount: 5
spinOnce
[ INFO] [1723126471.525746173]: [chatter1: 13]
[ INFO] [1723126471.525762475]: [chatter2: 13]
[ INFO] [1723126471.525793611]: ------------sleep-------------
[ INFO] [1723126476.526092998]: [chatter1: 17]
[ INFO] [1723126476.526162244]: [chatter2: 17]
[ INFO] [1723126476.526178266]: [chatter1: 18]
[ INFO] [1723126476.526195580]: [chatter2: 18]
[ INFO] [1723126476.526209655]: ------------sleep-------------
[ INFO] [1723126481.526543337]: [chatter1: 22]
[ INFO] [1723126481.526607650]: [chatter2: 22]
[ INFO] [1723126481.526625041]: [chatter1: 23]
[ INFO] [1723126481.526638999]: [chatter2: 23]
[ INFO] [1723126481.526659388]: ------------sleep-------------
[ INFO] [1723126486.526902511]: [chatter1: 27]
[ INFO] [1723126486.526961615]: [chatter2: 27]
[ INFO] [1723126486.526978680]: [chatter1: 28]
[ INFO] [1723126486.526992401]: [chatter2: 28]
[ INFO] [1723126486.527007543]: ------------sleep-------------
[ INFO] [1723126491.527156150]: [chatter1: 32]
[ INFO] [1723126491.527219639]: [chatter2: 32]
[ INFO] [1723126491.527236042]: [chatter1: 33]
[ INFO] [1723126491.527252247]: [chatter2: 33]
[ INFO] [1723126491.527267172]: ------------sleep-------------
[ INFO] [1723126496.527448772]: ---ccount: 14
spinOnce

分析：

        当程序执行到第二个 sleep 时，消息队列中全部消息如图所示，此时 message 11 被挤出消息队列。

总结

1、spinOnce 不是指执行一次 callback，而是执行当前缓存区中 msg.size() 次callback。

2、callback 取的是消息队列中最早接收的数据。

3、callback消息队列是先进先出的队列格式。

        在做slam相关的课题时，要考虑回调处理时间的问题，如果回调函数的周期大于了传感器信息发布周期，就会丢失数据，导致当前不同传感器数据时间戳不一致。

        例如：处理lidar数据和gnss数据，在lidar回调中做融合，导致回调函数的周期大于了传感器信息发布周期，当执行完本次相同时间戳下lidar和gnss的融合后，在下一次融合时，获取的最新gnss数据很可能是缓冲溢出后的最新数据。这就导致gnss时间戳晚于lidar时间戳。 可以使用deque容器存储消息队列中的消息，用deque存储可以做时间同步，将使用后的数据从deque前面清除，一直保持deque中是未使用的数据。

 扩展：

        机器人应用中难免会遇到运算起来很费时间的操作，比如图像的特征提取、点云的匹配等等。有时候，我们需要在ROS的Subscriber的Callback回调函数中进行这些费时的操作。如果我们要是没有取舍的对于每个消息都调用一次回调函数，那么势必会导致计算越来越不实时，很有可能当下在处理的还是几秒以前的数据。所以，如果我们想实时处理最新的消息，实际上只需要把两个queue_size都设置成1，那么系统不会缓存数据，自然处理的就是最新的消息。

    rospy的订阅者还有个buff_size参数，要设置一个很大很大的数，这个缓冲区的大小是指消息队列使用的缓冲区物理内存空间大小。如果这个空间小于一个队列中所有消息所需要的空间，比如消息是一副图片或者一帧点云，数据量超过了缓冲区的大小。这个时候为了保证通信不发生错误，就会触发网络通信的保护机制，TCP的Buffer会为你缓存消息。这种机制就会导致每一帧消息都被完整的缓存下来，没有消息被丢弃，感觉上就像queue_size被设置成了无穷大。

        但是roscpp里面没有buff_size参数，它获得 incoming message的大小，分配所需的缓冲区大小，然后一次读取整个incoming message

参考：

        发布和订阅

        关于ROS消息缓存区的存取理解