ROS - tf

kuan_li_lyg

已于 2024-04-11 09:20:41 修改

阅读量905

点赞数 18

分类专栏： ROS & ROS2 文章标签：机器人 ROS tf 自动驾驶机器人导航机械臂坐标系变换

于 2024-01-07 21:20:09 首次发布

本文链接：https://blog.csdn.net/weixin_46300916/article/details/135443455

版权

ROS & ROS2 专栏收录该内容

25 篇文章 5 订阅

订阅专栏

系列文章目录

前言

PR2

你想看看 tf 能做什么？看看 tf 介绍演示。

一个机器人系统通常有许多随时间变化的三维坐标系，如世界坐标系、基础坐标系、抓手坐标系、头部坐标系等：

5 秒钟前，头部坐标系相对于世界坐标系的位置在哪里？
我的抓手中的物体相对于底座的姿势是什么？
底座坐标系在地图坐标系中的当前姿态是什么？

tf 可以在分布式系统中运行。这意味着系统中任何一台电脑上的所有 ROS 组件都能获取机器人坐标系的所有信息。没有中央转换信息服务器。

有关设计的更多信息，请参阅/设计

我们创建了一套教程，一步步指导你使用 tf。您可以从 tf 入门教程开始。要查看所有 tf 和 tf 相关教程的完整列表，请访问教程页面。

任何用户使用 tf 基本上都有两项任务：监听变换和广播变换。

任何使用 tf 的用户都需要监听变换：

监听变换 - 接收和缓冲系统中广播的所有坐标系，并查询坐标系间的特定变换。请查看编写 tf 监听器教程（Python）（C++）。

要扩展机器人的功能，您需要开始广播变换。

广播变换 - 向系统其他部分发送坐标系的相对姿态。一个系统可以有多个广播变换器，每个变换器提供机器人不同部分的信息。请查看编写 tf 广播器教程（Python）（C++）。

完成基本教程后，您可以继续学习 tf 和时间。tf 和时间教程（Python）（C++）教授 tf 和时间的基本原理。关于 tf 和时间的高级教程 (Python) (C++) 讲授使用 tf 进行时间旅行的原理。

一、tf 介绍

1.1 设置 demo

本教程的节点是为 Ubuntu 发布的，因此请安装它们：

sudo apt-get install ros-noetic-ros-tutorials ros-noetic-geometry-tutorials ros-noetic-rviz ros-noetic-rosbash ros-noetic-rqt-tf-tree

1.2 运行演示

现在我们已经完成了 turtle_tf 教程软件包的安装，让我们运行 demo 程序。

roslaunch turtle_tf turtle_tf_demo.launch

您会看到海龟仿真器，从两只海龟开始。

