基于turtlesim♂的ROS_TF应用案例详解（♂有毐慎杁♂）

   

                                       参考wiki的ros tf demo教程                                                     纯粹科研精华～～～

---

 

很遗憾，截止发稿，笔者只有60小时的ROS系统开发经验，但是，这篇文章绝对不是拿来搞笑的，我会尽量写的正式一点。如果你看完了还是不懂，不能怪笔者写的太不正经，只能证明ROS这东西确实不是人学的。

ROS原生自带的  turtlesim（龟模拟）是一个功能全面，复古蒸汽波风格的仿真系统。我对原版文档中的TF教程demo做了一个小小升级——将原本的两只龟增加到了四只，其中涉及一些新的坐标变换、新节点注册、launch文档修改。理论上可以实现无限只龟跟随头龟运动！

这个demo中涉及的具体技术我会详细讲解，大多数翻译自 ros.wiki，内容严谨性和翻译质量都不好保证，大家看看可以，最好不要亲身尝试。

有兴趣的读者不妨移步http://wiki.ros.org/获取更多资讯。

 

准备原料：

1.安装Ubuntu的电脑。

2.电脑已安装完整版ROS，必要测试已完成，能打开turtlesim。

3.准备数只美学乌龟

美学乌龟效果图

 

 

 

1.在您的catkin_ws/src/目录下新建一个 叫 “vapor_turtle” 的文件夹（不叫这名字、不建文件夹，美学效果都会大打折扣）

    1.1懒人代码： $    catkin_create_pkg   vapor_turtle   roscpp    rospy    std_msgs

 

2.在vapor_turtle中新建 launch 和 nodes 文件夹，您一定知道这是干嘛的。

 

3.在nodes文件夹中新建两个新PY文件，“turtle_listener.py" 和"turtle_

broadcaster.py"。

 

4.复制以下代码

    4.1 turtle_tf_listener：

#!/usr/bin/env python
import roslib
roslib.load_manifest('sustc_follow')
import rospy #rospy 必须import的包
import math  #数学计算包
import tf    #TF包
import geometry_msgs.msg  #这个包含乌龟的几何信息
import turtlesim.srv   #这个是“龟模拟“仿真器



