ROS：ROS多線程訂閱消息(ros::asyncspinner)

對於一些只訂閱一個話題的簡單節點來說，我們使用ros::spin()進入接收循環，每當有訂閱的話題發佈時，進入回調函數接收和處理消息數據。但是更多的時候，一個節點往往要接收和處理不同來源的數據，並且這些數據的產生頻率也各不相同，當我們在一個回調函數裏耗費太多時間時，會導致其他回調函數被阻塞，導致數據丟失。這種場合需要給一個節點開闢多個線程，保證數據流的暢通。 
爲了觀察不同話題的消息被阻塞的情況，可以參考以下實驗代碼 

publisher：

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>


int main(int argc, char **argv)
{

	ros::init(argc, argv, "multi_pub");


	ros::NodeHandle n;


	ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter1", 1);
	ros::Publisher pub2 = n.advertise<std_msgs::String>("chatter2", 1);

	ros::Rate loop_rate(10);


	int count = 0;
	while (ros::ok())
	{

		std_msgs::String msg;
		std::stringstream ss;
		ss << "hello world " << count;
		msg.data = ss.str();

		std_msgs::String msg2;
		std::stringstream ss2;
		ss2 << "hello " << count;
		msg2.data = ss2.str();

		ROS_INFO("%s", msg.data.c_str());
		ROS_INFO("%s", msg2.data.c_str());

		chatter_pub.publish(msg);
		pub2.publish(msg2);

		ros::spinOnce();

		loop_rate.sleep();
		++count;
	}


	 return 0;
}

subscriber

#include "ros/ros.h"
#include "std_msgs/String.h"

class multiReceiver
{
public:
	multiReceiver()
	{	
		sub = nh.subscribe("chatter1", 1, &multiReceiver::chatterCallback1,this);
		sub2 = nh.subscribe("chatter2", 1, &multiReceiver::chatterCallback2,this);
	}
	void chatterCallback1(const std_msgs::String::ConstPtr& msg);
	void chatterCallback2(const std_msgs::String::ConstPtr& msg);

private:
	ros::NodeHandle nh;
	ros::Subscriber sub;
	ros::Subscriber sub2;
  
};


void multiReceiver::chatterCallback1(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
  ros::Rate loop_rate(0.5);//block chatterCallback2()
  loop_rate.sleep();
}

void multiReceiver::chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
  

int main(int argc, char **argv)
{

  ros::init(argc, argv, "multi_sub");

  multiReceiver recOb;
  ros::spin();

  return 0;
}

可以看到，發佈程序中，以10hz的頻率發佈了chatter1和chatter2兩個話題，在訂閱程序中，回調函數1中加入了2s的延時，導致了回調函數2也只能2s才能接收到一個數據，爲了是回調函數2能正常接收數據，我研究一下在一個ROS節點中開闢多個線程的方法。

在ROS中，有兩種方法可以在一個節點中開闢多個線程 
1.ros::MultiThreadedSpinner 
MultiThreadedSpinner類似於ros::spin()，在構造過程中可以指定它所用線程數，但如果不指定線程數或者線程數設置爲0，它將在每個cpu內核開闢一個線程。 

用法如下

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

2.ros::AsyncSpinner 

AsyncSpinner比MultiThreadedSpinner更優，它有start() 和stop() 函數，並且在銷燬的時候會自動停止。下面的用法等價於上面的MultiThreadedSpinner例子。

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

• 注意：ros::waitForShutdown() 函數，不會觸發單獨spin()，所以該例子將會4個線程同時循環。

  Please note that the ros::waitForShutdown() function does not spin on its own, so the example above will spin with 4 threads in total.

以上代碼片參考了ROS wiki

源碼：AsyncSpinner API (Jade)