首页 > 代码库 > ROS探索总结（十二）——坐标系统
ROS探索总结（十二）——坐标系统
ubuntu 14.04 indigo版本
转摘自：http://www.guyuehome.com/265
一、tf简介
1、安装turtle包
1 rosdep install turtle_tf rviz 2 rosmake turtle_tf rviz
2、运行demo
roslaunch turtle_tf turtle_tf_demo.launch
程序带turtlesim仿真，直接在终端进行键盘控制，可以看到两只小乌龟运行了。
3、demo分析
上例中使用tf建立了三个参考系：a world frame,a turtle1 frame, aturtle2 frame.
然后，使用tf broadcaster发布乌龟的参考系，使用tf listener计算乌龟参考系之间的差异，使用第二只乌龟跟随第一只乌龟。
使用tf工具来具体研究：
rosrun tf view_frames
可以看到生成了一个frames.pdf文件：
该文件描述了参考系之间的关系，三个节点分别是三个参考系，而/word是其他两个乌龟参考系的父参考系。还包含一些调试需要的发送频率、最近时间等信息。
tf提供了一个tf_echo工具来查看两个广播参考系之间的关系。第二只乌龟坐标如何根据第一只乌龟得出来。
rosrun tf tf_echo turtle1 turtle2
控制一只乌龟，在终端中会看到第二只乌龟的坐标转换关系
我们也可以通过rviz的图形界面更加形象的看到这三者之间的关系。
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
移动乌龟，可以看到在rviz中的坐标会跟随变化。其中左下角的是/world，其他两个是乌龟的参考系。
二、Writing a tf broadcaster
1、创建一个包
在catkin_ws/src下，创建一个learning_tf包
1 cd ~/catkin_ws/src 2 catkin_create_pkg learning_tf tf roscpp roscp turtlesim 3 cd .. 4 catkin_make
2、broadcast transforms
首先看下如何把参考系发布到tf。代码文件：
/nodes/turtle_tf_broadcaster.py
1 #!/usr/bin/env python 2 import roslib 3 roslib.load_manifest(‘learning_tf‘) 4 import rospy 5 6 7 import tf 8 import turtlesim.msg 9 10 11 def handle_turtle_pose(msg, turtlename): 12 br = tf.TransformBroadcaster() 13 br.sendTransform((msg.x, msg.y, 0), 14 tf.transformations.quaternion_from_euler(0, 0, msg.theta), 15 rospy.Time.now(), 16 turtlename, 17 "world") #发布乌龟的平移和翻转 18 19 20 if __name__ == ‘__main__‘: 21 rospy.init_node(‘turtle_tf_broadcaster‘) 22 turtlename = rospy.get_param(‘~turtle‘) #获取海龟的名字（turtle1，turtle2） 23 rospy.Subscriber(‘/%s/pose‘ % turtlename, 24 turtlesim.msg.Pose, 25 handle_turtle_pose, 26 turtlename) #订阅 topic "turtleX/pose" 27 rospy.spin()
创建launch文件start_demo.launch
1 <launch> 2 <!-- Turtlesim Node--> 3 <node pkg="turtlesim" type="turtlesim_node" name="sim"/> 4 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> 5 6 7 <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" > 8 <param name="turtle" type="string" value="turtle1" /> 9 </node> 10 <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" > 11 <param name="turtle" type="string" value="turtle2" /> 12 </node> 13 14 15 </launch>
运行start_demo.launch文件：
roslaunch learning_tf start_demo.launch
可以看到界面中只有移植乌龟了，打开tf_echo的信息窗口：
rosrun tf tf_echo /world /turtle1
注意，/world 与 /turtle1中间有空格。
world参考系的原点在最下角，对于turtle1的转换关系，其实就是turtle1在world参考系中所在的坐标位置以及旋转角度。
三、Writing a tf listener
如何使用tf进行参考系转换。
先建一个listener（turtle_tf_listener.py）
1 #!/usr/bin/env python 2 import roslib 3 roslib.load_manifest(‘learning_tf‘) 4 import rospy 5 import math 6 import tf 7 import turtlesim.msg 8 import turtlesim.srv 9 10 if __name__ == ‘__main__‘: 11 rospy.init_node(‘tf_turtle‘) 12 13 listener = tf.TransformListener() #TransformListener创建后就开始接受tf广播信息，最多可以缓存10s 14 15 rospy.wait_for_service(‘spawn‘) 16 spawner = rospy.ServiceProxy(‘spawn‘, turtlesim.srv.Spawn) 17 spawner(4, 2, 0, ‘turtle2‘) 18 19 turtle_vel = rospy.Publisher(‘turtle2/command_velocity‘, turtlesim.msg.Velocity) 20 21 rate = rospy.Rate(10.0) 22 while not rospy.is_shutdown(): 23 try: 24 (trans,rot) = listener.lookupTransform(‘/turtle2‘, ‘/turtle1‘, rospy.Time(0)) 25 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 26 continue 27 28 angular = 4 * math.atan2(trans[1], trans[0]) 29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) 30 turtle_vel.publish(turtlesim.msg.Velocity(linear, angular)) 31 32 rate.sleep()
在launch文件中添加下面的节点：
1 <launch> 2 ... 3 <node pkg="learning_tf" type="turtle_tf_listener.py" 4 name="listener" /> 5 </launch>
然后在运行，就可以看到两只turtle了，见到跟随效果。
四、Adding a frame
在一个world参考系下，一个激光扫描点，tf可以帮助将激光的信息坐标转换成全局坐标。
1、tf消息结构
tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。
2、建立固定参考系（fixed frame）
我们以turtle1作为父参考系，建立一个新的参考系“carrot1”。代码如下：fixed_tf_broadcaster.py
1 #!/usr/bin/env python 2 import roslib 3 roslib.load_manifest(‘learning_tf‘) 4 5 import rospy 6 import tf 7 8 if __name__ == ‘__main__‘: 9 rospy.init_node(‘my_tf_broadcaster‘) 10 br = tf.TransformBroadcaster() 11 rate = rospy.Rate(10.0) 12 while not rospy.is_shutdown(): 13 br.sendTransform((0.0, 2.0, 0.0), 14 (0.0, 0.0, 0.0, 1.0), 15 rospy.Time.now(), 16 "carrot1", 17 "turtle1") #建立一个新的参考系，父参考系为turtle1，并且距离父参考系2米 18 rate.sleep()
在launch文件中添加节点：
1 <launch> 2 ... 3 <node pkg="learning_tf" type="fixed_tf_broadcaster.py" 4 name="broadcaster_fixed" /> 5 </launch>
3、建立移动参考系（moving frame）
我们建立的新参考系是一个固定的参考系，在仿真过程中不会改变，如果我们要把carrot1参考系和turtle1参考系之间的关系设置可变的，可以修改代码如下：
1 #!/usr/bin/env python 2 import roslib 3 roslib.load_manifest(‘learning_tf‘) 4 5 import rospy 6 import tf 7 import math 8 9 if __name__ == ‘__main__‘: 10 rospy.init_node(‘my_tf_broadcaster‘) 11 br = tf.TransformBroadcaster() 12 rate = rospy.Rate(10.0) 13 while not rospy.is_shutdown(): 14 t = rospy.Time.now().to_sec() * math.pi 15 br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0), 16 (0.0, 0.0, 0.0, 1.0), 17 rospy.Time.now(), 18 "carrot1", 19 "turtle1") 20 rate.sleep()<font size="3"><br></font>
ROS探索总结（十二）——坐标系统