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 turtlesim3 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探索總結(十二)——坐標系統