ros同时接收多话题并发布

ros同时接收多话题并发布

实现接收两个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); 

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

void SubscribeAndPublish::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();
}

附：发布chatter1

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

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker1");

  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter1", 1000);

  ros::Rate loop_rate(10);
  int count = 0;

  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "chatter1: hello world " << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

发布chatter2

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

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker2");

  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter2", 1000);

  ros::Rate loop_rate(10);
  int count = 0;

  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "chatter2: hello world " << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

两个chatter一样，测试学习用而已。