Note: This tutorial assumes you have completed the writing a tf broadcaster tutorial (Python) (C++).
(!) 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 listenerを書く (Python)

Description: このチュートリアルでは、どのようにtfを使って座標系フレームの変化を取得するのかを学びます

Tutorial Level: BEGINNER

Next Tutorial: Adding a frame (Python)

  Show EOL distros: 

前回のチュートリアルでは、tf broadcasterをtfにturtleのポーズを配信するために作りました。このチュートリアルでは、tfを使い始めるためにtf listenerを作っていきます。

どのようにtf listenerをつくるか

まず、ソースコードを作りましょう。まず、前回のチュートリアルで作ったパッケージに移動します:

 $ roscd learning_tf

コード

エディタを立ち上げ、以下のソースコードをnodes/turtle_tf_listener.pyとして、コピー&ペーストして保存してください。

   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 import rospy
   5 import math
   6 import tf
   7 import turtlesim.msg
   8 import turtlesim.srv
   9 
  10 if __name__ == '__main__':
  11     rospy.init_node('tf_turtle')
  12 
  13     listener = tf.TransformListener()
  14 
  15     rospy.wait_for_service('spawn')
  16     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
  17     spawner(4, 2, 0, 'turtle2')
  18 
  19     turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity)
  20 
  21     rate = rospy.Rate(10.0)
  22     while not rospy.is_shutdown():
  23         try:
  24             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
  25         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  26             continue
  27 
  28         angular = 4 * math.atan2(trans[1], trans[0])
  29         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
  30         turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))
  31 
  32         rate.sleep()

   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 import rospy
   5 import math
   6 import tf
   7 import geometry_msgs.msg
   8 import turtlesim.srv
   9 
  10 if __name__ == '__main__':
  11     rospy.init_node('tf_turtle')
  12 
  13     listener = tf.TransformListener()
  14 
  15     rospy.wait_for_service('spawn')
  16     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
  17     spawner(4, 2, 0, 'turtle2')
  18 
  19     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist)
  20 
  21     rate = rospy.Rate(10.0)
  22     while not rospy.is_shutdown():
  23         try:
  24             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
  25         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  26             continue
  27 
  28         angular = 4 * math.atan2(trans[1], trans[0])
  29         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
  30         cmd = geometry_msgs.msg.Twist()
  31         cmd.linear.x = linear
  32         cmd.angular.z = angular
  33         turtle_vel.publish(cmd)
  34 
  35         rate.sleep()

nodeを実行可能にするのを忘れないようにしてください:

  • chmod +x nodes/turtle_tf_listener.py

コード解説

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

   6 import tf

tfパッケージは、transforms(座標変換)を簡単に受け取れるようにtf.TransformListener の実行を提供してくれます.

  13     listener = tf.TransformListener()

ここで、TransformListenerのオブジェクトを作っています。一旦listenerが作成されると、ケーブルを通してtfのtransformationsを受け取り始め、10秒間それらをバッファにためます。

  23         try:
  24             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
  25         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  26             continue
  27 
  28         angular = 4 * math.atan2(trans[1], trans[0])
  29         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
  30         turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))

ここで、実際の作業を行っています。特定のtransformationについてlistenerに尋ねます。 4つの引数を見てみましょう:

  1. まず、取得したいtransform(変換)のfromにあたるフレームを指定します。
  2. 取得したいtransform(変換)のtoにあたるフレームを指定します。
  3. transformがしたい時間を指定します。ros::Time(0)を指定すると最新のtransformを取得できます。

例外を取得するためにtry-catchに書かれているのがこれですべてです。

listenerを実行する

テキストエディタで start_demo.launchのlaunchファイルを開き、以下の行を足してください:

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener.py" 
          name="listener" />
  </launch>

まず、前回のチュートリアルのlaunchファイルは止めていることを確認してください(ctrl-cを使ってください)今、完全なturtleのデモを始める準備ができました。:

 $ roslaunch learning_tf start_demo.launch

2匹の亀が動いているturtle simが見えていますね。

結果を確認する

tfのbroadcasterがうまく動作しているかを確かめるには、矢印キーを用いて1匹目の亀を操作してみればよいだけです。(シミュレータではなくターミナルのウィンドウがアクティブになるようにしてください)すると、2匹目の亀が1匹目の亀についてくるのを確認できます。

turtlesimが立ち上がったときに以下のようなものが表示されているかもしれません:

  • [ERROR] [1412840409.742716292]: "turtle2" passed to lookupTransform argument target_frame does not exist. 

これは、turtlesimを立ち上げ、tfのフレームをブロードキャストし始めるのに少し時間がかかっていることで生じていて、この時間はlistenerがturtle2のメッセージが届く前に、transform(変換)を計算しようとしているからです。

さて、これでさらに次のチュートリアルへ進むことができます。次は、どのようにフレームを追加するかを学習します。(Python) (C++)

Wiki: ja/tf/Tutorials/Writing a tf listener (Python) (last edited 2014-09-25 04:55:24 by Moirai)