(如果你遇到进程死机的情况，也可以采取变通方法）。

启动海龟模拟器后，你可以使用键盘上的方向键在海龟模拟器中驱动中心海龟，选择 roslaunch 终端窗口，这样就可以捕捉到你驱动海龟的按键。

你可以看到，一只乌龟会不断移动，跟着你驾驶的乌龟转圈。

1.3发生了什么

本演示使用 tf 库创建了三个坐标系：世界坐标系、乌龟 1 坐标系和乌龟 2 坐标系。本教程使用 tf 广播器发布乌龟坐标系，并使用 tf 监听器计算乌龟坐标系的差异，然后移动一只乌龟跟随另一只乌龟。

1.4 tf 工具

现在让我们来看看如何使用 tf 制作这个演示。我们可以使用 tf 工具来查看 tf 在幕后所做的工作。

1.4.1 使用 view_frames

view_frames 可以创建 tf 通过 ROS 广播的坐标系图。

rosrun tf view_frames

将看到

Transform Listener initing
Listening to /tf for 5.000000 seconds
Done Listening
dot - Graphviz version 2.16 (Fri Feb  8 12:52:03 UTC 2008)

Detected dot version 2.16
frames.pdf generated

在这里，一个 tf 监听器正在监听通过 ROS 广播的坐标系，并绘制坐标系之间的连接树。查看树状图

evince frames.pdf

在这里，我们可以看到 tf 广播的三个坐标系：世界、turtle1 和 turtle2。我们还可以看到，world 是 turtle1 和 turtle2 两坐标系的父坐标系。为了便于调试，view_frames 还会报告一些诊断信息，如何时收到了最旧和最近的坐标系变换，以及 tf 坐标系发布到 tf 的速度。

1.4.2 使用 rqt_tf_tree

rqt_tf_tree 是一个运行时工具，用于可视化通过 ROS 广播的坐标系树。只需点击图表左上角的刷新按钮，即可刷新树形图。

使用方法

rosrun rqt_tf_tree rqt_tf_tree

或者

rqt &

然后从插件选项卡中选择 rqt_tf_tree。

1.4.3 使用 tf_echo

tf_echo 报告通过 ROS 广播的任意两个坐标系之间的变换。

使用方法：

rosrun tf tf_echo [reference_frame] [target_frame]

让我们来看看乌龟 2 坐标系相对于乌龟 1 坐标系的变换，这相当于乌龟 1 到世界的变换乘以世界到乌龟 2 坐标系的变换的乘积。

rosrun tf tf_echo turtle1 turtle2

当 tf_echo 监听器接收到通过 ROS 广播的坐标系时，就会显示变换。

At time 1416409795.450
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.914, 0.405]
            in RPY [0.000, -0.000, 2.308]
At time 1416409796.441
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.914, 0.405]
            in RPY [0.000, -0.000, 2.308]
At time 1416409797.450
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.914, 0.405]
            in RPY [0.000, -0.000, 2.308]
At time 1416409798.441
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.914, 0.405]
            in RPY [0.000, -0.000, 2.308]
At time 1416409799.433
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.691, 0.723]
            in RPY [0.000, -0.000, 1.526]

当您驾驶乌龟四处游动时，您会看到变换随着两只乌龟的相对移动而发生变化。

1.5 rviz 和 tf

rviz 是一种可视化工具，可用于检查 tf 框架。让我们使用 rviz 来查看我们的乌龟帧。让我们使用 rviz 的 -d 选项，通过 turtle_tf 配置文件启动 rviz：

rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

在侧边栏中，您将看到 tf 播放的坐标系。当你驾驶乌龟移动时，你会看到坐标系在 rviz 中移动。

既然我们已经研究了 turtle_tf_demo，那么让我们来看看如何为这个演示编写广播器（Python）（C++）。

二、编写一个 tf 广播器（Python）

注意：如果您还没有学习过编写 tf 广播员（C++）教程，请务必学习本节内容。

在接下来的两节教程中，我们将编写代码来重现 tf 入门教程中的演示。之后，下面的教程将重点使用更高级的 tf 功能来扩展演示。

在开始之前，您需要为本项目创建一个新的 ros 包。在沙盒文件夹中，创建一个名为 learning_tf 的包，该包依赖于 tf、roscpp、rospy 和 turtlesim：

cd %YOUR_CATKIN_WORKSPACE_HOME%/src
catkin_create_pkg learning_tf tf roscpp rospy turtlesim

构建新软件包后，才能运行 roscd：

cd %YOUR_CATKIN_WORKSPACE_HOME%/
catkin_make
source ./devel/setup.bash

2.1 如何广播变换

本教程教您如何向 tf 广播坐标框架。在本例中，我们要广播乌龟移动时不断变化的坐标系。

首先创建源文件。转到我们刚刚创建的软件包：

roscd learning_tf

2.1.1 代码

首先，让我们在 learning_tf 软件包中新建一个名为 nodes 的目录。

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

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')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

别忘了让节点可执行：

chmod +x nodes/turtle_tf_broadcaster.py

2.1.2 代码解释

现在，让我们来看看将乌龟姿势发布到 tf 的相关代码。

