Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

tfのbroadcasterを書く(Python)

catkin rosbuild

パッケージを作る

NOTE: C++でWriting a tf broadcaster (C++) tutorialを終えていない場合だけ、この項に取り組んで下さい.

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

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/
 $ catkin_make

 $ roscd tutorials
 $ roscreate-pkg learning_tf tf roscpp rospy turtlesim
 $ rosmake learning_tf

 $ roscd learning_tf

コード

それでは、'nodes'と呼ばれる新しいディレクトリをlearning_tfパッケージの中に作りましょう。

$ 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

コード解説

さて、tfにturtleのポーズを配信している部分に関係しているコードを見てみましょう。

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

このnodeは、亀の名前を明記する"turtle"という単一のパラメータのみをもちます。（例えば、 "turtle1" や "turtle2"のような名前）

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

nodeは、"turtleX/pose"というtopicを購読し、入力されるすべてのメッセージに対して、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")

亀の姿勢のメッセージを扱う関数は、この亀の移動と回転をブロードキャストし、"world"フレームから、"turtleX"フレームへの変換として配信します。

ブロードキャストを実行する

このデモのためのlaunchファイルを作成します。テキストエディタで新しく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のデモを実行する準備が整いました。:

 $ roslaunch learning_tf start_demo.launch

一匹の亀だけがturtlesimに表示されているでしょう。

 $ rosrun tf tf_echo /world /turtle1