对 ROS 中 callback 机制进行一点探索

前言

之前在使用 YOLO 时，因为我们使用的是 GPU 算力很低（或者说无）的 nuc 运行的，最大速度是 0.1s 一帧，也就是 10Hz，但是相机一般都有 30Hz，当时出现了很奇怪的延时，就是 YOLO 运行的结果总是比相机话题中的画面明显慢的多（秒为单位），当时错误的理解为队列长度设置为 1，就可以一直保证是最新的消息，所以一直没有很好的解决。当时是把相机的帧率转到只有 10Hz，再让 YOLO 跑，就没有延时了，当时没管，最新想起来了，就配合 ChatGPT 写了点代码测试了一下。

实验过程

先用python写了个发布者和订阅者，发布者一秒一发，1 2 3 4 … ，这样一个一个发下去

接受者使用 rospy.spin() 一直运行 callback 函数，callback 函数中延时 5s，模拟回调函数花费时间很长

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
rospy.init_node('publisher_node')
pub = rospy.Publisher('my_topic', String, queue_size=10)

rate = rospy.Rate(1)  # 发布频率为1Hz

message = 0
while not rospy.is_shutdown():
    message = message + 1
    rospy.loginfo(message)
    pub.publish(str(message))
    rate.sleep()

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import time
def callback(data):
    time.sleep(5)
    rospy.loginfo('I heard %s', data.data)

rospy.init_node('subscriber_node')
rospy.Subscriber('my_topic', String, callback)
rospy.spin()

结果如下：

[INFO] [1686216384.496250]: 1
[INFO] [1686216385.496821]: 2
[INFO] [1686216386.496821]: 3
[INFO] [1686216387.496814]: 4
[INFO] [1686216388.496830]: 5
[INFO] [1686216389.496828]: 6
[INFO] [1686216390.496855]: 7
[INFO] [1686216390.503832]: I heard 2
[INFO] [1686216391.496830]: 8
[INFO] [1686216392.496856]: 9
[INFO] [1686216393.496890]: 10
[INFO] [1686216394.496953]: 11
[INFO] [1686216395.496850]: 12
[INFO] [1686216395.508812]: I heard 3
[INFO] [1686216396.496810]: 13
[INFO] [1686216397.496806]: 14
[INFO] [1686216398.496805]: 15
[INFO] [1686216399.496846]: 16
[INFO] [1686216400.496842]: 17
[INFO] [1686216400.512905]: I heard 4
[INFO] [1686216401.496842]: 18
[INFO] [1686216402.496794]: 19
[INFO] [1686216403.496824]: 20
[INFO] [1686216404.496812]: 21
[INFO] [1686216405.496868]: 22
[INFO] [1686216405.520205]: I heard 5

回调函数的运行机制是，当有一个消息过来时，就执行回调函数，没有则不执行，这里依照此原则一个一个执行，于是从 1 2 3 4 … 一个一个进行接受。显然，这样无法达到我们希望消息实时更新的想法。

这时候加上队列长度为会变成如下结果，令 queue_size=1

[INFO] [1686216536.836731]: 1
[INFO] [1686216537.837433]: 2
[INFO] [1686216538.838505]: 3
[INFO] [1686216539.838197]: 4
[INFO] [1686216540.838405]: 5
[INFO] [1686216541.838377]: 6
[INFO] [1686216542.838640]: 7
[INFO] [1686216542.845194]: I heard 2
[INFO] [1686216543.838433]: 8
[INFO] [1686216544.837319]: 9
[INFO] [1686216545.837338]: 10
[INFO] [1686216546.838616]: 11
[INFO] [1686216547.838296]: 12
[INFO] [1686216547.853294]: I heard 7
[INFO] [1686216548.838559]: 13
[INFO] [1686216549.837330]: 14
[INFO] [1686216550.838616]: 15
[INFO] [1686216551.838163]: 16
[INFO] [1686216552.838629]: 17
[INFO] [1686216552.860943]: I heard 12
[INFO] [1686216553.838344]: 18
[INFO] [1686216554.838617]: 19
[INFO] [1686216555.837327]: 20
[INFO] [1686216556.837750]: 21
[INFO] [1686216557.838477]: 22
[INFO] [1686216557.868910]: I heard 17

