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