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

坐标变换实例解析"/>

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

一、前言

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

二、TF简介²

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

三、建立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 <ros/ros.h>
#include <tf/transform_broadcaster.h>
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 <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>int main(int argc,char** argv)
{ros::init(argc,argv,"tf_subscriber");       //初始化ROS节点与节点名称ros::NodeHandle n;                         //创建节点的句柄ros::Rate loop_rate(100);                   //控制节点运行的频率,与loop.sleep共同使用//创建一个监听器，监听所有tf变换，缓冲10stf::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 <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <iostream>int main(int argc,char** argv)
{ros::init(argc,argv,"tf_subscriber");       //初始化ROS节点与节点名称ros::NodeHandle n;                         //创建节点的句柄ros::Rate loop_rate(100);                   //控制节点运行的频率,与loop.sleep共同使用//创建一个监听器，监听所有tf变换，缓冲10stf::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.
2./
.html