ROS与Python入门教程-TF-开发tf的broadcaster(广播器)

ROS与Python入门教程-TF-开发tf的broadcaster(广播器)

说明

• 介绍如何开发broadcaster来广播机器人的状态到TF

步骤

创建包：

• 创建

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

• 编译

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

broadcast transforms（广播变换）

• 实现广播坐标系到TF。在这种情况下，我们要广播的是不断变化的坐标框架的海龟，因为他们在移动。
• 进入learning_tf, 建立nodes文件，新建nodes/turtle_tf_broadcaster.py.文件

$ roscd learning_tf
$ mkdir nodes
$ touch nodes/turtle_tf_broadcaster.py
$ chmod +x nodes/turtle_tf_broadcaster.py
$ rosed nodes/turtle_tf_broadcaster.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()

代码分析

• 代码：turtlename = rospy.get_param('~turtle')

• 分析：节点获取turtle参数，这个指定一个turtle名，如"turtle1" or "turtle2".

• 代码：

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

• 分析：

  • 节点订阅"turtleX/pose" 主题，指定handle_turtle_pose回调函数
  • 回调函数有参数，节点名turtlename和相关信息turtlesim.msg.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")

• 分析：
  • 处理函数针对turtle的位置信息广播平移和旋转。
  • 作为变换，从"world" 坐标系到"turtleX"坐标系进行发布。

运行广播

• 创建launch文件，launch/start_demo.launch

$ roscd learning_tf
$ mkdir launch
$ touch launch/start_demo.launch
$ rosed 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>

• 运行：

$ roslaunch learning_tf start_demo.launch

测试结果：

• 命令：

 $ rosrun tf tf_echo /world /turtle1

• 这应该显示你的第一个海龟的姿势。用箭头键（确保你的终端窗口是活动的，而不是你的模拟器窗口）。
• 如果你对世界和海龟2之间的变换tf_echo，你不应该看到一个变换，因为第二龟是没有。
• 然而，当我们在下一个教程中添加第二个海龟，海龟2的姿势将被广播到。