ROS - tf
系列文章目录
前言
????????你想看看 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-tree1.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.bash2.1 如何广播变换
本教程教您如何向 tf 广播坐标框架。在本例中，我们要广播乌龟移动时不断变化的坐标系。
首先创建源文件。转到我们刚刚创建的软件包：
roscd learning_tf2.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.py2.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_tf3.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.py3.1.2 代码解释
现在，让我们来看看将乌龟姿势发布到 tf 的相关代码。?
import tftf 软件包提供了一个 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_tf4.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.launch4.5 检查结果
因此，如果你驾驶第一只乌龟四处走动，你会发现，尽管我们添加了一个新的坐标系，但行为与上一个教程相比并没有变化。这是因为增加一坐标系并不会影响其他坐标系，而我们的监听器仍在使用之前定义的坐标系。因此，让我们来改变监听器的行为。
打开 nodes/turtle_tf_listener.py 文件，将"/turtle1 "替换为"/carrot1"：
(trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))现在好戏开始了：只要重新启动乌龟演示，你就会看到第二只乌龟而不是第一只乌龟跟在胡萝卜后面！记住，胡萝卜在乌龟 1 左侧 2 米处。胡萝卜没有可视化表示，但你应该能看到第二只乌龟移动到那个位置。
roslaunch learning_tf start_demo.launch4.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_make7.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）。?
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！