ros 在callback中publish （不用类的方法）
参考链接：
https://answers.ros.org/question/214386/how-to-publish-a-message-in-a-callback-function/
我的意图是接收到一个自定义的带时间戳的消息，做简单操作后，再以原来的时间戳发布出去。
自定义的 msg 叫 RPY
Header header
float32 roll
float32 pitch
float32 yaw
talker.cpp
1 #include <ros/ros.h> 2 #include <topic_demo/RPY.h> 3 4 int main(int argc, char **argv) 5 { 6 ros::init(argc, argv, "rpytalker"); 7 ros::NodeHandle nh; 8 9 topic_demo::RPY msg; 10 msg.roll = 1.0; 11 msg.pitch = 1.0; 12 msg.yaw = 1.0; 13 14 ros::Publisher pub = nh.advertise<topic_demo::RPY>("rpy_info", 1); 15 16 ros::Rate loop_rate(2.0); 17 18 while (ros::ok()) 19 { 20 //以指数增长，每隔0.5秒更新一次 21 msg.roll = 1.01 * msg.roll ; 22 msg.pitch = 1.02 * msg.pitch; 23 msg.yaw = 1.03 * msg.yaw; 24 25 msg.header.stamp = ros::Time::now(); 26 27 ROS_INFO("Talker: rpy: r = %f, p = %f, y = %f ", msg.roll ,msg.pitch, msg.yaw ); 28 29 pub.publish(msg); 30 31 loop_rate.sleep(); 32 } 33 34 return 0; 35 }
listener.cpp
1 /* 2 3 这里读取 rpy ，然后 加个1, 以原有的 时间戳发布出去 4 5 */ 6 7 #include <ros/ros.h> 8 #include <topic_demo/RPY.h> 9 #include <std_msgs/Float32.h> 10 11 void rpyCallback(const topic_demo::RPY::ConstPtr &msg , ros::Publisher *pub) 12 { 13 float roll = msg->roll; 14 float pitch = msg->pitch; 15 float yaw = msg->yaw; 16 17 std_msgs::Header h = msg->header; 18 19 ROS_INFO("Listener rpy : r = %f, p = %f, y = %f, time: %d", roll, pitch, yaw, h.stamp ); 20 21 topic_demo::RPY new_msg; 22 new_msg.roll = roll + 1.0; 23 new_msg.pitch = pitch + 1.0; 24 new_msg.yaw = yaw + 1.0; 25 26 new_msg.header = h; 27 28 pub->publish(new_msg); // 不能是 pub.publish， 必须是 pub->publish 29 30 ROS_INFO("Listener plus : r = %f, p = %f, y = %f, time: %d", new_msg.roll, new_msg.pitch, new_msg.yaw, new_msg.header.stamp ); 31 } 32 33 int main(int argc, char **argv) 34 { 35 ros::init(argc, argv, "rpylistener"); 36 ros::NodeHandle n; 37 38 ros::Publisher pub = n.advertise<topic_demo::RPY>("rpy_info_plus", 1); 39 40 // ros::Subscriber sub = n.subscribe("rpy_info", 1, rpyCallback); 41 42 ros::Subscriber sub = n.subscribe<topic_demo::RPY>("rpy_info", 0, boost::bind(&rpyCallback, _1, &pub)); 43 44 ros::spin(); 45 return 0; 46 }
