ROS tf package
Table of Contents
1. tf 简介
tf is a package that lets the user keep track of multiple coordinate frames over time. tf maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
1.1. 命令行工具
tf 包还提供了一些辅助工具用来帮调试等用途。
| 工具 | 描述 |
|---|---|
| view_frames | visualizes the full tree of coordinate transforms. |
| tf_monitor | monitors transforms between frames. |
| tf_echo | prints specified transform to screen. |
| roswtf | with the tfwtf plugin, helps you track down problems with tf. |
| static_transform_publisher | a command line tool for sending static transforms. |
You may also wish to use the tf_remap node, which is a utility node for remapping coordinate transforms.
2. tf demo
下面这个例子来自：http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf
首先，安装这个 demo 的依赖库。
$ sudo apt-get install ros-indigo-ros-tutorials ros-indigo-geometry-tutorials ros-indigo-rviz ros-indigo-rosbash ros-indigo-rqt-tf-tree
然后，直接运行 demo。
$ roslaunch turtle_tf turtle_tf_demo.launch
可以看到两只小乌龟。在启动 demo 的 terminal 中（注意不是有小乌龟的窗口中）用方向键（“上”为前进，“下”为后退，“左”逆时针转变方向，“右”顺时针转变方向）可以控制一只小乌龟，而另一只小乌龟会跟着它走。最后两只小乌龟重叠在一起。
Figure 1: ROS tf demo（线路 1 是小乌龟 1 的线路，可用“方向键”控制；线路 2 是小乌龟 2 的线路，它跟随着小乌龟 1）
2.1. 工作原理分析
这个 demo 使用 tf 库建立了三个坐标系：a world frame, a turtle1 frame, and a turtle2 frame。 使用 tf broadcaster 发布两只乌龟的坐标系，并且使用 tf listener 计算乌龟坐标系之间的差异，使得第二只乌龟跟随第一只乌龟移动。
2.2. 查看坐标系(rqt_tf_tree)
用工具 rqt_tf_tree 可以方便地查看发布的坐标系。
下面任一个命令都可以启动 rqt_tf_tree。
$ rosrun rqt_tf_tree rqt_tf_tree $ rqt & # 在plugins菜单中选择tf tree
可以看到 world 坐标系及发布的两个坐标系：
Figure 2: world 坐标系及两只乌龟的坐标系
2.3. 查看坐标系(rviz)
rviz is a visualization tool that is useful for examining tf frames. Let's start rviz with the turtle_tf configuration file using the -d option for rviz:
$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
Figure 3: 两只乌龟移动时，坐标中两个对应标记也会随着移动
3. Writing a tf broadcaster
现在开始通过两个 Tutorials（Writing a tf broadcaster 和 Writing a tf listener）来介绍前一节中 tf demo 的实现原理。
首先介绍 Writing a tf broadcaster，摘自：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
先创建一个名为 learning_tf 的 package，基本步骤如下：
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim $ cd %YOUR_CATKIN_WORKSPACE_HOME%/ $ catkin_make $ source ./devel/setup.bash
3.1. 创建 Node
先进入包目录，创建 nodes 子目录：
$ roscd learning_tf $ mkdir nodes
创建文件 nodes/turtle_tf_broadcaster.py，内容如下：
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg
# The handler function for the turtle pose message broadcasts this turtle's translation
# and rotation, and publishes it as a transform from frame "world" to frame "turtleX".
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(),
turtlename,
"world")
if __name__ == '__main__':
rospy.init_node('turtle_tf_broadcaster')
turtlename = rospy.get_param('~turtle') # takes a single parameter "turtle", which specifies a turtle name, e.g. "turtle1" or "turtle2".
rospy.Subscriber('/%s/pose' % turtlename, # subscribes to topic "turtleX/pose" and runs function handle_turtle_pose on every incoming message.
turtlesim.msg.Pose,
handle_turtle_pose,
turtlename)
rospy.spin()
把 node 加上可执行权限：
$ chmod +x nodes/turtle_tf_broadcaster.py
3.2. 创建 launch 文件并启动
创建一个 launch 文件，
$ mkdir launch
增加文件 launch/start_demo.launch，其内容如下：
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle1" />
</node>
<node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle2" />
</node>
</launch>
用下面命令启动这个例子：
$ roslaunch learning_tf start_demo.launch
将会出现一只小乌龟。在启动运行上面命令的 terminal 窗口中（注意不是有小乌龟的窗口中）用方向键可以控制它的移动。如下图所示：
Figure 4: 一只小乌龟，可以用方向键移动它
3.3. 检测结果
现在使用工具 tf_echo 来检测小乌龟的位置和方向是否发布给了 tf。
启动 tf_echo：
$ rosrun tf tf_echo /world /turtle1
下面这些数据是小乌龟按图 4 所示路径移动时产生的：
$ rosrun tf tf_echo /world /turtle1
At time 1465054378.890
- Translation: [5.544, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]
At time 1465054379.657
- Translation: [5.544, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]
At time 1465054380.665
- Translation: [6.728, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]
At time 1465054381.673
- Translation: [7.560, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.112, 0.994]
in RPY (radian) [0.000, -0.000, 0.224]
in RPY (degree) [0.000, -0.000, 12.834]
At time 1465054382.665
- Translation: [7.560, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.846, 0.534]
in RPY (radian) [0.000, -0.000, 2.016]
in RPY (degree) [0.000, -0.000, 115.508]
At time 1465054383.673
- Translation: [7.243, 6.209, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.846, 0.534]
in RPY (radian) [0.000, -0.000, 2.016]
in RPY (degree) [0.000, -0.000, 115.508]
At time 1465054384.665
- Translation: [6.692, 7.364, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.846, 0.534]
in RPY (radian) [0.000, -0.000, 2.016]
in RPY (degree) [0.000, -0.000, 115.508]
world 坐标系的原点位于左下角，对于小乌龟 turtle1 的转换关系，其实就是小乌龟 turtle1 在 world 坐标系中所在的坐标位置以及旋转角度。
在刚开始测试时，小乌龟往右移动，可以从数据中看到 Translation 的第 1 个参数（x轴）会变大，后来小乌龟向西北方向移动，Translation 的第 1 个参数又会变小。由于是在平面上，所以 Translation 的第 3 个参数（z轴）会一直为 0。
4. Writing a tf listener
这个 Tutorial 摘自：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
前一个 Tutorial 中创建了包 learning_tf，我们直接进入到包所在的目录：
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/ $ source ./devel/setup.bash $ roscd learning_tf
4.1. 创建 Node
创建文件 nodes/turtle_tf_listener.py，内容如下：
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('tf_turtle')
listener = tf.TransformListener() # we create a tf.TransformListener object. Once the listener is created
# it starts receiving tf transformations over the wire.
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
# query the listener for a specific transformation by lookupTransform.
(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
turtle_vel.publish(cmd)
rate.sleep()
把 node 加上可执行权限：
$ chmod +x nodes/turtle_tf_listener.py
4.2. 修改 launch 文件并启动
打开文件 launch/start_demo.launch，增加下面节点：
<launch>
...
<node pkg="learning_tf" type="turtle_tf_listener.py"
name="listener" />
</launch>
用下面命令启动这个例子（如果之前的例子没有关闭请先关闭它）：
$ roslaunch learning_tf start_demo.launch
一切正常的话，你将看到两只小乌龟。一只随着另一只移动，可以在 terminal 窗口中用方向键控制第 1 只小乌龟，和前面介绍的 tf demo 一样。
5. Adding a frame
这个例子来自：http://wiki.ros.org/tf/Tutorials/Adding%20a%20frame%20%28Python%29
增加一个参考坐标系和例子 Writing a tf broadcaster 很相似。
5.1. 为什么要增加参考坐标系
对于很多任务来说，增加一个参考坐标系会带来方便。如，在 world 参考系下，有很多不同方位的激光扫描器，每个都定义一个参考坐标系，tf 可以帮助我们将激光扫描器得到的信息坐标转换为 world 坐标系中的坐标。
5.2. 把参考坐标系添加到某个父坐标系下
tf 采用树形结构来管理坐标系： 每个坐标系只有一个父坐标系，可以有多个子坐标系。
前面例子中，已经有了三个坐标系：world，turtle1 和 turtle2。如果我们增加一个名为 carrot1 的参考坐标系，并使它为 turtle1 的子坐标系，那么坐标系关系会是这样的：
Figure 5: 四个坐标系关系（红色箭头表示父子坐标系，箭头指向子坐标系）
5.3. 创建 Node
先进入到包 learning_tf 所在目录。
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/ $ source ./devel/setup.bash $ roscd learning_tf
创建文件 nodes/fixed_tf_broadcaster.py，其内容如下：
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
if __name__ == '__main__':
rospy.init_node('my_tf_broadcaster')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
# create a new transform, from the parent "turtle1" to the new child "carrot1"
# The carrot1 frame is 2 meters offset from the turtle1 frame.
br.sendTransform((0.0, 2.0, 0.0),
(0.0, 0.0, 0.0, 1.0),
rospy.Time.now(),
"carrot1",
"turtle1")
rate.sleep()
把 node 加上可执行权限：
$ chmod +x nodes/fixed_tf_broadcaster.py
5.4. 修改 launch 文件并启动
打开文件 launch/start_demo.launch，增加下面节点：
<launch>
...
<node pkg="learning_tf" type="fixed_tf_broadcaster.py"
name="broadcaster_fixed" />
</launch>
用下面命令启动这个例子（如果之前的例子没有关闭请先关闭它）：
$ roslaunch learning_tf start_demo.launch
启动后，可以看到两只乌龟，效果和前面例子一样，一只随着另一只移动。
我们现在来改变跟随关系。打开 nodes/turtle_tf_listener.py，将"/turtle1"替换为"/carrot1"：
(trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))
重新启动后，可以看到两只乌龟，一只随着另一只移动，但它们会相距 2 米。
Figure 6: 两只乌龟，一只随着另一只移动（但相距 2 米）
5.5. 发布移动的坐标系
前面例子中，坐标系 carrot1 和坐标系 turtle1 固定地相距 2 米，之间的关系不会变化。
现在我们来动态地改变坐标系 carrot1 和坐标系 turtle1 的关系。其关键就是修改 sendTransform 函数。
打开 nodes/fixed_tf_broadcaster.py，修改为：
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import math
if __name__ == '__main__':
rospy.init_node('my_tf_broadcaster')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
t = rospy.Time.now().to_sec() * math.pi
br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
(0.0, 0.0, 0.0, 1.0),
rospy.Time.now(),
"carrot1",
"turtle1")
rate.sleep()
Figure 7: 两只乌龟，一只随着另一只移动（三角函数关系）