从结果来看，队列长度里面储存的内容是在上一次 callback 执行时更新的，如果callback花费很长时间，这个队列为一的消息并不是最新的，而是上一次 callback 运行时候的值，而不写队列长度则会把所有的数据全部储存，一个一个依次读取

显然，两种结果都不是想要的结果

由于 python 没有 spinOnce 机制，于是我测试了 c ++ 的 ros::spinOnce() 函数，其效果与队列长度设置为1更新机制一样，即使用的消息是上一次执行callback保存的值

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <chrono>
#include <thread>
void callback(const std_msgs::String::ConstPtr& msg)
{
    std::this_thread::sleep_for(std::chrono::seconds(5));
    ROS_INFO("c++ I heard: [%s]", msg->data.c_str());
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "subscriber_node");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe<std_msgs::String>("my_topic", 1, callback);  // 设置队列长度为10

    while (ros::ok()){
        ros::spinOnce();
    }
//    ros::spin();

    return 0;
}

[INFO] [1686236228.724176]: 1
[INFO] [1686236229.725438]: 2
[INFO] [1686236230.725013]: 3
[INFO] [1686236231.725994]: 4
[INFO] [1686236232.725972]: 5
[INFO] [1686236233.726002]: 6
[INFO] [1686236234.726026]: 7
[ INFO] [1686236234.727809610]: c++ I heard: [2]
[INFO] [1686236234.732852]: python I heard 2
[INFO] [1686236235.725834]: 8
[INFO] [1686236236.726048]: 9
[INFO] [1686236237.725904]: 10
[INFO] [1686236238.725657]: 11
[INFO] [1686236239.725623]: 12
[ INFO] [1686236239.730360900]: c++ I heard: [7]
[INFO] [1686236239.741026]: python I heard 7
[INFO] [1686236240.725924]: 13
[INFO] [1686236241.726004]: 14
[INFO] [1686236242.726119]: 15
[INFO] [1686236243.726149]: 16
[INFO] [1686236244.725790]: 17
[ INFO] [1686236244.730969842]: c++ I heard: [12]
[INFO] [1686236244.746302]: python I heard 12

解决方法

使用while not rospy.is_shutdown():，实测会来了一个消息就会开始执行callback

直接上代码

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import time
a = ' '
def callback(data):
    global a
    a = data.data

rospy.init_node('subscriber_node')
rospy.Subscriber('my_topic', String, callback, queue_size=1)

while not rospy.is_shutdown():
    time.sleep(5)
    rospy.loginfo('python %s', a)

输出

[INFO] [1686239068.123173]: 1
[INFO] [1686239069.124314]: 2
[INFO] [1686239070.124508]: 3
[INFO] [1686239071.124797]: 4
[INFO] [1686239072.124445]: 5
[INFO] [1686239073.124328]: 6
[INFO] [1686239073.138271]: python 6
[INFO] [1686239074.124528]: 7
[INFO] [1686239075.124556]: 8
[INFO] [1686239076.124529]: 9
[INFO] [1686239077.124631]: 10
[INFO] [1686239078.124316]: 11
[INFO] [1686239078.146387]: python 11
[INFO] [1686239079.124611]: 12
[INFO] [1686239080.124351]: 13
[INFO] [1686239081.124512]: 14
[INFO] [1686239082.124566]: 15
[INFO] [1686239083.124489]: 16
[INFO] [1686239083.154269]: python 16
[INFO] [1686239084.124850]: 17
[INFO] [1686239085.124540]: 18
[INFO] [1686239086.123901]: 19
[INFO] [1686239087.124629]: 20
[INFO] [1686239088.124192]: 21
[INFO] [1686239088.159253]: python 21