ROS自学实践(9)：TF坐标变换实例解析

一、前言

在进行SLAM建图或者自动驾驶系统开发的时候，往往需要多个传感器，比如激光雷达、相机、毫米波雷达、里程计等等，由于机器人本身具有自己的坐标系，每个传感器又有自己的坐标系，在建图的时候还涉及世界坐标系，了解各个坐标系之间的转换至关重要，ROS给我们提供了一个很好的坐标变换工具——TF包，下面简单介绍一下相关的使用。

二、TF简介²

基本数据类型：
数据类型定义在tf/transform_datatypes.h中

类型	TF
Quaternion	tf::Quaternion
Vector	tf::Vector3
Point	tf::Point
Pose	tf::Pose
Transform	tf::Transform

三、建立ROS实验包

1.创建ROS包

mkdir -p tf/src
cd tf/src
catkin_create_pkg mytftest roscpp tf geometry_msgs
cd ..
catkin_make

2.新建tf_broadcaster.cpp，发布坐标变换

#include 
#include 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_publisher");//初始化ROS节点与节点名称
    ros::NodeHandle n;                    //创建节点的句柄
    ros::Rate loop_rate(100);             //控制节点运行的频率,与loop.sleep共同使用

    tf::TransformBroadcaster broadcaster; //创建tf广播器
    
    tf::Transform base2laser;
    tf::Quaternion q;
    q.setRPY(0,0,0);
    base2laser.setRotation(q);              //设置旋转坐标
    base2laser.setOrigin(tf::Vector3(1,0,0));//设平移坐标，laser在base的(1,0,0)位置

    while (n.ok())
    {
        //循环发布坐标变换，两种方式
        broadcaster.sendTransform(tf::StampedTransform(base2laser,ros::Time::now(),"base_link","laser_link"));
        //broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 0), tf::Vector3(1, 0.0, 0)),ros::Time::now(),"base_link", "base_laser"));
        loop_rate.sleep();
    }
    return 0;
}

修改CmakeLists.txt文件：

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

启动节点：rosrun mytftest tf_broadcaster 这里第一个参数是ROS包名。第二个参数是可执行文件名add_executable()的第一个参数。
在rviz中显示：

2.新建tf_listener.cpp，监听坐标变换

#include 
#include 
#include 


int main(int argc,char** argv)
{
    ros::init(argc,argv,"tf_subscriber");       //初始化ROS节点与节点名称
    ros::NodeHandle n;                         //创建节点的句柄
    ros::Rate loop_rate(100);                   //控制节点运行的频率,与loop.sleep共同使用

    //创建一个监听器，监听所有tf变换，缓冲10s
    tf::TransformListener listener;             

    geometry_msgs::PointStamped laser_pos;      //创建一个点
    laser_pos.header.frame_id = "laser_link";   //将这个点绑定到雷达坐标系下
    laser_pos.header.stamp = ros::Time();
    laser_pos.point.x = 1;
    laser_pos.point.y = 0;
    laser_pos.point.z = 0;


    geometry_msgs::PointStamped base_pos;      //创建一个点

    while (n.ok())
    {
        //tf::TransformListener listener(ros::Duration(10))//等待10s，如果10s之后都还没收到消息，那么之前的消息就被丢弃掉。
        //监听两个坐标之间的变换关系
        listener.waitForTransform("base_link","laser_link",ros::Time(0),ros::Duration(3));//ros::Time(0)表示使用缓冲中最新的tf数据
        listener.transformPoint("base_link",laser_pos,base_pos);//将laser_link中的点变换到base_link中去

        ROS_INFO("laser_link: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_pos.point.x, laser_pos.point.y, laser_pos.point.z,
        base_pos.point.x, base_pos.point.y, base_pos.point.z, base_pos.header.stamp.toSec());
        
        loop_rate.sleep();
    }

修改CmakeLists.txt文件：

add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_listener ${catkin_LIBRARIES})

运行上述两个节点，就得到了在机器人坐标系下，雷达中的一个点的坐标

四、相关函数的使用

1.waitForTransform()函数^[3]

作用：等待变换在tf树中生效
示例：