if __name__ == '__main__':
     rospy.init_node('turtle_tf_listener') #初始化turtle_listener的节点名称

     listener = tf.TransformListener()  #实例化坐标监听器

     # 使用spawn命令生出多只小乌龟来
     rospy.wait_for_service('spawn')
     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
     # Add TMD new turtle!
     spawner(4, 2, 0, 'turtle2') #第一只小乌龟已经默认生成，这里从第二只开始
     spawner(2, 2, 0, 'turtle3') #前面的三个数字是在地图上的初始坐标
     spawner(1, 2, 0, 'turtle4')


     # Set TMD turtle twist value(by TMD publisher)
     #这是每一只龟的消息发布器
     turtle1_vel = rospy.Publisher('turtle1/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
     turtle2_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
     turtle3_vel = rospy.Publisher('turtle3/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
     turtle4_vel = rospy.Publisher('turtle4/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
     #每秒操作10次
     rate = rospy.Rate(10.0)
     
     while not rospy.is_shutdown():
         #这里做一个小循环，可以分别对每一只龟进行操作
         for i in range(3):
             try:
                 #此处返回的，是前后两只乌龟的坐标变换数据
                 (trans, rot) = listener.lookupTransform('/turtle'+bytes(i+2), '/turtle'+bytes(i+1), rospy.Time(0))
             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                 continue
                 
             #我们定义一下，cmd是一种Twist类型的msg
             cmd = geometry_msgs.msg.Twist()
             #这里是坐标变换
             angular = 4 * math.atan2(trans[1], trans[0])
             linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
             cmd.linear.x = linear
             cmd.angular.z = angular

             #向当前的乌龟发布新的线速度和角速度
             if i == 0:
                turtle2_vel.publish(cmd)
             if i == 1:
                turtle3_vel.publish(cmd)
             if i == 2:
                turtle4_vel.publish(cmd)

             rate.sleep()

        4.2 turtle_tf_broadcaster：

#!/usr/bin/env python
import roslib  
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    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')
    print 'turtlename: ' + turtlename
    rospy.Subscriber('/%s/pose' % turtlename,
                      turtlesim.msg.Pose,
                      handle_turtle_pose,
                     turtlename)
    rospy.spin()

             这些代码基本上就是ros wiki里的原版代码，我改动的部分等下会详细讲解

5.最后在launch文件夹里创建一个launch文件，暂且管它叫 vapor-turtle.launch

 复制完成后，懒惰的同学可以直接roslaunch这个文件，马上就可以看到乌龟在你的屏幕出  现，但如果您满脸懵逼，笔者还是建议您继续往下阅读。

 

 

我们从turtle_broadcaster开始讲起

       turtle_broadcaster 的作用：

               只要有龟跑了这个脚本，它就会把这只龟的当前的坐标系broadcast出去

       大家不妨上拉回看一下 vapor_turtle.launch 文件代码，其中重复次数最多的部分，一定是这段：

 以上代码，为每一只龟分配了一个tf-broadcast节点，节点type就是 turtle_tf_broadcaster.py 而每只乌龟的名称，在param参数项的value给予定义。

---

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    print 'turtlename: ' + turtlename
    rospy.Subscriber('/%s/pose' % turtlename,
                      turtlesim.msg.Pose,
                      handle_turtle_pose,
                     turtlename)
    rospy.spin()

这是turtle_tf_broadcaster的main函数，我们刚刚在launch文件中定义的value（龟的名字）可以轻松用rospy获得。随后的Subscriber订阅了turtlesim.msg.Pose类型的msg，Pose包含了32位浮点数的坐标信息；获得的Pose.msg 会交由名为 handle_turtle_pose 的回调函数做进一步处理。

---

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                      rospy.Time.now(),
                      turtlename,
                      "world")

这是回调函数 handle_turtle_pose。显然，它把刚刚获得的pose.msg 发布了出去（注意角度已转换为欧拉角）。

      注意 tf.TransformBroadcaster() 的深度用法笔者基本不会，所以笔者在此不做展开

---

                                               以上是turtle_tf_broadcaster的详解

 

 

 

 

---

下面来看turtle_tf_listener的工作原理

             turtle_tf_listener的作用：

      从tf系统中获得这一堆龟的坐标，稍作处理发布给每只龟的实体节点，以控制龟运动

很遗憾，笔者渐渐对写博客失去了兴趣，所以我把turtle_tf_listener的注释补充了一下，简单部分不单独讨论。 turtle_tf_listener的难点在于spawn、transFormListener()和龟与龟间的速度角度变换，我们先看spawn(再生)。

 rospy.wait_for_service('spawn')
     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
     # Add TMD new turtle!
     spawner(4, 2, 0, 'turtle2')
     spawner(3, 2, 0, 'turtle3')
     spawner(2, 2, 0, 'turtle4')

    spawn使用效果图

spawn是 turtlesim节点所提供的一个service。在终端输入：

$ rosservice type spawn| rossrv show

可查看该service的详细信息。注意，在此我们使用 rospy.ServiceProxy() 函数指定服务名来调用服务，传入的两个参数分别是service名和service类型，这个函数的实例可以直接使用。

# Add TMD new turtle!
     spawner(4, 2, 0, 'turtle2')
     spawner(3, 2, 0, 'turtle3')
     spawner(2, 2, 0, 'turtle4')

$ rosservice call spawn 2 2 0 "turtle2"

以上python代码和linux指令完成相同的工作，都是给定初始坐标、角度和乌龟名称，新生成一只乌龟。或许您会有疑问，为何没有‘turtle1’ ，实际上，在我们打开turtlesim时，turtle1就会自动生成，如果我们用spawner重复添加，运行脚本时会报错：

[ERROR] [1532226272.126496492]: A turtled named [turtle1] already exists

 

---

transformListener()的使用：

 listener = tf.TransformListener()
     rate = rospy.Rate(10.0)
     while not rospy.is_shutdown():
         for i in range(3):
             try:
                 (trans, rot) = listener.lookupTransform('/turtle'+bytes(i+2), '/turtle'+bytes(i+1), rospy.Time(0))
             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                 continue

tf.transformListener()是tf.TransformerROS的子类，用来接收tf类型的topic。 代码中的listener.lookupTransform() 实际上是 tf.TransformerROS的函数。它的详细用法在API可见

它接收三个参数，目标坐标系、源坐标系、时间。在本案例中，编号大的乌龟跟随前一只移动，如果要做turtle2和turtle1的坐标转换，目标坐标系应是turtle2，源坐标系应是turtle1。

返回值就是两坐标系之间的差值，给出了相对位置和四元数旋转两种信息。

---

 

本案例，我们使用返回的两龟相对位置信息做进一步处理。

angular = 3 * math.atan2(trans[1], trans[0])
 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)

使用math.atan2函数获得后龟与前龟间的方位角，其间我们在它的前面乘上一个系数，大家可以改一改这个系数看看效果，大概反应了龟的转弯响应速度，这个系数如果过小，龟会显得傻了吧唧的，过大呢龟会抽搐。

参数过大效果图：

后龟的线速度我们通过计算与前龟的距离得出，大概效果就是距离越远，后龟追的越快。

最后要做的事情，就是把新生成的线速度和角度包装成Twist.msg，发布给每一只龟。

             cmd = geometry_msgs.msg.Twist()
             cmd.linear.x = linear
             cmd.angular.z = angular

             if i == 0:
                turtle2_vel.publish(cmd)
             if i == 1:
                turtle3_vel.publish(cmd)
             if i == 2:
                turtle4_vel.publish(cmd)

  全文完