TF2坐标转换1-静态转换

我们在进行机器人操作的时候会有一个信息的输入口，比如雷达探测器、激光探测器、深度相机等，但这个信息输入口的输入坐标和机器人本身的坐标是并不相等的，需要进行一些坐标变化，此时我们需要引入TF2。

我们引入以下几个package：tf2,tf2_ros,tf2_geometry_msgs，以供我们进行使用。

我们先写下一个发布者文档：

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc,char* argv[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static01");
    tf2_ros::StaticTransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped ts;

    ts.header.seq=100;
    ts.header.stamp=ros::Time::now();
    ts.header.frame_id="base_link";
    ts.child_frame_id="laser";
    ts.transform.translation.x=0.2;
    ts.transform.translation.y=0.0;
    ts.transform.translation.z=0.5;

    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x=qtn.getX();
    ts.transform.rotation.y=qtn.getY();
    ts.transform.rotation.z=qtn.getZ();
    ts.transform.rotation.w=qtn.getW();

    broadcaster.sendTransform(ts);
    ros::spin();

    return 0;
}

我们接下来对这几个参数和函数进行解析：

ts.header.seq: 序列号；

ts.header.stamp:ts对象的时间戳，静态转换时候可以使用ros::Time::now()获取现在的时间值，但如果进行动态坐标转换订阅时候需要用ros::Time(0.0)；

ts.header.frame_id：基准坐标系，一般设置为"base_link"；

ts.header.child_frame:子坐标系，这个可以理解为基准坐标系的坐标位置；

ts.transform.translation.x/y/z:是子坐标系的相对坐标;

ts.transform.rotation.x/y/z/w:这是旋转参数转化出来的欧拉四元数;

boardcaster.sendTransform()发布静态坐标

然后我们再写下一个订阅者文档：

#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[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle n;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    geometry_msgs::PointStamped ps;
    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(1).sleep();
    ros::Rate rate(10);

    while(ros::ok()){
        geometry_msgs::PointStamped ps_out;
        
        try{
            ps_out=buffer.transform(ps,"base_link");
            ROS_INFO("transformed location:( %.2f , %.2f , %.2f ), the reference frame: %s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
            );
        }
        catch(const std::exception& e)
        {
           ROS_INFO("exception message: %s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

对相应参数和函数进行解析：

    tf2_ros::Buffer buffer;//设置缓存器
    tf2_ros::TransformListener listener(buffer);//监视器绑定到缓存器上
    geometry_msgs::PointStamped ps;//坐标对象
    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;

用try这个来检查报错可以应对一开始没有找到相应发布方的报错，防止程序无法继续执行。

然后我们在配置CMakeList.txt文件，重新配置资源环境，最后执行这两个文件可以得到我们的结果。

然后我们可以在rviz中查看，将一个fixed frame切换为base_link，然后再在add中加入TF就可以查看这两个坐标之间的相对关系。