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