博客

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


创建包

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

cd ~/catkin_ws/src
catkin_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/nodes
vim turtle_tf_broadcaste.py

将如下代码下入

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def 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/launch
vim 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