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