ROS中spinOnce的机制以及如何选择指定Topic进行Callback更新

0 背景

项目中有多个Sensor的Topic需要订阅，Topic的更新频率各不相同，假设Topic A为25Hz，Topic B为10Hz，Topic A为关键数据，需求是当收到TopicA后即进行数据的处理。这里就需要对Callback的调用机制进行区别。

1 SpinOnce机制

SpinOnce与Spin的区别比较简单，这里就不具体说明了。在ROS中，当收到可以激活Callback函数的时机时，会将其压入一个队列，这个队列就是ros::CallbackQueue。当调用Spin或SpinOnce时，就会检查这个队列，如果队列为空，也就是没收到注册的Topic，则返回继续执行主函数；如果不为空，则从其中取出Callback函数来invoke，也就是会依次处理收到的数据。在文档中，提到会处理Callback最新一次的数据，但在实际执行过程中，发现在一次SpinOnce中同一个Topic可能会收到多个数据后进行多次处理，这就背离了项目中需要及时处理TopicA的需求。

[ INFO] [1625046678.916048760, 1624994932.711482997]: 7-PubPart  = 0.006226
[ INFO] [1625046678.916618409, 1624994932.711482997]: PC1 Stamp = 1624994932.629655
[ INFO] [1625046678.916675676, 1624994932.711482997]: Current Laser Stamp = 1624994932.688399
[ INFO] [1625046678.916714987, 1624994932.711482997]: Ls Stamp = 1624994932.688399
Failed to find match for field 'rgb'.
[ INFO] [1625046678.983832186, 1624994932.785528414]: here 1
[ INFO] [1625046678.984639276, 1624994932.785528414]: PC2 Stamp = 1624994932.659626
[ INFO] [1625046678.984701021, 1624994932.785528414]: Current Laser Stamp = 1624994932.759400
[ INFO] [1625046678.984720163, 1624994932.785528414]: Ls Stamp = 1624994932.759400
Failed to find match for field 'rgb'.
[ INFO] [1625046679.037698877, 1624994932.839306189]: here 1
[ INFO] [1625046679.038953597, 1624994932.839306189]: PC1 Stamp = 1624994932.697870
[ INFO] [1625046679.039069509, 1624994932.839306189]: 8-SpinPart  = 0.129240
[ INFO] [1625046679.039086164, 1624994932.839306189]: Loop Time All  = 0.129265
1
2
3
4
5
6
7
8
9
10
11
12
13
14

7-PubPart 到 8-SpinPart中间为一次SpinOnce的执行过程，可以看到其中PC2的Callback执行了一次，但是Laser和PC1的Callback执行了两次。如果Laser或者PC1为关键TopicA的话则错过了执行数据分析的时间，因此需要通过拆分Topic进行执行，如果循环到TopicA则继续执行，这里TopicA即为Laser。

SpinOnce在源码中既是直接调用了

ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0));
1

既是循环执行了上文说的ros::CallbackQueue，为了执行关键Topic，在ros中可以选择一个队列中的项进行执行：
callOne(ros::WallDuration(0));
这个函数会执行队列中的最久的一项执行，并返回执行结果，结果类的可能为
enum CallOneResult { Called, TryAgain, Disabled, Empty }

2 单一Topic执行的实现

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/callback_queue_interface.h>

#include <std_msgs/Empty.h>

void callback_1(const std_msgs::EmptyConstPtr& _msg)
{
    ROS_INFO("Called callback_1\n");
}

void callback_2(const std_msgs::EmptyConstPtr& _msg)
{
    ROS_INFO("Called callback_2\n");
}

int main(int argn, char* args[])
{
    ros::init(argn, args, "callback_q_subscriber");
    ros::NodeHandle nh_1;
    ros::NodeHandle nh_2;

    ros::CallbackQueue queue_1, queue_2;

    nh_1.setCallbackQueue(&queue_1);
    nh_2.setCallbackQueue(&queue_2);

    ros::Subscriber s_1 = nh_1.subscribe("/topic_1", 1, callback_1);
    ros::Subscriber s_2 = nh_2.subscribe("/topic_2", 1, callback_2);

    pthread_t id_1, id_2;

    int i = 20;
    while(i--)
    {
        queue_1.callOne(ros::WallDuration(1.0)); // can also be callAvailable()
    }

    i = 20;
    while(i--)
    {
        queue_2.callOne(ros::WallDuration(1.0)); // can also be callAvailable()
    }

    ROS_INFO("Hurray!");

    return 0;   
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

执行的结果如下：

[ INFO] [1625047229.986131037, 1624994923.101778726]: 7-PubPart  = 0.005340
[ INFO] [1625047229.986607337, 1624994923.101778726]: PC2 Stamp = 1624994922.929261
[ INFO] [1625047229.986667684, 1624994923.101778726]: Callback Once
[ INFO] [1625047229.986754915, 1624994923.101778726]: 7_1-Callback  = 0.005923
[ INFO] [1625047229.986993282, 1624994923.101778726]: PC1 Stamp = 1624994922.935428
[ INFO] [1625047229.987048776, 1624994923.101778726]: Callback Once
[ INFO] [1625047229.987085780, 1624994923.101778726]: 7_1-Callback  = 0.006297
[ INFO] [1625047229.987549144, 1624994923.101778726]: PC2 Stamp = 1624994922.996081
[ INFO] [1625047229.987695149, 1624994923.101778726]: Callback Once
[ INFO] [1625047229.987754998, 1624994923.101778726]: 7_1-Callback  = 0.006961
[ INFO] [1625047229.987884339, 1624994923.101778726]: Current Laser Stamp = 1624994923.035926
[ INFO] [1625047229.987943186, 1624994923.101778726]: Ls Stamp = 1624994923.035926
Failed to find match for field 'rgb'.
[ INFO] [1625047230.012284062, 1624994923.133694720]: here 1
[ INFO] [1625047230.012347083, 1624994923.133694720]: Callback Once
[ INFO] [1625047230.012382432, 1624994923.133694720]: 7_1-Callback  = 0.031593
[ INFO] [1625047230.012473806, 1624994923.133694720]: 8-SpinPart  = 0.031684
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

可以看到，PC2执行了两次，但关键Topic Laser只执行了一次，并且在执行后就结束了Spin的过程，符合项目中的需求。