Note: This tutorial assumes you have completed the writing a tf listener 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.

フレームを加える (Python)

Description: このチュートリアルではどのようにして特別な固定のフレームをtfに加えるかを学びます

Tutorial Level: BEGINNER

Next Tutorial: tf and time (Python)

前回のチュートリアルの中で、tf broadcasterとtf listenerを加えることでturtle のデモを再現しました。今回のチュートリアルでは、どのようにtfツリーに特別なフレームを加えるかについて学んでいきましょう。この過程はtf broadcasterを作ることにとても似ていて、tfの利便さの一端を実感できるでしょう。

なぜフレームを追加するのか

ほとんどのタスクにおいて、ローカルフレームの中について考えるのは簡単です、例えば、レーザスキャナの中心でのフレームでレーザスキャンを推論するのは簡単です。tfは、それぞれのセンサにローカルフレームを定義し、リンクし、そのほかのこともできるようにしてくれています。そして、tfは導入された特別なフレームのtransformもすべて管理してくれます。

どこにフレームを追加すべきか

tfはフレームの木構造(tree structure)を作ります(フレーム構造の中に閉ループができることが許されません)。つまり、フレームは一つの親のみを持つが、子は複数もっているということです。現在、今までのチュートリアルでできたtfツリーは3つのフレームをもっています。ワールド、turtle1、turtle2です。二つのturtleは、worldの子にあたります。もし、新しいtfを加えたいなら、3つのすでにあるフレームが親のフレームになる必要があり、新しいフレームは子フレームになるでしょう。

  • tree.png

どのようにフレームを加えるか

turtleの例を用いて、1匹目のturtleに新しいフレームを加えましょう。このフレームは2匹目のturtleにとって、"carrot"になるでしょう。

ソースファイルから作っていきましょう。まずは一つ前のチュートリアルで作ったパッケージのディレクトリに移動してください。:

 $ roscd learning_tf

コード

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

   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 
   5 import rospy
   6 import tf
   7 
   8 if __name__ == '__main__':
   9     rospy.init_node('my_tf_broadcaster')
  10     br = tf.TransformBroadcaster()
  11     rate = rospy.Rate(10.0)
  12     while not rospy.is_shutdown():
  13         br.sendTransform((0.0, 2.0, 0.0),
  14                          (0.0, 0.0, 0.0, 1.0),
  15                          rospy.Time.now(),
  16                          "carrot1",
  17                          "turtle1")
  18         rate.sleep()

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

  • chmod +x nodes/fixed_tf_broadcaster.py

コードはとてもtf broadcaster tutorialに似ています。. ここでは、transformは時間とともに変化することはありません。

コード解説

このコードの中のキーとなる行を部分的に見ていきましょう。:

  13         br.sendTransform((0.0, 2.0, 0.0),
  14                          (0.0, 0.0, 0.0, 1.0),
  15                          rospy.Time.now(),
  16                          "carrot1",
  17                          "turtle1")

ここで新しい親のturtle1から新しい子のcarrot1へのtransformを作成しています。 carrot1フレームは、turtle1から左にオフセット2メートルのところです。

frame broadcasterを実行する

start_demo.launch に以下の行を加える様に変更してください:

  <launch>
    ...
    <node pkg="learning_tf" type="fixed_tf_broadcaster.py"
          name="broadcaster_fixed" />
  </launch>

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

 $ roslaunch learning_tf start_demo.launch

結果を確認する

もし一匹目のturtleを動かしたら、新しいフレームを加えたのにもかかわらず、以前のチュートリアルと動きが変わっていないことに気づくと思います。それは、特別なフレームを加えることは他のフレームになんら影響を与えないからで、listenerは今はまだ以前に定義されたフレームを使用しています。それでは、ここでlistenerの振る舞いを変えてみましょう。

nodes/turtle_tf_listener.pyを開いて、"/turtle1""/carrot1"に置き換えてください:

   1 (trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

うれしいことに、単に再実行するだけで、2匹目の亀がcarrotを1匹目の亀の代わりに追いかけているのが見えると思います。carrotはturtle1の左2メートルを常に動いていることを思い出してください。そこには、キャロットに該当する視覚的な表示はそこにありませんが、それを追っかけて2匹目の亀が動いているのが見えると思います。

 $ roslaunch learning_tf start_demo.launch

動いているフレームをブロードキャストする

このチュートリアルで配信するようにした余分なフレームは、親のフレームに対して時間がたっても位置が変わらないものとなっています。しかし、もし動いているフレームを配信したければ、時間で変化するようにbroadcasterを変えることができます。

/carrot1のフレームが/turtle1に呼応して、時間が経つにつれて位置が変わるようにfixed_tf_broadcaster.pyを修正しましょう。

   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 
   5 import rospy
   6 import tf
   7 import math
   8 
   9 if __name__ == '__main__':
  10     rospy.init_node('my_tf_broadcaster')
  11     br = tf.TransformBroadcaster()
  12     rate = rospy.Rate(10.0)
  13     while not rospy.is_shutdown():
  14         t = rospy.Time.now().to_sec() * math.pi
  15         br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
  16                          (0.0, 0.0, 0.0, 1.0),
  17                          rospy.Time.now(),
  18                          "carrot1",
  19                          "turtle1")
  20         rate.sleep()

これで、次のtfと時間についてのチュートリアルに進む準備ができました。(Python) (C++)

Wiki: ja/tf/Tutorials/Adding a frame (Python) (last edited 2016-11-06 10:31:37 by ronekko)