ROS 中TF學習

1.什么是TF

TF是處理機器人不同位置坐標系的一個包，機器人不同部位和世界的坐標系以  tree structure 的形式存儲起來，TF可以使任何兩個坐標系之間的點 向量相互轉化。

機器人系統是有很多坐標系系統，並且每一個坐標系系統都隨時間變化，tf 可以幫助你解決這些問題：頭 坐標系跟世界坐標系是什么關系，柵格圖中物體的位置跟我地盤有什么關系？在地圖坐標系中，底盤的位置跟現在有什么關系？

tf實例講解：

古月君 http://blog.csdn.net/hcx25909/article/details/9255001   

http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28Python%29

1.1 How to broadcast transforms

 #!/usr/bin/env python     
  import roslib  
  roslib.load_manifest('learning_tf') 
  import rospy 
   
   import tf
  import turtlesim.msg
  #使用 tf 完成坐標變換，訂閱這個topic "turtleX/pose" 並且對每一條msg進行以下計算
  def handle_turtle_pose(msg, turtlename):
      br = tf.TransformBroadcaster()   #將turtlebot的坐標系發到 tf tree中
      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()

 

1.2　　Running the broadcaster

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

  </launch>

rosrun tf tf_echo /world /turtle1   可以查看turtle的位置確實通過廣播發到了tf .

1.3How to create a tf listener

 

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf     #tf 里面有定義訂閱者的函數
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf_turtle')

    listener = tf.TransformListener()  #定義listener  一旦定義listener  ，他就開始接受信息，並且可以緩沖10S.

    rospy.wait_for_service('spawn')   #等待服務。 spwn ，這個服務可以創建另一只烏龜
    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)
   #這一段是我們真正做的工作，我們使 turtle1是父坐標系，turtle2是子坐標系，然后就調用了listener.lookupTransform
rate = rospy.Rate(10.0)
  while not rospy.is_shutdown(): 
      try: 
       (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))    #可以緩沖10S內最近的信息。
      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()

 

 

　　在launch 里面加入 

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

1.4 Adding a frame

往tf  tree 里面加入額外的坐標系。這跟創建一個tf boardcaster有點類似，為什么要加入坐標系呢？在tf 樹中是不能構成回路的、只能有一個父坐標系，但是可以有很多子坐標系

。我們先前創造的世界中有 三個坐標系，父坐標系：世界坐標系   兩個子：   turtle1坐標系跟 ，turtle2坐標系。

  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")   #carrot 1 是tuttle的子坐標系
 18         rate.sleep()

　　

　在Launch文件中加入下列， 

  <launch>
    ...
 <node pkg="learning_tf" type="fixed_tf_broadcaster.py"  name="broadcaster_fixed" />  </launch>

　如果將listener 中的代碼修改

1 (trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

1.5 Broadcasting a moving frame

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('my_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        t = rospy.Time.now().to_sec() * math.pi
        br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()