ros中默认消息队列只有一个,也就是说,即使你的话题在另外一个线程注册订阅,也会在主线程通过ros::spin / ros::spinOnce来获取到该话题的消息,并进入回调。
如果说,我们主线程需要固定频率来进行计算,而有一个服务/话题需要消耗很长时间,由于只有一个消息队列该服务会有阻塞到主线程的运算的
这里就引入了自定义消息队列,我们可以将比较耗时间的服务,或者需要分离的两类计算分别放在不同的队列中
比较常见的方式
ros::NodeHandle nh;
nh.setCallbackQueue(&my_callback_queue);
此时通过 nh节点句柄注册的消息或者服务都不会进入主消息队列,也就是通过ros::spin / ros::spinOnce无法在获取到该话题/服务的回调,可以使用ros::CallbackQueue::callAvailable() and ros::CallbackQueue::callOne()来手动操作
my_callback_queue.callAvailable(ros::WallDuration());
各种*Spinner对象也可以使用指向回调队列的指针,而不是使用默认的:
ros::AsyncSpinner spinner(0, &my_callback_queue);
spinner.start();
或者
ros::MultiThreadedSpinner spinner(0);
spinner.spin(&my_callback_queue);
以下为案例
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/Bool.h"
#include <thread>
using namespace std;
class CallOnce
{
private:
public:
CallOnce();
~CallOnce();
ros::NodeHandle nh;
ros::NodeHandle nh2;
ros::Subscriber test1_sub_;
ros::Subscriber test2_sub_;
//自定义消息队列
ros::CallbackQueue my_queue;
void callback1(const std_msgs::BoolConstPtr& msg);
void callback2(const std_msgs::BoolConstPtr& msg);
thread* t2;
void thread2();
};
CallOnce::CallOnce()
{
test1_sub_ = nh.subscribe("/test1", 1, &CallOnce::callback1, this);
//线程2
t2 = new thread(&CallOnce::thread2, this);
}
CallOnce::~CallOnce()
{
}
void CallOnce::callback1(const std_msgs::BoolConstPtr& msg)
{
cout << "callback 1" << endl;
}
void CallOnce::callback2(const std_msgs::BoolConstPtr& msg)
{
cout << "callback 2" << endl;
sleep(2);
}
void CallOnce::thread2()
{
//将nh2节点句柄的消息队列设为my_queue
nh2.setCallbackQueue(&my_queue);
test2_sub_ = nh2.subscribe("/test2", 1, &CallOnce::callback2, this);
//等待回调
ros::AsyncSpinner spinner(0, &my_queue);
spinner.start();
ros::waitForShutdown();
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "call_once");
CallOnce call_once;
ros::Rate rate(10);
while (ros::ok())
{
cout << " thread 1 spin" << endl;
ros::spinOnce();
cout << " thread 1 spin end" << endl;
rate.sleep();
}
ros::shutdown();
call_once.t2->join();
return 0;
}