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
7-PubPart 到 8-SpinPart中间为一次SpinOnce的执行过程,可以看到其中PC2的Callback执行了一次,但是Laser和PC1的Callback执行了两次。如果Laser或者PC1为关键TopicA的话则错过了执行数据分析的时间,因此需要通过拆分Topic进行执行,如果循环到TopicA则继续执行,这里TopicA即为Laser。
SpinOnce在源码中既是直接调用了
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0));
既是循环执行了上文说的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;
}
执行的结果如下:
[ 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
可以看到,PC2执行了两次,但关键Topic Laser只执行了一次,并且在执行后就结束了Spin的过程,符合项目中的需求。