Note: This tutorial assumes you have completed the adding a frame 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と時間について学ぶ (Python)

Description: このチュートリアルでは、tfツリーでtransformが使えるようになるまでwaitForTransformを使って待つ方法を学びます。

Tutorial Level: INTERMEDIATE

Next Tutorial: Time travel with tf (Python)

tf と 時間

前回のチュートリアルで、座標系フレームのツリーをtfがどのように追っているかについて学びました。この木は、時間とともに変化し、tfはすべてのtransformの日時でスナップショットを蓄えます。(デフォルトで10秒まで)今まで、いつtransformが記録されているか知らずにtfツリーの中の最新のtransformsをlookupTransform()を使って取得しました。このチュートリアルでは、どのように特定の時間でのtransformをとるかについて学びます。

nodes/turtle_tf_listener.pyを変更して、lookupTransform() を以下に変えてください:

   1         try:
   2              now = rospy.Time.now()
   3              (trans,rot) = listener.lookupTransform("/turtle2", "/turtle1", now)
   4         except (tf.LookupException, tf.ConnectivityException):

すると,いきなりlookupTransform()が全て失敗しています。それについてエラー報告しています。

Traceback (most recent call last):
  File "~/ros/pkgs/wg-ros-pkg-trunk/sandbox/learning_tf/nodes/turtle_tf_listener.py", line 25, in <module>
    (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
tf.ExtrapolationException: Extrapolation Too Far in the future: target_time is 1253830476.460, but the closest tf  data is at 1253830476.435 which is 0.024 seconds away.Extrapolation Too Far in the future: target_time is 1253830476.460, but the closest tf  data is at 1253830476.459 which is 0.001 seconds away.Extrapolation Too Far from single value: target_time is 1253830476.460, but the closest tf  data is at 1253830476.459 which is 0.001 seconds away. See http://pr.willowgarage.com/pr-docs/ros-packages/tf/html/faq.html for more info. When trying to transform between /turtle1 and /turtle2. See http://www.ros.org/wiki/tf#Frequently_Asked_Questions

または、もしelectricを使っているなら、以下のように見えるかもしれません:

Traceback (most recent call last):
  File "/home/rosGreat/ROS_tutorial/learning_tf/nodes/turtle_tf_listener.py", line 28, in <module>
    (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
tf.ExtrapolationException: Lookup would require extrapolation into the future.  Requested time 1319591145.491288900 but the latest data is at time 1319591145.490932941, when looking up transform from frame [/turtle1] to frame [/turtle2]

なぜでしょう?異なるtf broadcasterからやってくる全ての座標変換を蓄えるバッファを、それぞれのlistenerはもっています。broadcasterがtransformを送り出したときに、バッファにtransformが入れられるのに時間がかかってしまいます。(通常数ミリ秒)。このため、"今"のフレームtransformを要求したときに、情報が到着するのに数ミリ秒待つ必要があるのです。

transformを待つ

tfは、transformが利用可能になるまでの待ちを実現する便利なツールがあります。どのようなコードになるのか見てみましょう。:

   1     listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
   2     while not rospy.is_shutdown():
   3         try:
   4             now = rospy.Time.now()
   5             listener.waitForTransform("/turtle2", "/turtle1", now, rospy.Duration(3.0))
   6             (trans,rot) = listener.lookupTransform("/turtle2", "/turtle1", now)

waitForTransform()には4つの引数があります:

  1. 取得したい変換のfromにあたるframe
  2. 取得したい変換のtoにあたるframe
  3. 指定する時間
  4. 時間切れ: この最大の期間よりは長く待たない

したがって、waitForTransform()は実際には亀2匹の間のtransformが利用可能になるか、もしtransformが利用可能でない場合、時間切れが来るまでブロックします。

なぜwaitForTransform() を2つ呼ぶのか?コードの頭の方では、turtle2を立ち上げていますが、transformを待つ前なので、tfにはまだ/turtle2のフレームは見えていないかもしれません。最初のwaitForTransform()によって、/turtle2の frameがtfにブロードキャストされるのを待ち、ブロードキャストされてから、ループ内のwaitForTransform()で現在のフレームを取得するように変えています。

結果を確認する

さて、また矢印キーを用いて1匹の亀を動かすことで動作を簡単に確認できます。

これで、亀の振る舞いに確認できるような違いは出てないのがわかるでしょう。それは、実際のタイミングの違いがほんの数ミリ秒であるためです。では、なぜTime(0) からnow()への変更をしたのか?それは単に、tfのバッファとそれに関係して生じる時間の遅れについて教えておきたかったためです。実際のtfの使用の際は、Time(0)を使うのでなんら問題はありません。

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

Wiki: ja/tf/Tutorials/tf and Time (Python) (last edited 2020-02-02 11:11:39 by TatsuhisaYamaguchi)