turtlename = rospy.get_param('~turtle')

该节点只接受一个参数 "turtle"，即指定一个乌龟名称，如 "turtle1 "或 "turtle2"。

    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)

节点订阅主题 "turtleX/pose"，并对每条传入的信息运行函数 handle_turtle_pose。

    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

乌龟姿势信息的处理函数会广播乌龟的平移和旋转，并将其发布为从坐标系 "world "到坐标系 "turtleX "的变换。

2.2 运行广播器

现在为该演示创建一个启动文件。使用文本编辑器新建一个名为 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

你应该看到只有一只乌龟的乌龟模拟。

2.3 检查结果

现在，使用 tf_echo 工具来检查乌龟姿势是否真正广播到了 tf：

rosrun tf tf_echo /world /turtle1

这会显示第一只乌龟的姿势。使用方向键绕着海龟行驶（确保终端窗口处于活动状态，而不是模拟器窗口）。如果运行 tf_echo 命令在世界和乌龟 2 之间进行变换，应该看不到变换，因为第二个乌龟还没有出现。不过，一旦我们在下一个教程中添加了第二只乌龟，乌龟 2 的姿势就会广播给 tf。

三、编写一个 tf 监听器（Python）

3.1 如何创建 tf 监听器

首先创建源文件。转到我们在上一教程中创建的软件包：

roscd learning_tf

3.1.1 代码

启动你最喜欢的编辑器，将以下代码粘贴到名为 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('turtle_tf_listener')

    listener = tf.TransformListener()

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

别忘了让节点可执行：

chmod +x nodes/turtle_tf_listener.py

3.1.2 代码解释

现在，让我们来看看将乌龟姿势发布到 tf 的相关代码。

import tf

tf 软件包提供了一个 tf.TransformListener 实现，可帮助用户更轻松地接收变换。

listener = tf.TransformListener()

在此，我们创建了一个 tf.TransformListener 对象。一旦创建了监听器，它就会开始通过线路接收 tf 变换，并将其缓冲长达 10 秒。

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

在这里，真正的工作已经完成，我们通过 lookupTransform 查询监听器的特定变换。让我们来看看参数：

我们希望从"/turtle1 "坐标系 ...... 转换到"/turtle2 "坐标系。
... 转换到"/turtle2 "坐标系。
我们想要变换的时间。如果提供 rospy.Time(0)，我们将得到最新的可用变换。

此函数返回两个列表。第一个是子坐标系相对于父坐标系的 (x, y, z) 线性变换，第二个是从父坐标系方向旋转到子坐标系方向所需的 (x, y, z, w) 四元数。

所有这些都包裹在一个 try-except 块中，以捕捉可能出现的异常。

3.2 运行监听器

用文本编辑器打开名为 start_demo.launch 的启动文件，并添加以下几行：

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener.py" 
          name="listener" />
  </launch>

首先，确保停止了上一教程中的启动文件（使用 ctrl-c）。现在就可以开始完整的海龟演示了：

roslaunch learning_tf start_demo.launch

你应该看到有两只乌龟的乌龟模拟。

3.3 查看结果

要查看结果，只需使用方向键绕过第一只海龟（确保终端窗口处于活动状态，而不是模拟器窗口），就会看到第二只海龟跟在第一只海龟后面！

当海龟模拟器启动时，你可能会看到

[ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
[ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.

出现这种情况的原因是，我们的监听器试图在收到有关海龟 2 的信息之前计算变换，因为海龟 2 在海龟模拟中生成并开始广播 tf 坐标系需要一点时间。

四、添加一个坐标系（Python）

4.1 为什么要添加坐标系

对于许多任务来说，在局部坐标系内进行思考更为容易，例如，在激光扫描仪中心的坐标系内对激光扫描进行推理更为容易。此外，tf 还会处理所有引入的额外坐标系变换。

4.2 在何处添加坐标系

tf 构建了一个树形坐标系结构；它不允许在坐标系结构中形成闭环。这意味着一个坐标系只有一个父坐标系，但可以有多个子坐标系。目前，我们的 tf 树包含三个坐标系：world、turtle1 和 turtle2。这两只乌龟是 world 的子坐标系。如果我们想在 tf 中添加一个新的坐标系，现有的三个框架中必须有一个是父坐标系，而新的坐标系将成为子坐标系。

4.3 如何添加坐标系

在我们的乌龟示例中，我们将为第一只乌龟添加一个新坐标系。这个坐标系将成为第二只乌龟的 "胡萝卜"。

首先创建源文件。转到我们为前面的教程创建的软件包：

roscd learning_tf

4.3.1 代码

启动你最喜欢的编辑器，将以下代码粘贴到名为 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('fixed_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

别忘了让节点可执行：

chmod +x nodes/fixed_tf_broadcaster.py

代码与 tf 广播员教程中的示例非常相似。只是这里的变换不会随时间而改变。

4.3.2 代码解释

让我们来看看这段代码中的关键行：

        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")

在这里，我们创建一个新的变换，从父代 "turtle1 "变换到新的子代 "carrot1"。胡萝卜 1 "坐标系与 "乌龟 1 "坐标系相距 2 米。

4.4 运行坐标系广播器

编辑 start_demo.launch 启动文件。只需添加以下一行即可：

  <launch>
    ...
    <node pkg="learning_tf" type="fixed_tf_broadcaster.py"
          name="broadcaster_fixed" />
  </launch>

首先，确保停止了上一教程中的启动文件（使用 Ctrl-c）。现在就可以开始海龟广播员演示了：

roslaunch learning_tf start_demo.launch

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 "坐标系中的激光扫描进行推理，并安全地绕过环境中的障碍物。

7.2 编写代码

希望上面的示例能帮助我们从概念上理解 tf。现在，我们需要用代码来创建转换树。在本例中，我们假定对 ROS 非常熟悉，因此如果有不熟悉的术语或概念，请务必查看 ROS 文档。

假设我们要完成上面描述的高级任务，即把 "base_laser "坐标系中的点转换到 "base_link "坐标系中。首先，我们需要创建一个节点，负责在系统中发布转换结果。接下来，我们需要创建一个节点来监听通过 ROS 发布的变换数据，并将其应用于点的变换。首先，我们将为源代码创建一个包，并给它起一个简单的名字，比如 "robot_setup_tf"。我们将依赖于 roscpp、tf 和 geometry_msgs。

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

请注意，您必须在有权限的地方运行上述命令（例如，您可能为之前的教程创建的 ~/ros）。

在 fuerte、groovy 和 hydro 中的替代方法：在 navigation_tutorials 堆栈中有一个标准的 robot_setup_tf_tutorial 包。您可以按照以下方法安装（%YOUR_ROS_DISTRO% 可以是 { fuerte, groovy } 等）：

sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials

7.3 广播变换

现在我们已经有了自己的软件包，需要创建一个节点，在 ROS 上广播 base_laser → base_link 变换。在刚刚创建的机器人安装包（robot_setup_tf）中，启动你喜欢的编辑器，将以下代码粘贴到 src/tf_broadcaster.cpp 文件中。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

现在，让我们详细看看与发布 base_link → base_laser 变换相关的代码。

#include <tf/transform_broadcaster.h>

tf 软件包提供了一个 tf::TransformBroadcaster 实现，帮助我们更轻松地发布变换。要使用 TransformBroadcaster，我们需要包含 tf/transform_broadcaster.h 头文件。

  tf::TransformBroadcaster broadcaster;

在这里，我们创建了一个 TransformBroadcaster 对象，稍后我们将用它通过电线发送 base_link → base_laser 变换。

    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));

这才是真正的工作。使用 TransformBroadcaster 发送变换需要五个参数。首先，我们传递旋转变换，它由一个 btQuaternion 来指定两个坐标系之间需要进行的任何旋转。在本例中，我们不希望应用任何旋转，因此我们输入一个由俯仰、滚动和偏航值构成的 btQuaternion，其值等于零。其次，一个 btVector3 表示我们想要应用的任何平移。不过，我们确实想要应用平移，因此我们创建了一个 btVector3，对应于激光器与机器人基座之间 10 厘米的 x 偏移量和 20 厘米的 z 偏移量。第三，我们需要给正在发布的变换加一个时间戳，我们只需用 ros::Time::now() 给它盖个戳。第四，我们需要传入所创建刚体的父节点名称，本例中为 "base_link"。第五，我们需要传递所创建刚体的子节点的名称，本例中为 "base_laser"。

7.4 使用变换

上面，我们创建了一个节点，通过 ROS 发布 base_laser → base_link 变换。现在，我们要编写一个节点，使用该变换将 "base_laser "坐标系中的一个点变换为 "base_link "坐标系中的一个点。同样，我们先将下面的代码粘贴到一个文件中，然后再进行更详细的解释。在 robot_setup_tf 软件包中创建一个名为 src/tf_listener.cpp 的文件，并粘贴以下代码：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

好了......现在我们一步步来介绍重要的部分。

#include <tf/transform_listener.h>

在这里，我们包含了创建 tf::TransformListener 所需的 tf/transform_listener.h 头文件。TransformListener 对象会自动订阅 ROS 上的变换消息主题，并管理所有通过线路传送的变换数据。

void transformPoint(const tf::TransformListener& listener){

我们将创建一个函数，该函数在给定 TransformListener 的情况下，接收 "base_laser "坐标系中的一个点，并将其转换到 "base_link "坐标系中。该函数将作为在程序的 main() 中创建的 ros::Timer 的回调函数，每秒触发一次。

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

在这里，我们将以几何_msgs::PointStamped 的形式创建点。信息名称末尾的 "Stamped"（加盖）仅表示它包含一个标头，允许我们将时间戳和帧 ID 与信息关联起来。我们将把 laser_point 信息的 stamp 字段设置为 ros::Time()，这是一个特殊的时间值，允许我们向 TransformListener 询问最新的可用变换。至于报文头的 frame_id 字段，我们将其设置为 "base_laser"，因为我们要在 "base_laser "坐标系中创建一个点。最后，我们将为 point.... 设置一些数据，取值为 x：1.0、y： 0.2 和 z：0.0。

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }

现在，我们已经在 "base_laser "坐标系中找到了点，我们要将它转换到 "base_link "坐标系中。为此，我们将使用 TransformListener 对象，并调用带有三个参数的 transformPoint()：我们要将点转换到的坐标系名称（本例中为 "base_link"）、我们要转换的点以及转换后点的存储空间。因此，在调用 transformPoint() 之后，base_point 保存的信息与之前 laser_point 保存的信息相同，只是现在保存在 "base_link "坐标系中。

  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }

