【ROS第十三课】《TF坐标系广播与监听的编程实现》详解

首先要知道左上角坐标是（0,11），左下角是（0,0）

正中间位置是（5，5）

如果turtle2从左上角出来，面向正中间的turtle1的向量差就是（5，-5）

不要问我为什么，y值就是要+1

而这个向量差trans的计算是由

(trans, rot) = listener.lookupTransform("/turtle2", "/turtle1", rospy.Time(0))

得出

再将trans计算得出角度和直线距离

速度 = 直线距离*0.5， 距离越近速度越慢，距离越远速度越快

2.而listener.lookupTransform要能正常得出向量差的前提是要得出两只小乌龟的坐标，

这时turtle_tf_broadcaster.py就派用上场了，它的作用就是不停地发出每只小乌龟相对world的坐标。有人说，直接监听两个小乌龟的turtlesim.msg.Pose然后直接计算不就可以了吗？

当小乌龟还比较少的还确实省事，但是我们要知道真实世界我们要面对的是一个机器人大量的组件，当组件越来越多的时候，就如同小乌龟越来越多，订阅的监听器就会越来越多，而要计算任意两者之间的向量差，就要重新添加一次运算代码，复杂度就会成倍地增加。

但有没有一种方式，每个组件各自发布自己的坐标好了（等下我们会讲到如何启动每个小乌龟的tf广播），真正想计算时再取出指定两个组件的坐标来计算他们的关系呢？当然有，就是以上我们说的那个

 (trans, rot) = listener.lookupTransform("/turtle2", "/turtle1", rospy.Time(0))

想计算哪两个单元的向量差，随时替换就可以了。

3 . 最后把这个向量差，指定给某个小乌龟运行起来

turtle_vel.publish(cmd)

4.那如何启动每个小乌龟的tf广播？

通过start_tf_demo_py.launch我们可以观察到

<node pkg = "tf_hiwonder" type = "turtle_tf_broadcaster.py" name = "tf_broadcaster1" respawn = "false" output = "screen">
  <param name = "turtle" type = "string" value="turtle1"/>
</node>
<node pkg = "tf_hiwonder" type = "turtle_tf_broadcaster.py" name = "tf_broadcaster2" respawn = "false" output = "screen"><!--  if no broadcast ,turtle will not follow   -->
  <param name = "turtle" type = "string" value="turtle2"/> 
</node>

我们注意到中间的param节点，

<param name = "turtle" type = "string" value="turtle2"/>

它对应的是turtle_tf_broadcaster.py

turtlename = rospy.get_param("~turtle") #当上述valuse=turtle2是这里的turtlename=turtle2

当上述valuse=turtle2，这里的turtlename=turtle2

    rospy.Subscriber("/%s/pose"%turtlename,
        turtlesim.msg.Pose,
        handle_turtle_pose,
        turtlename)

此时就可以监听turtle2的位姿，注意这里的Subscriber第四个参数是自定义的，在后面的

handle_turtle_pose(msg, turtlename)函数中会带进去

5.其他关于lauch文件的注意事项：

• type=xx.py文件如果是自己写的py要加入.py

<node pkg = "tf_hiwonder" type = "turtle_tf_broadcaster.py" 
name = "tf_broadcaster2" respawn = "false" output = "screen">

• log要打印出来，必须要output=screen，否则终端不显示的print或者rospy.loginfo函数的内容