ROS Learning-015 learning_tf（编程） 编写一个监听器程序 (Python版)

ROS Indigo learning_tf-02 编写一个 监听器 程序 (Python版)

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

---

1. 这一节要做的事情：

上一节我们已经编写了一个广播员的程序，下面我们编写一个监听器程序来监听广播员发布的小海龟的坐标系信息：

监听器程序：利用广播员程序广播的坐标信息，在同一个窗口中，有2只小海龟，我们控制 小海龟1 ，让 小海龟2 自动跟随 小海龟1 运行。

2. 程序如何实现：

这个监听器，监听广播员发出的广播（turtle1 和 turtle2 之间坐标系的变化信息），然在同个运算，把结果发布给 turtle2 订阅的 turtle2/cmd_vel 话题上，从而控制 turtle2 运动。

废话少说，开始编程。

2.1. 将路径切换到 learning_tf 软件包，在 nodes 文件夹里创建一个 turtle_tf_listener.py 文件：

$ roscd learning_tf
$ gedit  nodes/turtle_tf_listener.py 

2.2. 将下面的代码添加进去：

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

    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()

2.3. 讲解代码：

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

$ chmod +x turtle_tf_listener.py

3. 运行程序

3.1. 再运行程序之前， 需要编写一个 start_demo2.launch 启动脚本文件：

$ roscd learning_tf/launch
$ gedit 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.py" name="listener"/>
</launch>

3.2. 讲解启动脚本程序：

3.3. 运行启动脚本文件：

$ roslaunch learning_tf start_demo2.launch 

当你在当前终端中操控键盘方向键时，turtle2（踪色的）跟随 turtle1（白色的）：

下面这幅图就是 world 固定坐标图：

我们使用下面的命令，来打印 turtle1 和 turtle2 的姿态信息：(分别开2个终端)

$ rosrun tf tf_echo /world /turtle1
$ rosrun tf tf_echo /world /turtle2

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

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

输出：

---

---

总结：

那么到这里，广播 tf 信息的程序 和 监听 tf 变化的监听器程序 我们都学完了。

你有没有发现，turtle2 跟随 turtle1 ，turtle2 最终会移动到与 turtle1 重叠。看到这样的运行现象，我想你肯定不舒服，想个办法来解决这个不算是问题的问题。

下一节，我教你如何将一个额外的坐标系添加到 TF树 中，从而解决这个问题。（这非常类似于创建 TF 广播员程序）