如果由于某种原因，base_laser → base_link 变换不可用（可能是 tf_broadcaster 没有运行），那么当我们调用 transformPoint() 时，TransformListener 可能会抛出异常。为确保从容应对，我们将捕获异常并打印出错误信息提供给用户。

7.5 构建代码

现在我们已经写好了我们的小示例，我们需要构建它。打开由 roscreate-pkg 自动生成的 CMakeLists.txt 文件，在文件底部添加以下几行。

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(tf_listener ${catkin_LIBRARIES})

接下来，确保保存文件并构建软件包。

cd %TOP_DIR_YOUR_CATKIN_WS%
catkin_make

7.6 运行代码

好了......我们都建好了。让我们试试看，看看 ROS 上到底发生了什么。在本节中，你需要打开三个终端。

在第一个终端，运行一个核心。

roscore

在第二个终端，我们将运行 tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

好极了。现在我们使用第三个终端运行 tf_listener，将模拟点从 "base_laser "坐标系转换到 "base_link "坐标系。

rosrun robot_setup_tf tf_listener

如果一切顺利，您将看到以下输出，显示一个点每秒一次从 "base_laser "坐标系转换到 "base_link "坐标系。

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19
[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19
[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19
[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19
[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

恭喜你，你刚刚为平面激光器编写了一个成功的变换广播器。下一步是将本示例中使用的 PointStamped 替换为通过 ROS 传输的传感器流。幸运的是，你可以在这里找到关于这部分过程的文档。

八、在机器人上使用机器人状态发布器（robot state publisher）

当您使用的机器人有许多相关坐标系时，将它们全部发布到 tf 就成了一项相当艰巨的任务。机器人状态发布器就是一个能帮您完成这项工作的工具。

机器人状态发布器可帮助您向 tf 变换库广播机器人的状态。机器人状态发布器内部有一个机器人运动学模型；因此，在给定机器人关节位置的情况下，机器人状态发布器可以计算并发布机器人每个环节的三维姿态。

您可以将机器人状态发布器作为独立的 ROS 节点或库来使用：

8.1作为 ROS 节点运行

8.1.1 机器人状态发布器（robot_state_publisher）

运行机器人状态发布器最简单的方法就是将其作为一个节点。对于普通用户来说，建议使用这种方式。运行机器人状态发布器需要两样东西：

参数服务器上加载的 urdf xml 机器人描述。
以 sensor_msgs/JointState 形式发布关节位置的源。

请阅读以下章节，了解如何配置机器人状态发布器的参数和主题。

8.1.1.1 订阅主题

joint_states (sensor_msgs/JointState)

关节位置信息

8.1.1.2 参数

机器人描述（urdf 地图）

urdf xml 机器人描述。可通过 `urdf_model::initParam` 访问

tf_prefix (string)

设置 tf 前缀，用于按名称空间发布变换。详情请参阅 tf_prefix。

publish_frequency (double)

状态发布器的发布频率，默认为 50Hz。

8.1.2 启动文件示例

设置好 XML 机器人描述和关节位置信息源后，只需创建一个类似的启动文件即可：

  <launch>
   <!-- Load the urdf into the parameter server. -->
   <param name="my_robot_description" textfile="$(find mypackage)/urdf/robotmodel.xml"/>
    
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
      <remap from="robot_description" to="my_robot_description" />
      <remap from="joint_states" to="different_joint_states" />
    </node>
  </launch>

8.2 作为库运行

高级用户还可以在自己的 C++ 代码中将机器人状态发布器作为库运行。加入头文件后

#include <robot_state_publisher/robot_state_publisher.h>

您只需使用构造函数，该构造函数将接收一棵 KDL 树

  RobotStatePublisher(const KDL::Tree& tree);

现在，每次要发布机器人的状态时，都要调用 publishTransforms 函数：

  // publish moving joints
  void publishTransforms(const std::map<std::string, double>& joint_positions,
                         const ros::Time& time);

  // publish fixed joints
  void publishFixedTransforms();

第一个参数是包含关节名称和关节位置的地图，第二个参数是记录关节位置的时间。如果地图不包含所有关节名，也没有关系。如果地图中包含一些不属于运动学模型的关节名称，也没有问题。但要注意的是，如果你没有告诉关节状态发布者运动学模型中的某些关节，那么你的 tf 树将是不完整的。

九、将 urdf 与 robot_state_publisher 结合使用

9.1 创建 URDF 文件

下面是一个大致与 R2-D2 相似的 7 link 模型的 URDF 文件。将以下链接保存到您的计算机：model.xml

<robot name="r2d2">
<link name="axis">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<cylinder radius="0.01" length=".5"/>
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<cylinder radius="0.01" length=".5"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<link name="leg1">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg1connect" type="fixed">
<origin xyz="0 .30 0"/>
<parent link="axis"/>
<child link="leg1"/>
</joint>
<link name="leg2">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg2connect" type="fixed">
<origin xyz="0 -.30 0"/>
<parent link="axis"/>
<child link="leg2"/>
</joint>
<link name="body">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -0.2"/>
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.2"/>
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="tilt" type="revolute">
<parent link="axis"/>
<child link="body"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit upper="0" lower="-.5" effort="10" velocity="10"/>
</joint>
<link name="head">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<geometry>
<sphere radius=".4"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin/>
<geometry>
<sphere radius=".4"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="swivel" type="continuous">
<origin xyz="0 0 0.1"/>
<axis xyz="0 0 1"/>
<parent link="body"/>
<child link="head"/>
</joint>
<link name="rod">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1"/>
<geometry>
<cylinder radius=".02" length=".2"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin/>
<geometry>
<cylinder radius=".02" length=".2"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="periscope" type="prismatic">
<origin xyz=".12 0 .15"/>
<axis xyz="0 0 1"/>
<limit upper="0" lower="-.5" effort="10" velocity="10"/>
<parent link="head"/>
<child link="rod"/>
</joint>
<link name="box">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin/>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="boxconnect" type="fixed">
<origin xyz="0 0 0"/>
<parent link="rod"/>
<child link="box"/>
</joint>
</robot>

9.2 发布状态

现在，我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整体运动轨迹。首先创建一个软件包：

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs

然后启动你最喜欢的编辑器，将以下代码粘贴到 src/state_publisher.cpp 文件中：

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    const double degree = M_PI/180;

    // robot state
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] ="swivel";
        joint_state.position[0] = swivel;
        joint_state.name[1] ="tilt";
        joint_state.position[1] = tilt;
        joint_state.name[2] ="periscope";
        joint_state.position[2] = height;


        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // Create new robot state
        tilt += tinc;
        if (tilt<-.5 || tilt>0) tinc *= -1;
        height += hinc;
        if (height>.2 || height<0) hinc *= -1;
        swivel += degree;
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;
}

9.3 启动文件

此启动文件假定您使用的软件包名称为 "r2d2"，节点名称为 "state_publisher"，并且您已将此 urdf 保存到 "r2d2 "软件包中。

<launch>
        <param name="robot_description" command="cat $(find r2d2)/model.xml" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node name="state_publisher" pkg="r2d2" type="state_publisher" />
</launch>

9.4 查看结果

首先，我们必须编辑保存上述源代码的软件包中的 CMakeLists.txt。除了其他依赖关系外，确保添加 tf 依赖关系：

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)

请注意，roscpp 用于解析我们编写的代码并生成 state_publisher 节点。为了生成 state_publisher 节点，我们还必须在 CMakelists.txt 末尾添加以下内容：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})

现在，我们应该转到工作区的目录，然后使用

catkin_make

现在启动软件包（假设启动文件名为 display.launch）：

roslaunch r2d2 display.launch

在新终端中运行 rviz，使用

rosrun rviz rviz

选择 odom 作为您的固定坐标系（在全局选项下）。然后选择 "添加显示屏 "并添加机器人模型显示屏和 TF 显示屏（见 http://wiki.ros.org/rviz/UserGuide）。

kuan_li_lyg

关注

18
点赞
踩
18

收藏

觉得还不错? 一键收藏
0
评论
ROS - tf

你想看看 tf 能做什么？看看 tf 介绍演示。一个机器人系统通常有许多随时间变化的三维坐标系，如世界坐标系、基础坐标系、抓手坐标系、头部坐标系等： tf 可以在分布式系统中运行。这意味着系统中任何一台电脑上的所有 ROS 组件都能获取机器人坐标系的所有信息。没有中央转换信息服务器。有关设计的更多信息，请参阅/设计我们创建了一套教程，一步步指导你使用 tf。您可以从 tf 入门教程开始。要查看所有 tf 和 tf 相关教程的完整列表，请访问教程页
复制链接

扫一扫