ros::spin() 或 ros::spinOnce(),ros消息队列理解

学习ROS中曾经不止一次困惑ros::spin() 或 ros::spinOnce()的区别，现在回过头看却是很简单。另外ros消息队列之前也是迷迷糊糊，这次做个实验总结。

一、ros::spin() 或 ros::spinOnce()

这两个都是叫作ROS消息回调处理函数，两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序。
不知道你们怎么样，我看到这当时觉得懂了，却一直很心虚的。实际理解是：

1. ros::spin()不会放在循环中，程序执行到这之后不会跳出来，永远执行消息的回调函数！
2. ros::spinOnce() 绝大多数会在循环里面，然后返回，跳转到下一条语句！
3. 一次 ros::spinOnce()会执行完所有消息队列中的消息！进入它时，会有个当前消息计数假设当前 5次 ，它会就会执行 5次 消息回调函数。至于消息内容具体就看消息队列的大小，和处理速度了，具体看二、Ros消息队列。

二、Ros消息队列

消息队列到底是怎么样的，做个实验：
talker.cpp

//talker node
/*
 * @Descripttion: 
 * @version: 
 * @Author: wjh 
 * @Date: 2021-06-26 10:45:46
 * @LastEditors: Wen JiaHao
 * @LastEditTime: 2021-12-06 15:33:24
 */
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "beginner_tutorials/AddTwoInts.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);
  printf("cesh222i-------------\n");
  ros::Rate loop_rate(1);/*每秒发一次数据*/
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    std::stringstream ss2;
    ss << "hello world chatter1: " << count;
    ss2 << "hello world chatter2: " << count;
    msg.data = ss.str();
    chatter_pub.publish(msg);
    msg.data = ss2.str();
    chatter_pub2.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

listener.cpp

#include "std_msgs/String.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <pthread.h>
using namespace std;

int ccount=0;
ros::ServiceClient liftAutoClient;

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

void chatterCallback2(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;
  pthread_t idpccount;
  liftAutoClient=n.serviceClient<beginner_tutorials::AddTwoInts>("Add");
  //pthread_create(&idpccount, NULL, pPrint, NULL);
  ros::Subscriber sub = n.subscribe("chatter2", 10, chatterCallback1);
  ros::Subscriber sub2 = n.subscribe("chatter1", 10, chatterCallback2);
  sleep(5);//启动后等待5秒
  printf("after 5 s\n");
  ros::spinOnce();//执行一次回调，你认为是连续5个消息，实际因为每次处理时间为5秒，后面几次的消息被挤出了缓冲区，读到的不是最开始的数据。
  printf("hello\n");
  return 0;
}

结果

talker

wjh@wjh_honor:~/catkin_ws$ rosrun beginner_tutorials talker 
cesh222i-------------

listen

wjh@wjh_honor:~/catkin_ws$ rosrun beginner_tutorials listener 
after 5 s
[ INFO] [1639467223.128549614]: I heard: [hello world chatter1: 3]
[ INFO] [1639467223.128777020]: I heard: [hello world chatter2: 3]
[ INFO] [1639467228.129120414]: I heard: [hello world chatter1: 4]
[ INFO] [1639467228.129264989]: I heard: [hello world chatter2: 4]
[ INFO] [1639467233.129756292]: I heard: [hello world chatter1: 8]
[ INFO] [1639467233.129906551]: I heard: [hello world chatter2: 8]
[ INFO] [1639467238.130446465]: I heard: [hello world chatter1: 13]
[ INFO] [1639467238.130619063]: I heard: [hello world chatter2: 13]
[ INFO] [1639467243.131248178]: I heard: [hello world chatter1: 18]
[ INFO] [1639467243.131403797]: I heard: [hello world chatter2: 18]
hello
wjh@wjh_honor:~/catkin_ws$ 

看到了吗，休眠5秒后执行listen节点的spinOnce时，接收缓冲区大概缓存了5个数据，因此会执行五次消息回调函数，之后就返回了。另外，可以注意到计数值不是期望的3，4，5，6，7，因为每次打印耗时5s，这5秒过程中新的数据依旧在放入缓冲队列，所以第三次打印时，缓冲区的数据5，6，7已经被挤出队列抛弃了。
sleep(5)时，消息队列有5个消息，但是每个消息回调执行时间也是5秒，因此下一次执行回调的消息可能早已不是当初的消息。所以之后设计消息接收时，耗时的操作不要放在回调函数里。

在做slam相关的课题时，要考虑回调处理时间的问题，如果回调函数的周期大于了传感器信息发布周期，就会丢失数据，导致当前不同传感器数据时间戳不一致。
例如：处理gnss数据和imu数据，当gnss回调中做融合，导致回调函数的周期大于了传感器信息发布周期，当执行完imu回调函数，获取了5个消息中最新imu数据后，执行完gnss回调函数，你获取的最新gnss数据很可能已经不是队列中当时的数据，而是缓冲溢出后的最新数据。这就导致gnss时间戳晚于imu时间戳。 可以采取队列将消息数据缓存，单独进行处理！