本篇博客是慕课《ROS机器人操作系统入门》的第四篇笔记，慕课网址是这里。本篇笔记主要记录tf和在C++和Python下的两种实现方式、需要注意的问题以及urdf。
1.基本知识
(1)Transform
简单来说tf是ROS中用来处理坐标变换而生的一套工具，是Transform的缩写。tf也可以理解为一个Topic，这个Topic上的消息(Message)就是就是各坐标系(frame)间的变换关系，这些消息都有对应的发布者(Publisher)进行发布与更新。多个变换和坐标系组成树状的图，叫TF树，如下示意图所示。
树状图中每个节点表示一个坐标系(frame)，对应机器人的一个link，节点之间的箭头则代表变换关系。各个节点之间必须是可以连通的，否则会报错。
在ROS里两个frame之间的变换是发布者(Publisher)发布的消息，这个消息的格式是在geometry_msgs/TransformStamped.msg中定义的，如下图所示。
而tf树的数据结构则定义在tf/tfMessage.msg(一代)或tf2_msgs/TFMessage.msg(二代)，如下。
利用rostopic info /tf即可以知道当前的tf是什么格式。
(2)urdf
urdf全称Unified Robot Description Format，统一机器人描述格式，指定了一个用于描述机器人结构的规范，以xml文件格式储存。tf的维护、可视化、仿真等都会用到它。在urdf文件中有link和joint。link对应tf树的坐标系frame，joint对应tf树的坐标系间的变换transform。link和joint的常用属性如下。
2.需求与描述
现有一个机器人，包含有一个RGB-D相机、一个LiDAR一个云台，云台上有一个相机。建立一个tf树来维护它们彼此之间的坐标变换关系，如下图所示。
trans_broadcaster节点用于发布消息，trans_listener节点用于接收消息。
3.Transform的C++实现
C++中提供了很多相关数据类型，如下：
注意tf::StampedTransform和上面提到的geometry_msgs/TransformStamped.msg的区别，前者是C++中的一个类，只能用于C++中，而后者则是ROS的消息，只依赖于ROS，与语言无关，都可以使用。
TransformBroadcaster类及相关成员函数。
TransformListener类及相关成员函数。
注意获取变换的时间，不能是ros::Time::now()。因为一个消息从发送到接收会有一定的延迟，如果选择现在的时间作为接收，则永远也收不到之前发送的消息，解决办法是ros::Time(0)，表示接收最近一次发布的消息。
(1)创建Package
打开RoboWare，在”资源管理器”中找到src文件夹，右键单击选择”新建ROS包”，输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”，输入roscpp、tf、geometry_msgs(空格隔开)即可添加依赖。
(2)创建trans_broadcaster节点
在RoboWare的资源管理器中找到transform_learn，右键选择”新建Src文件夹”，在src文件夹上右键，然后选择”新建CPP源文件”，输入名称为trans_broadcaster.cpp并回车，然后在弹出的对话框中选择”加入到新的可执行文件中”即可。
(3)编写trans_broadcaster节点代码
对于一个Transform的发布者来说，其对外发布tf消息与之前的Topic类似。主要包含：初始化ROS节点(ros::init())、新建NodeHandle(ros::NodeHandle)、构造TransformBroadcaster(tf::TransformBroadcaster)、发布消息(broadcaster.sendTransform())、循环执行(ros::spin())。具体代码如下：
#include<ros/ros.h>
#include<tf/tf.h> // 引入ROS的TF框架
#include<tf/transform_broadcaster.h> // 用于引入广播者相关API
int main(int argc, char *argv[])
{
// 初始化并指定节点名称
ros::init(argc, argv, "trans_broadcaster");
// 新建句柄
ros::NodeHandle nh;
// 新建广播对象，后续调用它的成员函数实现广播
tf::TransformBroadcaster broadcaster;
// 事先定义好各坐标系之间的变换关系
tf::Transform tf_body_plat;
// setOrigin用于设置变换的平移部分，只支持传入向量形式数据
tf_body_plat.setOrigin(tf::Vector3(0.3,0.6,0.1));
// setRotation用于设置变换的旋转部分，只支持传入四元数形式数据
tf_body_plat.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_body_rgbd;
tf_body_rgbd.setOrigin(tf::Vector3(0.4,0.6,0.2));
tf_body_rgbd.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_body_laser;
tf_body_laser.setOrigin(tf::Vector3(0.8,-0.2,0.3));
tf_body_laser.setRotation(tf::createIdentityQuaternion());
tf::Transform tf_plat_cam;
tf_plat_cam.setOrigin(tf::Vector3(0.1,0.3,-0.2));
tf_plat_cam.setRotation(tf::createIdentityQuaternion());
// 构造Rate对象使循环按指定频率运行
ros::Rate rate(1);
while (nh.ok())
{
// broadcaster发送的是StampedTransform类型对象，因此需要对Transform对象转换
// StampedTransform的构造函数四个参数分别是：
// 第一个参数是构造好的Transform数据
// 第二个参数是这个Transform所对应的时间
// 第三个参数是这个Transform的源坐标系名称(from orign)
// 第四个参数是这个Transform的目标坐标系名称(to target)
tf::StampedTransform stf_body_rgbd = tf::StampedTransform(tf_body_rgbd,ros::Time::now(),"body","rgbd");
tf::StampedTransform stf_body_plat = tf::StampedTransform(tf_body_plat,ros::Time::now(),"body","plat");
tf::StampedTransform stf_body_laser = tf::StampedTransform(tf_body_laser,ros::Time::now(),"body","laser");
tf::StampedTransform stf_plat_cam = tf::StampedTransform(tf_plat_cam,ros::Time::now(),"plat","cam");
// 调用成员函数发送广播
broadcaster.sendTransform(stf_body_rgbd);
broadcaster.sendTransform(stf_body_plat);
broadcaster.sendTransform(stf_body_laser);
broadcaster.sendTransform(stf_plat_cam);
// 输出部分提示信息
std::cout<<"broadcasting transform......"<<std::endl;
// 休眠等待
rate.sleep();
}
return 0;
}
(4)创建trans_listener节点
在RoboWare的资源管理器中找到src文件夹，点击右键，然后选择”新建CPP源文件”，输入名称为trans_listener.cpp并回车，然后在弹出的对话框中选择”加入到新的可执行文件中”即可。
(5)编写trans_listener节点代码
对于一个Transform的接收者来说，其接收tf消息与之前的Topic类似。主要包含：初始化ROS节点(ros::init())、新建NodeHandle(ros::NodeHandle)、构造TransformListener(tf::TransformListener)、接收消息or应用变换(listener.lookupTransform()orlistener.transformPoint)。具体代码如下：
#include<ros/ros.h>
#include<tf/tf.h> // 引入ROS的TF框架
#include<tf/transform_listener.h> // 用于引入接收者相关API
#include<geometry_msgs/PointStamped.h> // 代码中用到ROS中定义好的一些几何类型，如PointStamped
int main(int argc, char *argv[])
{
// 初始化节点并指定名称
ros::init(argc, argv, "trans_listener");
// 新建句柄
ros::NodeHandle nh;
// 创建一个tf接收对象，后续调用该对象的成员函数进行接收
tf::TransformListener listener;
// 阻塞等待，直到指定的某两个frame连通，注意参数
// 第一个参数是变换的目标坐标系(to)
// 第二个参数是变换的源坐标系(from)
// 第三个参数指定那一时刻的变换，如果写0则表示当前时刻
// 第四个参数是阻塞时间长度
listener.waitForTransform("body","cam",ros::Time(0),ros::Duration(4.0));
// 新建测试点用于进行测试
geometry_msgs::PointStamped point_in_rgbd;
point_in_rgbd.header.frame_id = "rgbd";
point_in_rgbd.header.stamp = ros::Time();
point_in_rgbd.point.x = 0.4;
point_in_rgbd.point.y = 0.5;
point_in_rgbd.point.z = 0.3;
// 构造Rate对象使循环按指定频率运行
ros::Rate rate(1);
while (nh.ok())
{
// 在正常运行的情况下利用try-catch语句尝试接收
try
{
// 测试一，利用lookupTransform接收
// 新建标记变换对象用于接收收到的变换
tf::StampedTransform trans_body_laser;
// 根据坐标名字查找变换，若找到赋值给传入的参数
// 第一个参数是变换的目标坐标系(to)
// 第二个参数是变换的源坐标系(from)
// 第三个参数指定那一时刻的变换，如果写0则表示当前时刻。ros::Time(0)表示最近的一帧坐标变换，不能写成ros::Time::now()
// 第四个参数表示用于接收变换数据的对象
listener.lookupTransform("laser","body",ros::Time(0),trans_body_laser);
// 获取变换的平移部分
std::cout<<"translation body->laser：x="<<
trans_body_laser.getOrigin().x()<<
",y="<<trans_body_laser.getOrigin().y()<<
",z="<<trans_body_laser.getOrigin().z()<<std::endl<<std::endl;
// 测试二，利用transformPoint应用变换
// 将rgbd下的点转换到cam下
geometry_msgs::PointStamped tf_p_cam;
listener.transformPoint("cam",point_in_rgbd,tf_p_cam);
ROS_INFO("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
point_in_rgbd.point.x,point_in_rgbd.point.y,point_in_rgbd.point.z,
tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z);
// 再将刚刚转换的点转回rgbd下，以此验证是否转换正确闭合
geometry_msgs::PointStamped tf_p_rgbd;
listener.transformPoint("rgbd",tf_p_cam,tf_p_rgbd);
ROS_INFO("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z,
tf_p_rgbd.point.x,tf_p_rgbd.point.y,tf_p_rgbd.point.z);
}
catch (tf::TransformException &ex)
{
// 如果失败输出错误信息
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
rate.sleep();
}
return 0;
}
(6)编译运行测试
最后在Catkin工作空间根目录下打开终端，使用catkin_make进行编译。然后先输入roscore打开Master，然后在新终端中输入rosrun transform_learn trans_broadcaster启动trans_broadcaster节点，新终端中输入rosrun transform_learn trans_listener启动trans_listener节点。如下：
可以看到成功运行了刚刚写的两个节点，并且实现了tf消息的收发。最后简单说一下，生成的两个节点的可执行文件放在了/root/catkin_ws/devel/lib/transform_learn里，直接在终端中运行这个可执行文件也是可以的(Master已经运行的情况下)。
4.Topic的Python实现
TF相关数据类型：
tf.transformations包提供了一些基本的变换运算。
tf.TransformListener类提供了接收者的相关函数。
tf.TransformBroadcaster类提供了发布者的相关函数。
(1)创建Package
打开RoboWare，在”资源管理器”中找到src文件夹，右键单击选择”新建ROS包”，输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”，输入rospy、tf、geometry_msgs(空格隔开)即可添加依赖。
(2)创建trans_broadcaster节点文件
根据之前的习惯，src文件夹里一般放C/C++文件，Python代码一般放在scripts文件夹里。在包名上右键点击，选择”新建文件夹”，输入名称为scripts，在scripts文件夹上右键选择”新建文件”，输入trans_broadcaster.py即可。
(3)编写trans_broadcaster代码
在Python中有两种发布方法：sendTransform()和sendTransformMessage()。第一种Python代码如下：
# coding=utf-8
import rospy
import tf
if __name__ == "__main__":
# 初始化并指定节点名称
rospy.init_node("trans_broadcaster")
# 新建广播对象，后续调用它的成员函数实现广播
broadcaster = tf.TransformBroadcaster()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 在Python中向量、点、四元数、矩阵都表示成类似数组形式，Tuple、List、Numpy Array通用
# 调用成员函数发送广播
# 第一个参数是以tuple表示的平移分量
# 第二个参数是以tuple表示的旋转分量，四元数顺序是(x, y, z, w)
# 第三个参数是变换对应的时间
# 第四个参数是变换的目标坐标系(to target)
# 第五个参数是变换的源坐标系(from origin)
broadcaster.sendTransform((0.4,0.6,0.2),(0,0,0,1),rospy.Time.now(),"rgbd","body")
broadcaster.sendTransform((0.3,0.6,0.1),(0,0,0,1),rospy.Time.now(),"plat","body")
broadcaster.sendTransform((0.8,-0.2,0.3),(0,0,0,1),rospy.Time.now(),"laser","body")
broadcaster.sendTransform((0.1,0.3,-0.2),(0,0,0,1),rospy.Time.now(),"cam","plat")
print "broadcasting transform..."
rate.sleep()
第二种如下：
# coding=utf-8
import rospy
import tf
import geometry_msgs.msg
import tf2_ros.transform_broadcaster
if __name__ == "__main__":
# 初始化并指定节点名称
rospy.init_node("trans_broadcaster")
# 新建广播对象，后续调用它的成员函数实现广播
broadcaster = tf.TransformBroadcaster()
# 相比于第一种方法，先构建好Message然后再广播，这样写起来好看一些
tf_body_plat = geometry_msgs.msg.TransformStamped()
tf_body_plat.header.frame_id = "body"
tf_body_plat.header.stamp = rospy.Time(0)
tf_body_plat.child_frame_id = "plat"
tf_body_plat.transform.translation.x = 0.3
tf_body_plat.transform.translation.y = 0.6
tf_body_plat.transform.translation.z = 0.1
tf_body_plat.transform.rotation.w = 1
tf_body_plat.transform.rotation.x = 0
tf_body_plat.transform.rotation.y = 0
tf_body_plat.transform.rotation.z = 0
tf_body_rgbd = geometry_msgs.msg.TransformStamped()
tf_body_rgbd.header.frame_id = "body"
tf_body_rgbd.header.stamp = rospy.Time(0)
tf_body_rgbd.child_frame_id = "rgbd"
tf_body_rgbd.transform.translation.x = 0.4
tf_body_rgbd.transform.translation.y = 0.6
tf_body_rgbd.transform.translation.z = 0.2
tf_body_rgbd.transform.rotation.w = 1
tf_body_rgbd.transform.rotation.x = 0
tf_body_rgbd.transform.rotation.y = 0
tf_body_rgbd.transform.rotation.z = 0
tf_body_laser = geometry_msgs.msg.TransformStamped()
tf_body_laser.header.frame_id = "body"
tf_body_laser.header.stamp = rospy.Time(0)
tf_body_laser.child_frame_id = "laser"
tf_body_laser.transform.translation.x = 0.8
tf_body_laser.transform.translation.y = -0.2
tf_body_laser.transform.translation.z = 0.3
tf_body_laser.transform.rotation.w = 1
tf_body_laser.transform.rotation.x = 0
tf_body_laser.transform.rotation.y = 0
tf_body_laser.transform.rotation.z = 0
tf_plat_cam = geometry_msgs.msg.TransformStamped()
tf_plat_cam.header.frame_id = "plat"
tf_plat_cam.header.stamp = rospy.Time(0)
tf_plat_cam.child_frame_id = "cam"
tf_plat_cam.transform.translation.x = 0.1
tf_plat_cam.transform.translation.y = 0.3
tf_plat_cam.transform.translation.z = -0.2
tf_plat_cam.transform.rotation.w = 1
tf_plat_cam.transform.rotation.x = 0
tf_plat_cam.transform.rotation.y = 0
tf_plat_cam.transform.rotation.z = 0
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 调用成员函数发送广播
broadcaster.sendTransformMessage(tf_body_plat)
broadcaster.sendTransformMessage(tf_body_laser)
broadcaster.sendTransformMessage(tf_body_rgbd)
broadcaster.sendTransformMessage(tf_plat_cam)
print "broadcasting transform..."
rate.sleep()
(4)创建trans_listener节点文件
在scripts文件夹上右键选择”新建文件”，输入trans_listener.py即可。
(5)编写trans_listener代码
代码如下：
# coding=utf-8
import rospy
import tf
import geometry_msgs.msg
if __name__ == "__main__":
# 初始化节点并指定名称
rospy.init_node("trans_listener")
# 创建一个tf接收对象，后续调用该对象的成员函数进行接收
listener = tf.TransformListener()
# 阻塞直到frame相通
listener.waitForTransform("body", "cam", rospy.Time(), rospy.Duration(4.0))
# 新建测试点用于进行测试
point_in_rgbd = geometry_msgs.msg.PointStamped()
point_in_rgbd.header.frame_id = "rgbd"
point_in_rgbd.header.stamp = rospy.Time()
point_in_rgbd.point.x = 0.4
point_in_rgbd.point.y = 0.5
point_in_rgbd.point.z = 0.3
# 构造Rate对象使循环按指定频率运行
rate = rospy.Rate(1)
while not rospy.is_shutdown():
try:
# 测试一，利用lookupTransform接收,监听对应的tf,返回平移和旋转
# ros::Time(0)表示最近的一帧坐标变换，不能写成ros::Time::now()
(translation, rotation) = listener.lookupTransform(
"laser", "body", rospy.Time(0))
print "translation body->laser:", translation, "\n"
# 测试二，利用transformPoint应用变换
# 将rgbd下的点转换到cam下
tf_p_cam = listener.transformPoint("cam", point_in_rgbd)
rospy.loginfo("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
point_in_rgbd.point.x, point_in_rgbd.point.y, point_in_rgbd.point.z,
tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z)
# 再将刚刚转换的点转回rgbd下，以此验证是否转换正确闭合
tf_p_rgbd = listener.transformPoint("rgbd", tf_p_cam)
rospy.loginfo("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z,
tf_p_rgbd.point.x, tf_p_rgbd.point.y, tf_p_rgbd.point.z)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
rate.sleep()
(6)运行测试
最后在终端中输入roscore启动Master，在scripts目录下打开新终端然后分别输入python trans_broadcaster1.py启动广播节点，python trans_listener.py启动接收节点。运行结果如下：
最后所有代码上传到了Github，点击查看。
5.tf常用工具介绍
rosrun tf view_frames：订阅tf树5秒钟，然后保存成一个pdf文件。例如本文开头的树状图就是用这个工具生成的。
rosrun rqt_tf_tree rqt_tf_tree：动态查看当前的tf树。
rosrun tf tf_echo [reference_frame] [target_frame]：查看指定两个frame之间的变换关系。
本文作者原创,未经许可不得转载,谢谢配合
