tf中静态坐标订阅方实现

/*
订阅方：订阅发布的坐标相对关系，传入一个坐标点，调用·tf实现转换

流程：
1.包含头文件
2.编码，初始化节点 Nodehandle
3.创建订阅对象 ----> 订阅坐标相对关系
4组织一个坐标点的数据
5.转换算法，需要调用tf内置函数
6.最后输出
*/

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

int main(int argc, char *argv[])
{
    // 2.编码，初始化节点 Nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建订阅对象  ----> 订阅坐标相对关系
    //3.1创建一个buffer缓存（#include "tf2_ros/buffer.h"）
    tf2_ros::Buffer buffer;
    //3.2创建监听对象（将订阅的数据存入buffer，#include "tf2_ros/transform_listener.h"）    
    tf2_ros::TransformListener listener(buffer);
    // 4.组织一个坐标点的数据
    geometry_msgs::PointStamped ps;	//#include "geometry_msgs/PointStamped.h"
    ps.header.frame_id="laser";
    ps.header.stamp=ros::Time::now();
    ps.point.x=2.0;
    ps.point.y=3.0;
    ps.point.z=5.0;
    //休眠
    ros::Duration(2).sleep();
    // 5.转换算法，需要调用tf内置函数
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心代码实现
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"base_link");

        // 6.最后输出
        ROS_INFO("转换的坐标系：（%2.f,%2.f,%2.f）,参考的坐标系：%s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
        );
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}