理解ROS Topic 通信频率背后的机制

Topic是ROS的三种通信方式中最为基本、也是常用的一种。本文对于ROS的Topic通信背后的数据吞吐机制做一个较为详细、深入的介绍。

Publisher

ROS中发布一个topic的函数是这样的

ros::Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false);

Parameters:
topic:	Topic to advertise on
queue_size:	Maximum number of outgoing messages to be queued for delivery to subscribers
latch:	(optional) If true, the last message published on this topic will be saved and sent to new subscribers when they connect

有三个参数：topic就是我们要发布的话题，queue_size是publisher队列中可以存储的消息数量, latch是锁存，比如停止publish后保存最后一条message，如果有新的subscriber订阅的话，把之前保存的消息发给新的subscriber。
下面讲一讲和queue_size相关的，关系到我们发送的数据能否按照期望频率传输，会不会丢帧等。

案例分析

ros::init(argc, argv, "talker");
ros::NodeHandle handle;
ros::Publisher chatter_pub = handle.advertise<std_msgs::String>("chatter", 10);
ros::Rate loop_rate(100);
int count = 0;
std::stringstream ss;
std_msgs::String msg;
while (ros::ok())
{
    
    
    ss<<"Message ["<<count<<"]";
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);
    loop_rate.sleep();
    ++count;
    // ss.str("");
}

比如上面这个例子，我设置了publish()的循环频率为100Hz ，但是使用rostopic hz <topic> 查看这个topic的发布频率是不到100Hz的（下图），出现了实际发布频率和设置频率不一致的情况。

这是因为我用的stringstream没有清零，而是逐渐累积，字符串越来越长，所以发布的消息是越来越复杂的，publisher thread处理耗时越来越多，所以发布频率也在逐渐降低。但是如果发布的消息很简单的话，还是可以跟上的，一般而言发布频率和publish()频率是几乎相等的。

那么这种频率错位的背后是什么导致的呢？

Publisher 背后

The publisher queue is another queue like callback queue, but the queue is for queuing published message which is filled every time publish() function is called.
There is a separate thread (publisher thread) that is responsible for taking the message from the publisher queue and send it to subscribers of the topic if there are any. If you are calling the publish()more quickly than the publisher thread can send the messages, the messages start piling up in the publisher queue and if it reaches over the specified queue size (in this case 1000), the old message starts to get overwritten by new messages.

每调用一次publish()都会往 publisher message queue (以下简称PMQ) 当中放入一个message，然后有个单独的线程publisher thread 从PMQ当中取出message并真正发布。

我们叫publish()的循环频率为 期望发布频率，publisher thread处理的频率为 实际发布频率。

一般而言，publisher thread的处理频率要比我们调用publish()的频率高，所以表现出来发布消息的频率就是我们publish()的循环速率。但是如果publish()的循环频率过高，以至于超过了publisher thread处理的速度（或者说publisher thread处理的速度低于publish()的速度）消息就会在PMQ中积累，超过了queue_size的话旧的消息便会丢失。

所以说如果发布的消息比较复杂，且publish()频率较高的话，最好将queue_size设置得大一点，要不然会丢失数据。当然也要看你应用的场景，有些场景丢帧也无所谓，反而是需要更新的数据。

Subscriber

ROS中订阅一个话题的函数如下

ros::Subscriber subscribe(const std::string& topic, uint32_t queue_size, <callback, which may involve multiple arguments>, const ros::TransportHints& transport_hints = ros::TransportHints());

topic还是很好理解，就是我们要订阅的话题。下面讲一讲callback和 queue_size。

Callback

subscriber初始化的时候指明了回调函数callback，这就是回调函数的注册。

那么什么叫回调函数的注册呢？我的理解是下面这样，熟悉的小伙伴可以跳过。

回调就是告诉调用方你应该调用哪个回调函数，其实就是将这个函数的函数指针传给调用方，比如我们这里subscribe(callback)，就是将callback的指针通过subscribe这个函数传给了subscriber。

打个日常生活的比方：好比你（回调函数）刚来一个单位，你跟上级（调用方）说“有事您叫我”，这个露脸的过程就是 回调函数的注册。然后领导有事真的找了你，就是调用了你这个回调函数。

ROS中与回调处理相关的有两类对象：callback queues，spinners 。

subscriber节点初始化之时，它就创建了一个用于接收消息的线程 (receiver thread)，每个subscriber都有一个接收消息的队列 subscriber message queue (SMQ)。同样地还有一个callback queue用于存放回调函数。每当subscriber接收一个消息，就有一个callback放入callback queue当中。

A spinner is an object that has the ability to call the callbacks contained in a callback queue. A callback queue is the object in which each subscriber will add an element each time a message is received by resolving which kind of message should call which callbacks (and with which arguments).
Callback queues/spinning do not have any effect on the internal network communication in roscpp. They only affect when user callbacks occur. They will have an effect on the subscription queue, since how fast you process your callbacks and how quickly messages are arriving determines whether or not messages will be dropped.

roscpp支持三种回调

1. function

   void callback(const std_msgs::StringConstPtr& str)
   {
         
         	}
   ros::Subscriber sub = nh.subscribe("my_topic", 1, callback);
   

2. class methods

   void Foo::callback(const std_msgs::StringConstPtr& message)
   {
         
         	}
   
   Foo foo_object;
   ros::Subscriber sub = nh.subscribe("my_topic", 1, &Foo::callback, &foo_object);
   

3. functor objects，这里就不做介绍了。

SMQ中的回调处理是由Spinner实现的

spinner

ros::spin()

ros::spin() 创建了一个单独的线程 spinner thread 不停循环地、依次将callback从队列中取出并执行 。

虽然主程序当中没有循环，但是ros::spin()里有个循环。subscriber接收到消息并向callback queue当中放入一个回调函数，但是真正执行是在spin()里执行的。跟上面publisher thread类似， spinner thread处理回调的速度是根据回调函数的复杂程度决定的。

ros:spin() 是一个单独的线程，这个线程是循环处理回调函数的，一次处理一个回调函数。具体循环时间要依据callback而决定，这个跟receiver接收信息的频率一般是不一致的。如果比receiver接收信息要快，callback queue可以设置得很小；如果spinner处理回调的速度比receiver接收信息要慢，那么callback queue当中就开始累积callbacks，这时候queue_size最好设置地比较大一点。

有时候spinner thread实在处理不过来怎么办呢？我们可以让spinner thread开影分身——使用多线程spinner，并行处理callbacks。

总共有三种实现spinner的方式，可以参考Three implementations of spinners

Multi-threaded spinning

多线程的有两种ros::MultiThreadedSpinner()和ros::AsyncSpinner()。一般用AsyncSpinner()。

Multi spinner thread

ros::MultiThreadedSpinner spinner(2); 	// 开两个spinner并行处理
spinner.spin();

Instead of a blocking spin() call, it has start() and stop() calls, and will automatically stop when it is destroyed. An equivalent use of AsyncSpinner to the MultiThreadedSpinner example above, is:

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

Reference

ROS Spinning, Threading, Queuing