ROS应用开发入门 tf 坐标系广播与监听的编程(python)

ROS是机器人操作系统的简称，本文介绍ROS应用开发入门，TF坐标系广播与监听的编程。小乌龟仿真中有2只乌龟，键盘控制第1只乌龟走动，2只乌龟都都广播自己的坐标。监听器听取2个乌龟的坐标，获得相对位置，控制第2只乌龟不断追随第1只乌龟。代码是Python。测试验证采用roslaunch 方式。

工程包建立

本文是 ROS应用开发入门 tf 坐标系广播与监听的编程一文的继续，如果你做了那个实验，就是工程包已经建立好了，跳过本节。

在 ROS 开发应用准备：创建工作空间一文中建立了ROS工作空间，现在就建立一个功能包：

cd ~/catkin_ws/src

catkin_create_pkg learning_tf roscpp rospy tf turtlesim

第1行是回到工作空间的src 目录，功能包建立必须在这个目录下运行。

第2行是建立功能包的命令，第一个参数是功能包的名字，这里是learning_tf ，接下来是功能包的依赖库，这里是roscpp rospy tf turtlesim共4个依赖库。

上面命令执行后，可以在src 目录下看到有一个目录，名字是learning_tf，其下有下面内容：

CMakeLists.txt include package.xml src

广播器代码

在工程目录下，也是~/catkin_ws/src/learning_tf/目录，新建一个文件夹scripts，并建立一个文件turtle_tf_broadcaster.py，

cd ~//catkin_ws/src/learning_tf/

mkdir scripts

cd scripts

nano turtle_tf_broadcaster.py

内容为:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

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

先定义一个回调函数，里面包括建立一个广播

主程序，先定义一个订阅者节点，设置回调函数
一旦收到数据，就调用回调函数

更详细内容访问：http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28Python%29

监听器代码

在工程目录的scripts 目录下，也是~/catkin_ws/src/learning_tf/scripts 目录，建立一个文件turtle_tf_listener.py，

cd ~//catkin_ws/src/learning_tf/scripts

nano turtle_tf_listener.py

内容为:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

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

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

代码介绍在 http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29

launch 文件启动测试

python代码不用编译，但要设置为可执行文件

chmod +x *.py

我们这里用roslaunch启动测试：

cd ~//catkin_ws/src/learning_tf/

mkdir launch

cd launch

nano start_tf_demo_py.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">
          <param name="turtle" type="string" value="turtle1" />
        </node>
        <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
          <param name="turtle" type="string" value="turtle2" />
        </node>

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

</launch>

保存退出

用下面命令启动roslaunch

roslaunch start_tf_demo_py.launch

这个命令是在launch 所在文件夹，否则前面要夹路径名。

也可以用工程包方式启动：

roslaunch learning_tf start_tf_demo_py.launch

看到仿真乌龟界面，回到启动终端，用方向键控制第一只乌龟运动，可以看到第二只乌龟在跟随。

效果如下：

介绍到此。