ROS Indigo learning_tf-04 （编程）让 turtle2 海龟跟随 turtle1 海龟，并绕着 turtle1 海龟转圈 (Python版)

我使用的虚拟机软件：VMware Workstation 11
使用的Ubuntu系统：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo

这个博客是在上一个博客（添加额外的坐标系）的基础上进行的。

现在，我们想让：让 turtle2 海龟跟随 turtle1 海龟，并绕着 turtle1 海龟转圈。

我们需要在上一个博客的基础上，做三件事情：

  • 第一件事：让 turtle2 小海龟 设置在 carrot1 坐标系节点上。即：改写 turtle_tf_listener.py 程序。

  • 第二件事：设置旋转圆的半径，将其设置为5比较好。即：改写 fixed_tf_broadcaster.py 里面的一个参数。

  • 第三件事：重写启动文件。 改写 start_demo3.launch 文件。

第一件事：让 turtle2 小海龟 设置在 carrot 坐标系节点上

想要完成这样的运行效果，使用之前的（编写一个 监听 tf 变化 的程序 ）的知识，就可以做到：

修改 1 . 我们只需要将 turtle_tf_listener.py 文件（监听tf变化的监听器程序）中的 try: 下面的 (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0)) 这段代码中的 /turtle2 该为 carrot1 ，即可。其他代码不变。

修改 2 . 将 turtle2 小海龟的运行的线速度提高，同时将角速度降低：

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

我们不在原来的 turtle_tf_listener.py 文件里面改。而是，新建一个文件：fixed_tf_listener.py

$ roscd learning_tf/nodes
$ gedit fixed_tf_listener.py

最终修改后的代码：

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

if __name__ == '__main__':
    rospy.init_node('fixed_tf_listener')
    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'carrot1')

    turtle_vel = rospy.Publisher('carrot1/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/carrot1', '/turtle1',rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 6 * 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()

编写 python 程序的时候一定要注意：python 程序对程序中的 Tab 键敏感，所以，不要忘了 。

记住： 一定要给这个 python 程序加可执行权限。

chmod 777 fixed_tf_listener.py

第二件事：设置旋转圆的半径，将其设置为5比较好

在 fixed_tf_broadcaster.py 文件里面直接修改。

修改 ： 将br.sendTransform() 函数的第一个参数修改为：(5.0 * math.sin(t), 5.0 * math.cos(t), 0.0) 。这样 /carrot1 的坐标系节点就会以 /turtle1 坐标系节点为圆心，绕其顺时针画圆。

修改后的完整的 fixed_tf_broadcaster.py 文件 里的代码：

#!/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((5.0 * math.sin(t), 5.0 * math.cos(t), 0.0),
                (0.0, 0.0, 0.0, 1.0),
                rospy.Time.now(),
                "carrot1",
                "turtle1")
        rate.sleep()

第三件事：编写一个启动文件：

并且在启动脚本文件中添加 fixed_tf_listener.py 程序，启动脚本文件另存为：start_demo4.launch : 

$ roscd learning_tf/launch
$ gedit start_demo4.launch

修改 1 . 在 start_demo3.launch 文件的 <launch> 便签里面添加下面这句：

<node pkg="learning_tf" type="fixed_tf_listener.py" name="listener_fixed"/>

修改 2 . 并删除下面这句：

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

另存为：start_demo4.launch :

完整的程序：

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

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

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

搞定

运行启动文件 start_demo4.launch ：

roslaunch learning_tf start_demo4.launch

运行效果：

这里写图片描述

下一节，我来这是 tf 的一个比较强的功能：时间穿梭。