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

 1  #!/usr/bin/env python     
 2   import roslib  
 3   roslib.load_manifest('learning_tf') 
 4   import rospy 
 5    
 6    import tf
 7   import turtlesim.msg
 8   #使用 tf 完成坐标变换，订阅这个topic "turtleX/pose" 并且对每一条msg进行以下计算
 9   def handle_turtle_pose(msg, turtlename):
10       br = tf.TransformBroadcaster()   #将turtlebot的坐标系发到 tf tree中
11       br.sendTransform((msg.x, msg.y, 0),
12                        tf.transformations.quaternion_from_euler(0, 0, msg.theta),  #使用四元数完成坐标变换
13                        rospy.Time.now(),               #
14                       turtlename,
15                        "world")
16  
17   if __name__ == '__main__':
18       rospy.init_node('turtle_tf_broadcaster')  #添加新的节点
19       turtlename = rospy.get_param('~turtle')#
20       rospy.Subscriber('/%s/pose' % turtlename,   #发布的信息内容
21                        turtlesim.msg.Pose,
22                        handle_turtle_pose,
23                        turtlename)
24       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

 

 1 #!/usr/bin/env python  
 2 import roslib
 3 roslib.load_manifest('learning_tf')
 4 import rospy
 5 import math
 6 import tf     #tf 里面有定义订阅者的函数
 7 import geometry_msgs.msg
 8 import turtlesim.srv
 9 
10 if __name__ == '__main__':
11     rospy.init_node('tf_turtle')
12 
13     listener = tf.TransformListener()  #定义listener  一旦定义listener  ，他就开始接受信息，并且可以缓冲10S.
14 
15     rospy.wait_for_service('spawn')   #等待服务。 spwn ，这个服务可以创建另一只乌龟
16     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
17     spawner(4, 2, 0, 'turtle2')   #位置 跟名字
18 
19     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
20    #这一段是我们真正做的工作，我们使 turtle1是父坐标系，turtle2是子坐标系，然后就调用了listener.lookupTransform
21 rate = rospy.Rate(10.0)
22   while not rospy.is_shutdown(): 
23       try: 
24        (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))    #可以缓冲10S内最近的信息。
25       except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 
26       continue 
27    angular = 4 * math.atan2(trans[1], trans[0]) 
28    linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) 
29    cmd = geometry_msgs.msg.Twist() 
30    cmd.linear.x = linear 
31    cmd.angular.z = angular 
32    turtle_vel.publish(cmd) 
33    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

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

 

posted @ 2016-11-24 14:53  小萝、卜  阅读(9500)  评论(0)  编辑  收藏  举报