【ROS】ubuntu16.04下实现多线程话题订阅和发布

最近博主在ROS的学习中，接触到了多线程控制，以前一直不懂，也不知道怎么编程，今天博主在这里关于ROS中多话题订阅和发布谈谈自己的见解
我们都知道，在ROS的学习过程中，很容易实现单个话题的订阅和发布，但是在实际的项目需求中，往往需要同时订阅多个话题的数据进行处理，再进行发布，对数据进行整合处理，这就要求我们对C++的多线程编程有一定的了解了。

一、传统的多线程发布和订阅程序

我们学习ROS的话题订阅和发布时，都是以一个话题的发布和订阅为基础进行学习的，这里博主以两个话题的发布和订阅进行说明，程序如下

1.mulit_topic_pub.cpp

//包含ros相关的API头文件
#include "ros/ros.h"
//包含在std_msgs库里的String类型
#include "std_msgs/String.h"
#include <sstream>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "multi_pub");
	ros::NodeHandle n;
	//创建发布者，消息类型为std_msgs::String，队列长度为1
	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);//10HZ
	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;
}

2.multi_topic_sub.cpp

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

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

可以看到，发布程序中，以10hz的频率发布了chatter1和chatter2两个话题，在订阅程序中，回调函数1中加入了2s的延时，导致了回调函数2也只能2s才能接收到一个数据，为了是回调函数2能正常接收数据，为此我们要探究以下多线程控制的相关技术。

二、多线程函数介绍

首先，在多线程编程中，有两个常用的多线程函数，分别是ros::MultiThreadedSpinner和ros::AsyncSpinner，他们可以在一个节点中开辟多个线程，这里博主参考的是官方的多线程教程中的相关介绍，链接如下
链接: http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning.

1.ros::MultiThreadedSpinner

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

2.ros::AsyncSpinner

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

三、多线程发布和订阅程序

在原有mulit_topic_sub.cpp文件的基础上进行修改，修改如下，multi_thread_sub.cpp文件是根据ros::MultiThreadedSpinner函数进行修改的，multi_thread_sub2.cpp文件是根据ros::AsyncSpinner函数进行修改的

1.mulit_thread_sub.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <boost/thread.hpp>

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

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

void multiThreadListener::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 multiThreadListener::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");

  multiThreadListener listener_obj;
  ros::MultiThreadedSpinner s(2);
  ros::spin(s);

  return 0;
}

2.mulit_thread_sub2.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <boost/thread.hpp>

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

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

void multiThreadListener::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 multiThreadListener::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");

  multiThreadListener listener_obj;
  
  ros::AsyncSpinner spinner(2); // Use 2 threads
  spinner.start();
  ros::waitForShutdown();

  return 0;
}

代码运行效果一致，可以很清楚的看到，两个订阅者之间并没有相互阻塞，同时完成了对话题的订阅和信息打印

相比较之下，AsyncSpinner比MultiThreadedSpinner更优，它有start() 和stop() 函数，并且在销毁的时候会自动停止，这样我们就完成了ubuntu16.04下多话题订阅和发布的目的了