ROS Learning-022 learning_tf-06（编程） 现在与过去中穿梭 (Python版) --- waitForTransformFull() 函数

ROS Indigo learning_tf-06 现在与过去中穿梭 (Python版) — waitForTransformFull() 函数

我使用的虚拟机软件：VMware Workstation 11
使用的Ubuntu系统：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo

---

一 . 前言

这一节要做的事情：使用 tf 的一个强大功能：可以再现在与过去中穿梭。（就是：如何使用 waitForTransformFull() 函数。）

我们不让 turtle2 跟随当前的 turtle1 的坐标， 而是让 turtle2 去跟随 过去的 turtle1 的坐标。
一样，我们只需要改变 监听器 程序 （ turtle_tf_listener.py ）中的一点点代码就可以实现。我们就让 turtle2 去跟随5秒钟前的 turtle1 的坐标吧。

跟随过去 的 程序编写

1 . 重点讲解

我们将之前编写的 监听器 程序 （ turtle_tf_listener.py ）里面的这一段代码：

        try:
            (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

修改为：

        try: 
            now = rospy.Time.now()
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')

我们来具体了解一下上面代码中出现的：lookupTransformFull() 函数，这个函数也是坐标系信息转换的功能。这个函数有6个形参： ( 其实它的功能和lookupTransform()函数的功能一样，只不过是个升级版函数 ) ：

1 . 父类坐标系节点名
2 . 指定时间
3 . 子类坐标系节点名
4 . 指定时间
5 . 指定不随时间改变的坐标系, 在这里是 “/world” 坐标系.

通过 形参1 和 形参2 就可以得到一个 姿态的信息（线速度和角速度），通过 形参3 和 形参4 就可以得到另一个姿态的信息。

图示 lookupTransformFull() 函数的作用：

解说一下这幅图的意思： lookupTransformFull() 函数 做了下面三步的事情。
第1步 ： 将 形参1 在指定的时间 形参2 时的姿态信息转换到 形参5 坐标系上；
第2步 ： 接着，将 形参3 在指定的时间 形参4 时的姿态信息转换到 形参5 坐标系上；
第3步 ： 最后，将 第1步 得到的姿态信息做为坐标原点（即：以 形参1 的位置做为参考坐标系），求出 第二步 得到的姿态信息 在 这个参考坐标中的姿态信息，将其做为 lookupTransformFull() 函数返回值输出。

---

2 . 编写代码

在learning_tf 程序包中的 nodes 路径下，新建一个文件：turtle_tf_listener_pastNow.py:

roscd learning_tf/node/
gedit turtle_tf_listener_pastNow.py

下面是完整的程序。将这段程序复制到 turtle_tf_listener_pastNow.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('tf_turtle')
    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)
    listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try: 
            now = rospy.Time.now()
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')
        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 777 turtle_tf_listener_pastNow.py

---

3 . 编写启动文件

在learning_tf 程序包中的 launch 路径下，新建一个文件：start_demo6.launch:

roscd learning_tf/launch/
gedit start_demo6.launch

将下面的代码拷贝进去。（下面这段代码就是通过 start_demo2.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>

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

</launch>

4 . 运行

运行 start_demo6.launch 这个启动脚本文件：

$ roslaunch  learning_tf start_demo6.launch 

这次的运行效果，很不错，当前时间的 turtle2 小海龟 就在跟随这个 过去5秒钟之前的 turtle1 小海龟做运动。

搞定

---

5 . 扩展： ROS 官方上的错误

下面这段是 ROS 官网中的程序：（这个网站）

        try: 
            now = rospy.Time.now()
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull('/turtle2', now, '/turtle1', past, '/world', rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')

这段代码是错误的。如果运行的话，会输出下面的错误信息：

[listener-6] process has died [pid 8919, exit code 1, cmd /home/aobosir/catkin_ws/src/learning_tf/nodes/turtle_tf_listener_pastNow.py __name:=listener __log:=/home/aobosir/.ros/log/71c1eedc-75ad-11e6-8577-000c29fd0c33/listener-6.log].
log file: /home/aobosir/.ros/log/71c1eedc-75ad-11e6-8577-000c29fd0c33/listener-6*.log

正确的代码就是：（这样就不会出现问题）

        try: 
            now = rospy.Time.now()
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')

---

---

总结：
现在，我们学会了如果在ROS 中进行不同时间段之间的 tf 转换。
接下来，下一节，介绍：如果调试使用 tf 转换的程序（C++ 版）。

---