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一样,测试学习用而已。