ROS探索总结(十二)——坐标系统

  ROS探索总结(十二)——坐标系统

  http://www.guyuehome.com/265

  在机器人的控制中,坐标系统是非常重要的,在ROS使用tf软件库进行坐标转换。

  相关链接:http://www.ros.org/wiki/tf/Tutorials#Learning_tf

  一、tf简介

  我们通过一个小小的实例来介绍tf的作用。

  1、安装turtle包

  1. $ rosdep installturtle_tf rviz
  2. $ rosmake turtle_tf rviz
2、运行demo

  运行简单的demo:

  $ roslaunch turtle_tf turtle_tf_demo.launch

  然后就会看到两只小乌龟了。

  

  该例程中带有turtlesim仿真,可以在终端激活的情况下进行键盘控制。

  

  可以发现,第二只乌龟会跟随你移动的乌龟进行移动。

  3、demo分析

  接下来我们就来看一看到底ROS做了什么事情。

  这个例程使用tf建立了三个参考系:a world frame, a turtle1 frame, and a turtle2 frame。然后使用tf broadcaster发布乌龟的参考系,并且使用tf listener计算乌龟参考系之间的差异,使得第二只乌龟跟随第一只乌龟。

  我们可以使用tf工具来具体研究。

  $ rosrun tf view_frames

  然后会看到一些提示,并且生成了一个frames.pdf文件。

  

  该文件描述了参考系之间的联系。三个节点分别是三个参考系,而/world是其他两个乌龟参考系的父参考系。还包含一些调试需要的发送频率、最近时间等信息。

  tf还提供了一个tf_echo工具来查看两个广播参考系之间的关系。我们可以看一下第二只得乌龟坐标是怎么根据第一只乌龟得出来的。

  $ rosrun tf tf_echo turtle1 turtle2

  控制一只乌龟,在终端中会看到第二只乌龟的坐标转换关系。

  

  我们也可以通过rviz的图形界面更加形象的看到这三者之间的关系。

  $ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.vcg

  rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz 修正

  

  

  移动乌龟,可以看到在rviz中的坐标会跟随变化。其中左下角的是/world,其他两个是乌龟的参考系。

  下面我们就来详细分析这个实例。

  二、Writing a tf broadcaster1、创建包

  1. $ roscd tutorials
  2. $ roscreate-pkg learning_tf tf roscpp rospy turtlesim
  3. $ rosmake learning_tf
