一 tf / Tutorials / Writing a tf broadcaster (Python)

原文地址：http://wiki.ros.org/tf/Tutorials/Writing a tf broadcaster (Python)

Writing a tf broadcaster (Python)

描述：本教程教您如何向tf广播机器人的状态。

0.1 Create package

注意：只有在没有做tf broadcaster（C++）教程的情况下，才需要看本节。
在接下来的两个教程中，我们将编写代码再现tf introduction tutorial中的demo。之后，下面的教程将重点介绍如何用更高级的tf特性扩展demo。
在我们开始之前，您需要为这个object创建一个新的ros包。在sandbox中，创建一个名为learning_tf的包，该包依赖于tf、roscpp、rospy和turtlesim：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

在roscd之前编译您的新包：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/
 $ catkin_make
 $ source ./devel/setup.bash

1 How to broadcast transforms

本教程教您如何向tf广播坐标系。在这种情况下，我们希望广播海龟在移动时坐标系的变化。
让我们首先创建源文件。转到我们刚刚创建的包：

 $ roscd learning_tf

1.1 The Code

让我们首先在我们的learning_tf包中创建一个名为nodes的新directory。

$ mkdir nodes

启动您喜欢的编辑器，并将以下代码粘贴到一个名为nodes/turtle_tf_broadcaster.py的新文件中。



   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 import rospy
   5 
   6 import tf
   7 import turtlesim.msg
   8 
   9 def handle_turtle_pose(msg, turtlename):
  10     br = tf.TransformBroadcaster()
  11     br.sendTransform((msg.x, msg.y, 0),
  12                      tf.transformations.quaternion_from_euler(0, 0, msg.theta),
  13                      rospy.Time.now(),
  14                      turtlename,
  15                      "world")
  16 
  17 if __name__ == '__main__':
  18     rospy.init_node('turtle_tf_broadcaster')
  19     turtlename = rospy.get_param('~turtle')
  20     rospy.Subscriber('/%s/pose' % turtlename,
  21                      turtlesim.msg.Pose,
  22                      handle_turtle_pose,
  23                      turtlename)
  24     rospy.spin()

不要忘记使节点可执行：

chmod +x nodes/turtle_tf_broadcaster.py

1.2 The Code Explained

现在，让我们看一下与向tf发布turtle pose相关的代码。

  19     turtlename = rospy.get_param('~turtle')

该节点采用单个参数“turtle”，该参数指定一个turtle名称，例如“turtle1”或“turtle2”。

  20     rospy.Subscriber('/%s/pose' % turtlename,
  21                      turtlesim.msg.Pose,
  22                      handle_turtle_pose,
  23                      turtlename)

节点订阅话题“turtleX/pose”，并在每个传入消息上运行函数handle_turtle_pose。

  10     br = tf.TransformBroadcaster()
  11     br.sendTransform((msg.x, msg.y, 0),
  12                      tf.transformations.quaternion_from_euler(0, 0, msg.theta),
  13                      rospy.Time.now(),
  14                      turtlename,
  15                      "world")

用于海龟pose message的handler function广播海龟的平移和旋转，并将其发布为从坐标系“world”到坐标系“turtleX”的transform。

2 Running the broadcaster

现在为这个demo创建一个启动文件。使用文本编辑器，创建一个名为launch/start_demo.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>

  </launch>

现在你已经准备好开始你自己的turtle broadcaster demo了：

$ roslaunch learning_tf start_demo.launch

你会看到带有一个turtle的turtlesim。

3 Checking the results

现在，使用tf_echo工具来检查乌龟的pose是否真的被广播到tf:

 $ rosrun tf tf_echo /world /turtle1

这应该能显示第一只乌龟的pose。使用箭头键驱动海龟（确保你的终端窗口是活动的，而不是你的模拟器窗口）。如果运行tf_echo来transform world和turtle 2，则不应该看到transform，因为第二只turtle还没有出现。然而，一旦我们在下一教程中添加了第二只乌龟，乌龟2的姿态将被广播到tf。
为了实际使用到tf的转换广播，您应该转到下一篇关于创建tf侦听器的教程。