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 的一个比较强的功能:时间穿梭