ROS：关于tf的探索（1） Writing a tf broadcaster（Python）

此小节教会你如何发布机器人的状态给tf。
创建包

 首先我们要建立一个新的名为learning_tf 的 package，它依赖于 tf,roscpp,rospy,turtlesim.

cd ~/catkin_ws/srccatkin_creat_pkg learning_tf tf roscpp rospy turtlesim

1.如何发布变化

本教程教你如何发布坐标系给tf，在这个案例中，我们要发布小乌龟在不断运动中的坐标系。
我们首先创建资源文件，到达我们刚刚创建的包目录下：

roscd learning_tf

1.1 代码
首先在包中创建一个新的目录名为nodes

mkdir nodes

然后用编辑器将如下代码写入文件nodes/turtle_tf_broadcaster.py.

roscd learning_tf/nodesvim turtle_tf_broadcaste.py

将如下代码下入

#!/usr/bin/env python  import roslibroslib.load_manifest('learning_tf')import rospyimport tfimport turtlesim.msgdef handle_turtle_pose(msg, turtlename):    br = tf.TransformBroadcaster()    br.sendTransform((msg.x, msg.y, 0),                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),                     rospy.Time.now(),                     turtlename,                     "world")if __name__ == '__main__':    rospy.init_node('turtle_tf_broadcaster')    turtlename = rospy.get_param('~turtle')    rospy.Subscriber('/%s/pose' % turtlename,                     turtlesim.msg.Pose,                     handle_turtle_pose,                     turtlename)    rospy.spin()

让代码可执行

chmod +x nodes/turtle_tf_broadcaster.py

1.2 代码解释

turtlename = rospy.get_param('~turtle')

该节点采用单个参数“turtle”，其指定乌龟名称，例如。 “乌龟”或“乌龟2”。

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

该节点订阅话题“turtlex/pose”，并对进来的消息进行handle_turtle_pose处理。

br = tf.TransformBroadcaster()    br.sendTransform((msg.x, msg.y, 0),                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),                     rospy.Time.now(),                     turtlename,                     "world")

处理乌龟姿态的函数会发布乌龟的平移和旋转，并且以乌龟坐标系相对于基坐标系的转换形式发布。

2.执行broadcaste

现在创建launch文件，launch/start_demo.launch，并添加如下代码。

roscd learning_tf/launchvim 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>

现在可以开始自己的乌龟发布展示了。

roslaunch learning_tf start_demo.launch

运行出现问题：

ERROR: cannot launch node of type [learning_tf/turtle_tf_broadcaster.py]: can't locate node [turtle_tf_broadcaster.py] in package [learning_tf]ERROR: cannot launch node of type [learning_tf/turtle_tf_broadcaster.py]: can't locate node [turtle_tf_broadcaster.py] in package [learning_tf]

解决办法

chmod +x nodes/turtle_tf_broadcaster.py

将Python文件变成可执行文件

现在可以看到乌龟仿真器内有一只乌龟。

3.检查结果

运用tf_echo检查乌龟的位置是否被准确发送给tf

rosrun tf tf_echo /world /turtle1