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

#include <ros/ros.h>
#include <topic_demo/RPY.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rpytalker");
    ros::NodeHandle nh;

    topic_demo::RPY msg;
    msg.roll = 1.0;
    msg.pitch = 1.0;
    msg.yaw = 1.0;

    ros::Publisher pub = nh.advertise<topic_demo::RPY>("rpy_info", 1);
 
    ros::Rate loop_rate(2.0);

    while (ros::ok())
    {
        //以指數增長，每隔0.5秒更新一次
        msg.roll = 1.01 * msg.roll ;
        msg.pitch = 1.02 * msg.pitch;
        msg.yaw = 1.03 * msg.yaw;

        msg.header.stamp = ros::Time::now(); 

        ROS_INFO("Talker: rpy: r = %f, p = %f, y = %f ",  msg.roll ,msg.pitch, msg.yaw );

        pub.publish(msg);

        loop_rate.sleep();
    }

    return 0;
} 

 

listener.cpp

/*
 
這里讀取 rpy ，然后 加個1, 以原有的 時間戳發布出去
 
*/
 
#include <ros/ros.h>
#include <topic_demo/RPY.h>
#include <std_msgs/Float32.h>
 
void rpyCallback(const topic_demo::RPY::ConstPtr &msg , ros::Publisher *pub)
{ 
    float roll = msg->roll;
    float pitch = msg->pitch;
    float yaw = msg->yaw;
 
    std_msgs::Header h = msg->header;
 
    ROS_INFO("Listener rpy : r = %f, p = %f, y = %f, time: %d", roll, pitch, yaw, h.stamp );
 
    topic_demo::RPY new_msg;
    new_msg.roll = roll + 1.0;
    new_msg.pitch = pitch + 1.0;
    new_msg.yaw = yaw + 1.0;
 
    new_msg.header = h;
 
    pub->publish(new_msg); // 不能是 pub.publish， 必須是 pub->publish
 
    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 );
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "rpylistener");
    ros::NodeHandle n;
     
    ros::Publisher pub = n.advertise<topic_demo::RPY>("rpy_info_plus", 1);
 
    // ros::Subscriber sub = n.subscribe("rpy_info", 1, rpyCallback);
 
    ros::Subscriber sub = n.subscribe<topic_demo::RPY>("rpy_info", 0, boost::bind(&rpyCallback, _1, &pub));
 
    ros::spin();
    return 0;
}