#python版本
listener.waitForTransform("/turtle2", "/carrot1", now, rospy.Duration(4.0))
#C++版本
ros::Duration dur (0.5);
tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur)

(1) waitForTransform()有四个参数：

• /turtle2，父类坐标系
• /carrot1，子类坐标系
• rospy.Time()，这个时间的变换，就是变换的时刻
• rospy.Duration(4.0)，等待最长的时间段

(2) waitForTransform() 实际上会阻塞，阻塞时间由第四个参数决定，直到两个坐标系变换开始。（通常是几毫秒）
(3) waitForTransform()常常会调用两次，一开始产生turtle2时候 ，在第一次调用waitForTransform()前，TF可能还没有更新。第一次调用waitForTransform()就会等待，直到/turtle2坐标系开始广播。第二次调用waitForTransform()用Now参数才能获取到值。

2.lookupTransform()函数

作用：获取某一个时间的坐标变换，与waitForTransform配合使用。

   rate = rospy.Rate(10.0)
   listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try: 
            now = rospy.Time.now()
            listener.waitForTransform('/turtle2', '/turtle1', now, rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

(1)这个lookupTransform()API，有六个参数：

• 变换从坐标系turtle2
• 到turtle1坐标系
• 在now时间
• 变换结果保存的变量

示例：
坐标变换的发送方还是采用上面的还是采用tf_broadcaster.cpp，为了方便查看，平移坐标改成了

    base2laser.setRotation(q);              //设置旋转坐标
    base2laser.setOrigin(tf::Vector3(1,0,1));//设平移坐标，laser在base的(1,0,0)位置

修改tf_listener.cpp

#include 
#include 
#include 
#include 

int main(int argc,char** argv)
{
    ros::init(argc,argv,"tf_subscriber");       //初始化ROS节点与节点名称
    ros::NodeHandle n;                         //创建节点的句柄
    ros::Rate loop_rate(100);                   //控制节点运行的频率,与loop.sleep共同使用

    //创建一个监听器，监听所有tf变换，缓冲10s
    tf::TransformListener listener;             

    geometry_msgs::PointStamped laser_pos;      //创建一个点
    laser_pos.header.frame_id = "laser_link";   //将这个点绑定到雷达坐标系下
    laser_pos.header.stamp = ros::Time();
    laser_pos.point.x = 1;
    laser_pos.point.y = 0;
    laser_pos.point.z = 0;

    geometry_msgs::PointStamped base_pos;      //创建一个点

    tf::TransformListener tf_;

    while (n.ok())
    {
        //tf::TransformListener listener(ros::Duration(10))//等待10s，如果10s之后都还没收到消息，那么之前的消息就被丢弃掉。
        //监听两个坐标之间的变换关系
        // listener.waitForTransform("base_link","laser_link",ros::Time(0),ros::Duration(3));//ros::Time(0)表示使用缓冲中最新的tf数据
        // listener.transformPoint("base_link",laser_pos,base_pos);//将laser_link中的点变换到base_link中去

        // ROS_INFO("laser_link: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        // laser_pos.point.x, laser_pos.point.y, laser_pos.point.z,
        // base_pos.point.x, base_pos.point.y, base_pos.point.z, base_pos.header.stamp.toSec());
        if (tf_.waitForTransform("base_link","laser_link",ros::Time(0),ros::Duration(3)))
        {
            tf::StampedTransform laserTransform;
            tf_.lookupTransform("base_link","laser_link", ros::Time(0), laserTransform);
            std::cout << "laserTransform.getOrigin().getX(): " << laserTransform.getOrigin().getX() << std::endl;
            std::cout << "laserTransform.getOrigin().getY(): " << laserTransform.getOrigin().getY() << std::endl;
            std::cout << "laserTransform.getOrigin().getZ(): " << laserTransform.getOrigin().getZ() << std::endl;
        }  
        loop_rate.sleep();
    }

}

laserTransform就是当初设置的坐标变换矩阵，将这个矩阵左乘laser_link下的点的位姿就可以得到在base_link下的坐标。
参考文献：
1.https://www.guyuehome.com/355
2.https://www.ncnynl.com/category/roscpp/2/
https://www.ncnynl.com/archives/201611/1079.html