ROS与Python入门教程-TF-Time travel(时间穿梭)

ROS与Python入门教程-TF-Time travel(时间穿梭)

说明

• 介绍TF的高级特性Time travel，这是TF最有用的技巧。

Time travel(时空穿梭)

• 修改nodes/turtle_tf_listener.py，跟随第一只乌龟不再使用now()参数而是用5秒前。

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

• 现在，如果你想运行这个，你会期待看到什么？肯定在头5秒，第二只乌龟不知道去哪里，因为我们还没有一个5秒的历史的第一只乌龟。但这5秒后呢？让我们只是给它一个尝试：

$ make  or  catkin_make
$ roslaunch learning_tf start_demo.launch

• 效果：

• 像上图，乌龟不收控制，那么到底发生了什么呢？
  • 我们问TF，“/turtle1 5秒前的姿势是什么，相对于/turtle2 5秒前？” 这意味着我们控制的第二只乌龟，它是5秒前，以及第一只乌龟是5秒前。
  • 我们真正想问的是，“/turtle1 5秒前的姿势是什么，相对的/turtle2当前位置吗？”。

lookupTransform的高级API

• 我们如何才能问这样的问题呢？这个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()高级APIlookupTransformFull()有6个方面：

  • /turtle2，父坐标系
  • now，当前时间
  • /turtle1，子坐标系
  • past，过去的某时间
  • /world，指定不随时间改变的坐标系，这里使用/world
  • (trans, rot)，存放结果的变量

• 注意：waitForTransform() 同样有类似的高级API waitForTransformFull()

• 图示效果：

• 这个图显示TF在后台做了什么。
• 在过去，TF计算从第一只乌龟到世界的变换，在世界坐标系，TF时空穿梭，从过去到现在。
• 现在TF计算世界到第二只乌龟的变换。

检查结果

• 运行：

 $ roslaunch learning_tf start_demo.launch

• 效果： 应该是，第二只乌龟直接到第一只乌龟5秒前的位置