Bootstrap

ros自定义消息队列

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;
}
;