ROS多线程发布订阅话题信息

下面讲述几种ros订阅多个话题的例子：
1、实现接收两个topic，并发布一个topic，采用的是ros多线程的方法解决。

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

class SubscribeAndPublish  
{  
public:  
  SubscribeAndPublish()  
  {  
    count = 0;
    //Topic you want to publish  
    pub_ = n_.advertise<std_msgs::String>("/chatter_pub", 1000);  

    //Topic1 you want to subscribe  
    sub_ = n_.subscribe("chatter1", 10, &SubscribeAndPublish::callback1, this); 
    //Topic2 you want to subscribe  
    sub2_ = n_.subscribe("chatter2", 10, &SubscribeAndPublish::callback2, this);  
  }  

  void callback1(const std_msgs::String::ConstPtr& msg1)  
  {  
    ROS_INFO("I heard: [%s]", msg1->data.c_str()); 
    //.... do something with the input and generate the output...
    std::stringstream ss;
    ss << "Pub: hello world " << count;
    output.data = ss.str();
    pub_.publish(output);
    ROS_INFO("%s", output.data.c_str()); 
    ++count; 
  }  
  void callback2(const std_msgs::String::ConstPtr& msg2)
  {
  ROS_INFO("I heard: [%s]", msg2->data.c_str());
  ros::Rate loop_rate(0.5);//block chatterCallback2() 0.5Hz
  loop_rate.sleep();
}
private:  
  ros::NodeHandle n_;   
  ros::Publisher pub_;  
  ros::Subscriber sub_;
  ros::Subscriber sub2_; 
  std_msgs::String output;
  int count; 

};//End of class SubscribeAndPublish  

int main(int argc, char **argv)  
{  
  //Initiate ROS  
  ros::init(argc, argv, "subscribe_and_publish");  

  //Create an object of class SubscribeAndPublish that will take care of everything  
  SubscribeAndPublish test;  
  //ros::spin();
  ros::MultiThreadedSpinner s(2);  //多线程
  ros::spin(s);  

  return 0;  
}  

或者：

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

2、利用一个类的不同成员函数实现同一个topic的不同处理(在一个线程中)。

#include "ros/ros.h"  
#include "std_msgs/String.h"  
  
/** 
 * This tutorial demonstrates simple receipt of messages over the ROS system, with 
 * multiple callbacks for the same topic.  See the "listener" tutorial for more 
 * information. 
 */  
  
class Listener  
{  
public:  
  void chatter1(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter1: [%s]", msg->data.c_str()); }  
  void chatter2(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter2: [%s]", msg->data.c_str()); }  
  void chatter3(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter3: [%s]", msg->data.c_str()); }  
};  
  
void chatter4(const std_msgs::String::ConstPtr& msg)  
{  
  ROS_INFO("chatter4: [%s]", msg->data.c_str());  
}  
  
int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "listener");  
  ros::NodeHandle n;  
  
  Listener l;  
  ros::Subscriber sub1 = n.subscribe("chatter", 10, &Listener::chatter1, &l);  
  ros::Subscriber sub2 = n.subscribe("chatter", 10, &Listener::chatter2, &l);  
  ros::Subscriber sub3 = n.subscribe("chatter", 10, &Listener::chatter3, &l);  
  ros::Subscriber sub4 = n.subscribe("chatter", 10, chatter4);  
  
  ros::spin();  
  
  return 0;  
}  

3、利用一个类的不同成员函数实现同一个topic的不同处理(在四个不同线程中)。

#include "ros/ros.h"  
#include "std_msgs/String.h"  
  
#include <boost/thread.hpp>  
  
/** 
 * This tutorial demonstrates simple receipt of messages over the ROS system, using 
 * a threaded Spinner object to receive callbacks in multiple threads at the same time. 
 */  
  
ros::Duration d(0.01);  
  
class Listener  
{  
public:  
  void chatter1(const std_msgs::String::ConstPtr& msg)  
  {  
    ROS_INFO_STREAM("chatter1: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");  
    d.sleep();  
  }  
  void chatter2(const std_msgs::String::ConstPtr& msg)  
  {  
    ROS_INFO_STREAM("chatter2: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");  
    d.sleep();  
  }  
  void chatter3(const std_msgs::String::ConstPtr& msg)  
  {  
    ROS_INFO_STREAM("chatter3: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");  
    d.sleep();  
  }  
};  
  
void chatter4(const std_msgs::String::ConstPtr& msg)  
{  
  ROS_INFO_STREAM("chatter4: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");  
  d.sleep();  
}  
  
int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "listener");  
  ros::NodeHandle n;  
  
  Listener l;  
  ros::Subscriber sub1 = n.subscribe("chatter", 10, &Listener::chatter1, &l);  
  ros::Subscriber sub2 = n.subscribe("chatter", 10, &Listener::chatter2, &l);  
  ros::Subscriber sub3 = n.subscribe("chatter", 10, &Listener::chatter3, &l);  
  ros::Subscriber sub4 = n.subscribe("chatter", 10, chatter4);  
  
  /** 
   * The MultiThreadedSpinner object allows you to specify a number of threads to use 
   * to call callbacks.  If no explicit # is specified, it will use the # of hardware 
   * threads available on your system.  Here we explicitly specify 4 threads. 
   */  
  ros::MultiThreadedSpinner s(4);  
  ros::spin(s);  
  
  return 0;  
}  

4、ros中timers的使用：

#include "ros/ros.h"  
  
/** 
 * This tutorial demonstrates the use of timer callbacks. 
 */  
  
void callback1(const ros::TimerEvent&)  
{  
  ROS_INFO("Callback 1 triggered");  
}  
  
void callback2(const ros::TimerEvent&)  
{  
  ROS_INFO("Callback 2 triggered");  
}  
  
int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "talker");  
  ros::NodeHandle n;  
  
  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);  
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);  
  
  ros::spin();  
  
  return 0;  
}  

编译运行出现以下结果：