ROS - tf（三）

ROS - tf（二）+https://developer.aliyun.com/article/1585196

4.5 检查结果

因此，如果你驾驶第一只乌龟四处走动，你会发现，尽管我们添加了一个新的坐标系，但行为与上一个教程相比并没有变化。这是因为增加一坐标系并不会影响其他坐标系，而我们的监听器仍在使用之前定义的坐标系。因此，让我们来改变监听器的行为。

打开 nodes/turtle_tf_listener.py 文件，将"/turtle1 "替换为"/carrot1"：

(trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

现在好戏开始了：只要重新启动乌龟演示，你就会看到第二只乌龟而不是第一只乌龟跟在胡萝卜后面！记住，胡萝卜在乌龟 1 左侧 2 米处。胡萝卜没有可视化表示，但你应该能看到第二只乌龟移动到那个位置。

roslaunch learning_tf start_demo.launch

4.6 广播移动坐标系

我们在本教程中发布的额外帧是一个固定坐标系，与父坐标系相比不会随时间变化。但是，如果你想发布一个移动坐标系，可以编写代码使广播器随时间改变坐标系。让我们改变胡萝卜 1 坐标系，使其相对于乌龟 1 随着时间的推移而变化。

创建包含以下内容的文件 nodes/dynamic_tf_broadcaster.py（并像上面那样使用 chmod +x 使其可执行）：

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import math
if __name__ == '__main__':
    rospy.init_node('dynamic_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()

请注意，我们不是从 turtle1 开始定义一个固定的偏移量，而是根据当前时间使用 sin 和 cos 函数来定义帧的偏移量。

要测试这段代码，请记住更改启动文件，在定义 carrot1 时指向我们新的动态广播器，而不是上面的固定广播器：

<launch>
    ...
    <node pkg="learning_tf" type="dynamic_tf_broadcaster.py"
          name="broadcaster_dynamic" />
  </launch>

五、学习 tf 和时间 (Python)

5.1 tf 和时间

在前面的教程中，我们了解了 tf 如何跟踪坐标系树。该树会随时间变化，tf 会为每个变换存储一个时间快照（默认情况下最多 10 秒）。到目前为止，我们使用 lookupTransform() 函数访问 tf 树中最新的可用变换，却不知道该变换是在什么时间记录的。本教程将教你如何获取特定时间的变换。

编辑 nodes/turtle_tf_listener.py，将 lookupTransform() 调用和 except 语句改为

try:
             now = rospy.Time.now()
            (trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", now)
        except (tf.LookupException, tf.ConnectivityException):

所以，lookupTransform()会突然失败，告诉你

Traceback (most recent call last):
  File "~/ros/pkgs/wg-ros-pkg-trunk/sandbox/learning_tf/nodes/turtle_tf_listener.py", line 25, in <module>
    (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', now)
tf.ExtrapolationException: Extrapolation Too Far in the future: target_time is 1253830476.460, but the closest tf  data is at 1253830476.435 which is 0.024 seconds away.Extrapolation Too Far in the future: target_time is 1253830476.460, but the closest tf  data is at 1253830476.459 which is 0.001 seconds away.Extrapolation Too Far from single value: target_time is 1253830476.460, but the closest tf  data is at 1253830476.459 which is 0.001 seconds away. See http://pr.willowgarage.com/pr-docs/ros-packages/tf/html/faq.html for more info. When trying to transform between /carrot1 and /turtle2. See http://www.ros.org/wiki/tf#Frequently_Asked_Questions

或者，如果您使用的是电子设备，信息可能看起来像这样：

Traceback (most recent call last):
  File "/home/rosGreat/ROS_tutorial/learning_tf/nodes/turtle_tf_listener.py", line 28, in <module>
    (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', now)
tf.ExtrapolationException: Lookup would require extrapolation into the future.  Requested time 1319591145.491288900 but the latest data is at time 1319591145.490932941, when looking up transform from frame [/carrot1] to frame [/turtle2]

为什么会这样？每个监听器都有一个缓冲区，用来存储来自不同 tf 广播器的所有坐标变换。当广播者发送变换时，变换进入缓冲区需要一些时间（通常是几毫秒）。因此，当您在 "now "时间请求坐标系变换时，应等待几毫秒，等待信息到达。

5.2 等待变换

tf 提供了一个很好的工具，可以等待变换出现。让我们看看代码会是什么样子：

listener.waitForTransform("/turtle2", "/carrot1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            listener.waitForTransform("/turtle2", "/carrot1", now, rospy.Duration(4.0))
            (trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", now)

waitForTransform() 包含四个参数：

• 等待从本帧...
• ... 到此帧的变换、
• 在此时间，以及
• 超时：等待时间不得超过此最长持续时间

因此，waitForTransform() 实际上会阻塞，直到两只乌龟之间的变换可用（通常需要几毫秒），或者--如果变换不可用--直到超时。

那么为什么要调用两次 waitForTransform()呢？在代码开始时，我们生成了 turtle2，但在等待变换之前，tf 可能从未见过 /turtle2 坐标系。第一次 waitForTransform() 将等到 /turtle2 坐标系在 tf 上广播后，才会在此时尝试 waitForTransform()。

5.3 检查结果

现在，你可以再次使用方向键绕过第一只乌龟（确保终端窗口处于活动状态，而不是模拟器窗口），你会看到第二只乌龟跟在第一只乌龟后面！

所以，你会发现乌龟的行为并没有明显的不同。这是因为实际的时间差只有几毫秒。那我们为什么要将 Time(0) 改为 now()呢？只是为了让你了解一下 tf 缓冲区以及与之相关的时间延迟。在实际的 tf 用例中，使用 Time(0) 通常是完全没问题的。

六、使用 tf 进行时间旅行（Python）

6.1 时间旅行

让我们回到上一个教程结束的地方。进入你的教程包：

roscd learning_tf

现在，不要让第二只乌龟去第一只乌龟现在所在的位置，而是让第二只乌龟去第一只乌龟 5 秒前所在的位置。编辑 nodes/turtle_tf_listener.py：

try:
            now = rospy.Time.now() - rospy.Duration(5.0)
            listener.waitForTransform("/turtle2", "/turtle1", now, rospy.Duration(1.0))
            (trans, rot) = listener.lookupTransform("/turtle2", "/turtle1", now)
        except (tf.Exception, tf.LookupException, tf.ConnectivityException):

那么现在，如果你运行这个程序，你希望看到什么呢？在最初的 5 秒钟里，第二只海龟肯定不知道该去哪里，因为我们还没有第一只海龟的 5 秒钟历史记录。但这 5 秒钟之后呢？让我们试一试：

make  or  catkin_make
roslaunch learning_tf start_demo.launch

您的乌龟是否像这张截图中一样不受控制地到处乱跑？到底发生了什么？

我们问 tf："相对于 /turtle2 5 秒钟前的姿势，/turtle1 5 秒钟前的姿势是什么？这意味着我们是根据第二只乌龟 5 秒钟前的位置以及第一只乌龟 5 秒钟前的位置来控制它的。

我们真正想问的是 "相对于 /turtle2 的当前位置，/turtle1 在 5 秒钟前的姿势是什么？"。

查找变换的高级应用程序接口

那么我们如何向 tf 提出这样的问题呢？这个 API 赋予我们明确说明每坐标系何时变换的能力。代码如下

try:
            now = rospy.Time.now()
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull("/turtle2", now,
                                      "/turtle1", past,
                                      "/world", rospy.Duration(1.0))
            (trans, rot) = listener.lookupTransformFull("/turtle2", now,
                                      "/turtle1", past,
                                      "/world")

lookupTransform() 的高级应用程序接口需要六个参数：

给出本坐标系的变换、

此时...

... 到此坐标系的变换、

的变换。

指定不随时间变化的坐标系，本例中为"/world "坐标系，以及

变量来存储结果。

请注意，waitForTransform() 和 lookupTransform() 一样，也有基本和高级 API。

该图显示了 tf 正在后台进行的工作。在过去，它计算从第一只乌龟到世界的变换。在世界帧中，tf 的时间从过去走到现在。现在，tf 正在计算从世界到第二个海龟的变换。

检查结果

让我们再次运行模拟器，这次使用高级时间旅行 API：

roslaunch learning_tf start_demo.launch

七、使用 tf 设置机器人

7.1 变换配置

许多 ROS 软件包都要求使用 tf 软件库发布机器人的变换树。在抽象层面上，变换树定义了不同坐标系之间的平移和旋转偏移。为了使这一点更加具体，我们以一个简单的机器人为例，它有一个移动底座，上面安装有一台激光器。在提到机器人时，我们先定义两个坐标系：一个对应于机器人底座的中心点，另一个对应于安装在底座顶部的激光器的中心点。为了便于参考，我们也给它们起个名字。我们将连接到移动底座上的坐标系命名为 "base_link"（为了便于导航，必须将其置于机器人的旋转中心），将连接到激光器上的坐标系命名为 "base_laser"。有关坐标系命名规则，请参见 REP 105

此时，我们假定从激光器上获得了一些数据，这些数据的形式是激光器中心点的距离。换句话说，我们在 "base_laser "坐标系中已经有了一些数据。现在，假设我们想利用这些数据帮助移动基地避开世界上的障碍物。要成功做到这一点，我们需要一种方法，将我们收到的激光扫描数据从 "base_laser "坐标系转换到 "base_link "坐标系。实质上，我们需要定义 "base_laser "和 "base_link "坐标系之间的关系。

在定义这种关系时，假设我们知道激光器安装在移动底座中心点前方 10 厘米、上方 20 厘米处。这就给了我们一个平移偏移量，将 "base_link "坐标系与 "base_laser "坐标系联系起来。具体来说，我们知道要从 "base_link "坐标系获取数据到 "base_laser "坐标系，必须进行（x: 0.1m，y: 0.0m，z: 0.2m）的平移，而要从 "base_laser "坐标系获取数据到 "base_link "坐标系，必须进行相反的平移（x: -0.1m，y: 0.0m，z: -0.20m）。

我们可以选择自己管理这种关系，即存储并在必要时在坐标系之间应用适当的平移，但随着坐标系数量的增加，这将变得非常麻烦。不过，幸运的是，我们不必亲自动手。我们只需使用 tf 定义一次 "base_link "和 "base_laser "之间的关系，然后让它为我们管理两个坐标系之间的转换。

要使用 tf 定义并存储 "base_link "和 "base_laser "坐标系之间的关系，我们需要将它们添加到变换树中。从概念上讲，变换树中的每个节点都对应一个坐标系，而每条边都对应从当前节点移动到其子节点所需要应用的变换。Tf 使用树形结构来保证只有一次遍历能将任意两个坐标系连接在一起，并假定树中的所有边都是从父节点指向子节点。

要为我们的简单示例创建变换树，我们将创建两个节点，一个用于 "base_link "坐标系，另一个用于 "base_laser "坐标系。要在它们之间创建边缘，我们首先需要确定哪个节点是父节点，哪个是子节点。请记住，这种区分非常重要，因为 tf 假定所有变换都是从父节点移动到子节点的。让我们选择 "base_link "坐标系作为父节点，因为当其他部件/传感器被添加到机器人上时，它们通过遍历 "base_link "坐标系来与 "base_laser "坐标系建立联系是最合理的。这意味着连接 "base_link "和 "base_laser "的边缘的变换应为（x: 0.1m，y: 0.0m，z: 0.2m）。有了这个转换树，将 "base_laser "坐标系中接收到的激光扫描结果转换到 "base_link "坐标系中，就像调用 tf 库一样简单。我们的机器人可以利用这些信息对 "base_link "坐标系中的激光扫描进行推理，并安全地绕过环境中的障碍物。

ROS - tf（四）+https://developer.aliyun.com/article/1585201