ROS Indigo learning_tf-01 坐标系变换(tf)广播员 (Python版)

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

1. 什么是 tf ：

tf : 坐标系变换。

想要确定一个物体在什么位置，最好的方法是找一个坐标系，我们就可以得到这个物体的坐标，所以就确定了这个物体的空间位置。tf 库就是为这个而生的。

你可能还是没有明白究竟是什么意思，下面我们来写个程序，看看 tf 这个库到底是干什么的：

2. 我们下面编写一个程序：

程序的功能：
（我们先启动一个小海龟节点） 当这个小海龟在小海龟窗口中的 pose（pose：姿态，包括平移和旋转）发生了改变的时候，我们发布这个小海龟在这个小海龟窗口中的pose信息。

这就是我们下面这个程序要做的事情，简单的说就是编写一个：当小海龟坐标改变时，向外界广播 ( broadcast ) 小海龟的坐标(pose )的程序。（将坐标换成 pose 更为准确。）

2.1. 好，我们先创建一个程序包：learning_tf，用来学习 tf 的。

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
$ cd ~/catkin_ws
$ catkin_make

2.2. 在刚刚创建的 learning_tf 程序包中，新建一个 nodes 文件夹，再在这里面新建 turtle_tf_broadcaster.py 文件： （注 ： 因为第一次使用 roscd 打开新建的程序包， Tab 键不好使。所以需要你手动全输入）

$ roscd learning_tf
$ mkdir nodes
$ gedit nodes/turtle_tf_broadcaster.py

2.3. 添加下面的代码：

#!/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()

2.4. 代码讲解：

这里写图片描述

2.5. 最后一步，给这个 turtle_tf_broadcaster.py 文件加上可执行权限：

$ chmod +x nodes/turtle_tf_broadcaster.py

大功告成，我们下面开运行这个程序，看看效果：

3. 运行程序：

我们如何才能看出上面这个程序的运行效果呢， 下面跟我一步一步的做：

我们直接运行上面程序是运行不了的，我们必须要将上面那个程序写在一个 启动脚本程序 中，然后运行这个启动脚本。（为什么不能直接运行： turtle_tf_broadcaster.py 。 因为它需要传入一个参数，使用终端输入的命令的方式，无法传入这个参数，所以，我们只能使用，编写启动脚本程序的方式，来启动这个 turtle_tf_broadcaster.py 程序）

3.1. 下面编写一个 launch 启动文件：

在这 learning_tf 软件包中新建一个 launch 文件夹，然后在里面创建一个 start_demo.launch 文件：

$ roscd learning_tf
$ mkdir launch
$ gedit launch/start_demo.launch

3.2. 添加下面的代码：

 <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>
</launch>

3.3. 讲解代码：

这里写图片描述

3.4. 运行这个启动文件：

$ roslaunch learning_tf start_demo.launch

现在你可以在当前终端中使用 方向键 来控制屏幕中的小海龟。

3.5. 现在我们来检测一下 turtle_tf_broadcaster.py 程序运行的效果：(新开一个终端)

$ rosrun tf tf_echo /world /turtle1

运行后，该终端在实时刷新小海龟 /turtle1 相对于 世界坐标系 /world 的姿态（pose） 信息。

这里写图片描述

tf_echo 关键字 ： 打印出 源坐标系 和 目标坐标系 之间的特定转换信息。

$ rosrun tf tf_echo [源坐标系(父类)] [目标坐标系(子类)]

在终端中不断刷屏：

这里写图片描述

总结：
现在，你还没有看到 tf 强大的一面，上面的程序，只是想让你了解 tf 。下一节，我编写一个 程序：使用上面程序广播的小海龟坐标信息，来让另一个小海龟跟随这只小海龟。