2、broadcast transforms

  我们首先看一下如何把参考系发布到tf。

  代码文件:/nodes/turtle_tf_broadcaster.py

  1. #!/usr/bin/env python
  2. importroslib
  3. roslib.load_manifest('learning_tf')
  4. importrospy
  5. importtf
  6. importturtlesim.msg
  7. defhandle_turtle_pose(msg,turtlename):
  8. br =tf.TransformBroadcaster()
  9. br.sendTransform((msg.x,msg.y,0),
  10. tf.transformations.quaternion_from_euler(0,0,msg.theta),
  11. rospy.Time.now(),
  12. turtlename,
  13. "world")#发布乌龟的平移和翻转
  14. if__name__ =='__main__':
  15. rospy.init_node('turtle_tf_broadcaster')
  16. turtlename =rospy.get_param('~turtle')#获取海龟的名字(turtle1,turtle2)
  17. rospy.Subscriber('/%s/pose'% turtlename,
  18. turtlesim.msg.Pose,
  19. handle_turtle_pose,
  20. turtlename)#订阅 topic "turtleX/pose"
  21. 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. <node name="turtle1_tf_broadcaster"pkg="learning_tf"type="turtle_tf_broadcaster.py"respawn="false"output="screen">
  6. <paramname="turtle"type="string"value="turtle1"/>
  7. </node>
  8. <node name="turtle2_tf_broadcaster"pkg="learning_tf"type="turtle_tf_broadcaster.py"respawn="false"output="screen">
  9. <paramname="turtle"type="string"value="turtle2"/>
  10. </node>
  11. </launch>

  运行:

  $ roslaunch learning_tf start_demo.launch

  可以看到界面中只有移植乌龟了,打开tf_echo的信息窗口:

  $ rosrun tf tf_echo /world /turtle1

  

  world参考系的原点在最下角,对于turtle1的转换关系,其实就是turtle1在world参考系中所在的坐标位置以及旋转角度。

  三、Writing a tf listener

  这一步,我们将看到如何使用tf进行参考系转换。首先写一个tf listener(nodes/turtle_tf_listener.py):

  1. #!/usr/bin/env python
  2. importroslib
  3. roslib.load_manifest('learning_tf')
  4. importrospy
  5. importmath
  6. importtf
  7. importturtlesim.msg
  8. importturtlesim.srv
  9. if__name__ =='__main__':
  10. rospy.init_node('tf_turtle')
  11. listener =tf.TransformListener()#TransformListener创建后就开始接受tf广播信息,最多可以缓存10s
  12. rospy.wait_for_service('spawn')
  13. spawner =rospy.ServiceProxy('spawn',turtlesim.srv.Spawn)
  14. spawner(4,2,0,'turtle2')
  15. turtle_vel =rospy.Publisher('turtle2/command_velocity',turtlesim.msg.Velocity)
  16. rate =rospy.Rate(10.0)
  17. whilenotrospy.is_shutdown():
  18. try:
  19. (trans,rot)=listener.lookupTransform('/turtle2','/turtle1',rospy.Time(0))
  20. except(tf.LookupException,tf.ConnectivityException,tf.ExtrapolationException):
  21. continue
  22. angular =4* math.atan2(trans[1],trans[0])
  23. linear =0.5* math.sqrt(trans[0]** 2+ trans[1]** 2)
  24. turtle_vel.publish(turtlesim.msg.Velocity(linear,angular))
  25. 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”。代码如下(nodes/fixed_tf_broadcaster.py):

  1. #!/usr/bin/env python
  2. importroslib
  3. roslib.load_manifest('learning_tf')
  4. importrospy
  5. importtf
  6. if__name__ =='__main__':
  7. rospy.init_node('my_tf_broadcaster')
  8. br =tf.TransformBroadcaster()
  9. rate =rospy.Rate(10.0)
  10. whilenotrospy.is_shutdown():
  11. br.sendTransform((0.0,2.0,0.0),
  12. (0.0,0.0,0.0,1.0),
  13. rospy.Time.now(),
  14. "carrot1",
  15. "turtle1")#建立一个新的参考系,父参考系为turtle1,并且距离父参考系2米
  16. rate.sleep()

  在launch文件中添加节点:

  1. <launch>
  2. ...
  3. <node pkg="learning_tf"type="fixed_tf_broadcaster.py"
  4. name="broadcaster_fixed"/>
  5. </launch>

  运行,还是看到两只乌龟和之前的效果一样。新添加的参考系并没有对其他参考系产生什么影响。打开nodes/turtle_tf_listener.py文件,将turtle1改成carrot1:

  (trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

  重新运行,现在乌龟之间的跟随关系就改变了:

  

  3、建立移动参考系(moving frame)

  我们建立的新参考系是一个固定的参考系,在仿真过程中不会改变,如果我们要把carrot1参考系和turtle1参考系之间的关系设置可变的,可以修改代码如下:

  1. #!/usr/bin/env python
  2. importroslib
  3. roslib.load_manifest('learning_tf')
  4. importrospy
  5. importtf
  6. importmath
  7. if__name__ =='__main__':
  8. rospy.init_node('my_tf_broadcaster')
  9. br =tf.TransformBroadcaster()
  10. rate =rospy.Rate(10.0)
  11. whilenotrospy.is_shutdown():
  12. t =rospy.Time.now().to_sec()* math.pi
  13. br.sendTransform((2.0* math.sin(t),2.0* math.cos(t),0.0),
  14. (0.0,0.0,0.0,1.0),
  15. rospy.Time.now(),
  16. "carrot1",
  17. "turtle1")
  18. rate.sleep()<font size="3"><br></font>

  这次carrot1的位置现对于turtle1来说是一个三角函数关系了。

  

  代码下载:http://download.csdn.net/detail/hcx25909/5708199

  原创文章,转载请注明:转载自古月居

  本文链接地址:ROS探索总结(十二)——坐标系统

声明:本文由入驻搜狐公众平台的作者撰写,除搜狐官方账号外,观点仅代表作者本人,不代表搜狐立场。
推荐阅读