ROS 中的多线程、Nodelet、DDS、QoS、Pluginlib

对于一些只订阅一个话题的简单节点来说，我们使用ros::spin()进入接收循环，每当有订阅的话题发布时，进入回调函数接收和处理消息数据。但是更多的时候，一个节点往往要接收和处理不同来源的数据，并且这些数据的产生频率也各不相同，当我们在一个回调函数里耗费太多时间时，会导致其他回调函数被阻塞，导致数据丢失。这种场合需要给一个节点开辟多个线程，保证数据流的畅通。
https://blog.csdn.net/wengge987/article/details/50619851

roscpp 的api文档：

https://docs.ros.org/api/roscpp/html/namespaceros.html

多线程：

http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

http://blog.chinaunix.net/uid-27875-id-5817906.html

 

单线程调用

• ros::spin()
  单线程根据需要只调用一次
  ros::spinOnce()

•  自定义自己的rose::spin，自定义调用频率
  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1))

• 只调用一次
  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0));

ros自带多线程调用：

• ros::MultiThreadedSpinner ，自定义多个线程管理

ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown

•  ros :: AsyncSpinner、一个更有用的线程微调器是AsyncSpinner。它具有start（）和stop（）调用，而不是阻塞spin（）调用，并且销毁后将自动停止，waitForShutdown不会自动创建线程，可以认为是写在while外边的spinOnce. while内仅仅需要sleep即可. 这样主线程,子线程可以并行执行.

ros::AsyncSpinner s(4);
  s.start();

  ros::Rate r(5);
  while (ros::ok())
  {
    ...
    r.sleep();
  }

•  CallbackQueue::callAvailable() 将获取队列中当前的所有内容并调用它们；callOne()将仅调用队列中最旧的回调

#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/String.h"
 
#include <boost/thread.hpp> 
/**
 * tutorial demonstrates the use of custom separate callback queues that can be processed
 * independently, whether in different threads or just at different times. 
 * 演示了自定义独立回调队列的使用，
 * 这些回调队列可以在不同的线程中独立处理，也可以在不同的时间进行处理。
 */
 
void chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "] (Main thread)");
}
 
//主线程中的调用
void chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" 
        << boost::this_thread::get_id() << "]");
}
 
/** 
 * The custom queue used for one of the subscription callbacks
 */
ros::CallbackQueue g_queue; //第一步:用于订阅回调的自定义队列   

void callbackThread()
{
  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
 
  ros::NodeHandle n;
  while (n.ok())
  {
    //第四步: 执行自定义队列中的回调函数. 
        // CallbackQueue类有两种调用内部回调的方法:callAvailable()和callOne()。
        // callAvailable()将获取队列中当前的所有内容并调用它们。callOne()将简单地调用队列上最古老的回调。
    g_queue.callAvailable(ros::WallDuration(0.01));  
  }
}
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_with_custom_callback_processing");
  ros::NodeHandle n;
 
  /**
   * The SubscribeOptions structure lets you specify a custom queue to use for a specific 
   * subscription. SubscribeOptions结构允许您指定用于特定订阅的自定义队列
   * You can also set a default queue on a NodeHandle using the 
   * NodeHandle::setCallbackQueue() function. 
       * 还可以使用NodeHandle::setCallbackQueue()函数在节点句柄上设置默认队列
   *
   * AdvertiseOptions and AdvertiseServiceOptions offer similar functionality.
   * 对于话题发布者, 有 AdvertiseOptions 和 AdvertiseServiceOptions  可以使用
   */
  //第二步: 声明订阅或者发布选项, 然后和订阅器/发布器绑定在一起  
  ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::String>
        ("chatter", 1000, chatterCallbackCustomQueue, ros::VoidPtr(), &g_queue);
  ros::Subscriber sub = n.subscribe(ops);   
 
  ros::Subscriber sub2 = n.subscribe("chatter", 1000, chatterCallbackMainQueue);
 
  //第三步: 声明线程.
  boost::thread chatter_thread(callbackThread); 
  ROS_INFO_STREAM("Main thread id=" << boost::this_thread::get_id());
 
  ros::Rate r(1);
  while (n.ok())
  {
    ros::spinOnce();
    r.sleep(); 
  }
 
  chatter_thread.join();
 
  return 0;
}

 

 

• roscpp还允许分配自定义回调队列并分别为其提供服务。可以使用以下两种之一来完成此操作：

1. 每个subscription（），advertise（），advertiseService（）等。

2. 每个节点句柄

(1) is possible using the advanced versions of those calls that take a *Options structure. See the API docs for those calls for more information.

(2) is the more common way:

ros::NodeHandle nh;
nh.setCallbackQueue(&my_callback_queue);

这使所有订阅，服务，计时器等都通过my_callback_queue而不是roscpp的默认队列进行回调。这意味着ros :: spin（）和ros :: spinOnce（）将不会调用这些回调。相反，您必须单独服务该队列。您可以使用ros :: CallbackQueue :: callAvailable（）和ros :: CallbackQueue :: callOne（）方法手动进行操作：

my_callback_queue.callAvailable(ros::WallDuration());
  // alternatively, .callOne(ros::WallDuration()) to only call a single callback instead of all available

 

各种* Spinner对象还可以使用指向要使用的回调队列的指针，而不是默认对象：

ros::AsyncSpinner spinner(0, &my_callback_queue);
spinner.start();
---
ros::MultiThreadedSpinner spinner(0);
spinner.spin(&my_callback_queue);

出于多种原因，将回调分为不同的队列可能很有用。一些示例包括：

1. 长期运行的服务。为服务分配自己的回调队列，该队列在单独的线程中得到服务，这意味着可以保证该服务不会阻止其他回调。
2. 线程化特定于计算的回调。与长期运行的服务情况类似，这允许您对特定的回调进行线程化，同时为其余应用程序保持单线程回调的简单性。

 

boost中的多线程调用

https://www.boost.org/doc/libs/1_67_0/doc/html/thread/thread_management.html#thread.thread_management.tutorial

多个不同的topic 用一个回调处理

void callback(const ImageConstPtr& image_color_msg,
        const ImageConstPtr& image_depth_msg,
        const CameraInfoConstPtr& info_color_msg,
        const CameraInfoConstPtr& info_depth_msg)
{
  ....
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vision_node");
 
    ros::NodeHandle nh;
 
    //不同类型的topic订阅
    message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_raw", 1);
    message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth_registered/image_raw", 1);
    message_filters::Subscriber<CameraInfo> info_color_sub(nh,"/camera/rgb/camera_info", 1);
    message_filters::Subscriber<CameraInfo> info_depth_sub(nh,"/camera/depth_registered/camera_info", 1);
    pointcloud_pub = nh.advertise<PointCloud> ("mypoints", 1);
 
    //定义规则
    typedef sync_policies::ApproximateTime<Image, Image, CameraInfo, CameraInfo> MySyncPolicy;
    //规则化实例
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_color_sub, image_depth_sub, info_color_sub, info_depth_sub);
    //绑定回调
    sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));  // _1 代表实力的第一个对象.
 
    ros::spin();
 
    return 0;
}