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 包还提供了一些辅助工具用来帮调试等用途。

Table 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 中(注意不是有小乌龟的窗口中)用方向键(“上”为前进,“下”为后退,“左”逆时针转变方向,“右”顺时针转变方向)可以控制一只小乌龟,而另一只小乌龟会跟着它走。最后两只小乌龟重叠在一起。

ROS_tf_demo.jpg

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 坐标系及发布的两个坐标系:

ROS_tf_rqt_tf_tree.jpg

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

ROS_tf_rviz.jpg

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 窗口中(注意不是有小乌龟的窗口中)用方向键可以控制它的移动。如下图所示:

ROS_tf_broadcaster.png

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 的子坐标系,那么坐标系关系会是这样的:

ROS_tf_add_frame.png

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 米。

ROS_tf_two_turtle.png

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()

ROS_tf_moving_frame.png

Figure 7: 两只乌龟,一只随着另一只移动(三角函数关系)

Author: cig01

Created: <2016-06-04 Sat>

Last updated: <2016-06-09 Thu>

Creator: Emacs 27.1 (Org mode 9.4)