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