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
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
看到仿真乌龟界面，回到启动终端，用方向键控制第一只乌龟运动，可以看到第二只乌龟在跟随。
效果如下：
介绍到此。