ros tf 发布和订阅demo（附c++代码）

发布demo代码如下：

#include 
#include 
int main(int argc, char** argv){
    //初始化发布节点，demo_tf_broadcaster是节点的名字
    ros::init(argc, argv, "demo_tf_broadcaster");
    //创建一个tf发布对象
    static tf::TransformBroadcaster br;
    while(ros::ok())
    {
      //创建一个tf对象
      tf::Transform transform;
      //设置tf的位移量
      transform.setOrigin( tf::Vector3(0.3369507, -0.37187648, -1.80606454));
      //初始化一个四元数变量
      tf::Quaternion q;
      //设置俯仰角
      q.setRPY(0.93040044, 0.43019939, -3.08609162);
      //将四元数存入tf对象中，tf只能存储四元数。
      transform.setRotation(q);
      //"robot" 是父坐标，"world"是子坐标
      //转换关系是从robot坐标系转到world坐标系
      br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "robot"));
      ROS_INFO("The transform has broadcasted!");
      ros::Duration(0.05).sleep();
    }
    return 0;
};

订阅demo代码如下：

#include 
#include 
int main(int argc, char** argv){
    ros::init(argc, argv, "demo_tf_listener");  //初始化一个监听器

    ros::NodeHandle n; //创建一个句柄

    tf::TransformListener listener; //声明一个监听器 

    float saved_rx = 0.0;
    float saved_ry = 0.0;
    float saved_rz = 0.0;

    ros::Rate rate(10.0);
    while (ros::ok()){
        tf::StampedTransform transform;

        try{
            ros::Time now=ros::Time::now();
            //订阅的是以robot作为父坐标系，world为子坐标系
            listener.waitForTransform("world","robot",now,ros::Duration(1.0));
            listener.lookupTransform("/world", "/robot",now, transform);
            saved_rx = transform.getOrigin().x();
            saved_ry = transform.getOrigin().y();
            saved_rz = transform.getOrigin().z();
            ROS_INFO("Transform rotation(RPY) received is %f %f %f",saved_rx,saved_ry,saved_rz);

        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }
        ROS_INFO("Saved transform is %f %f %f",saved_rx,saved_ry,saved_rz);
        rate.sleep();
    }
  return 0;
}