tf,tf2完全理解

1. TF,TF2TF,TF2　　完全理解完全理解 Koji Terada 第5回ROS勉強会 in 名古屋資料

2. 目次 1. 　自己紹介 2. 　はじめに 3. 　発表趣旨 4. 　今日のゴール 5. 　TFとは？ 6. 　TFの使い方 7. 　空間位置姿勢 8. 　TFの内部構造と実装 9. 　TFとTF2の違いについて 10.　TFアンチパターン　無駄にハマらないために 11. 結論

3. 自己紹介氏名　　　　寺田耕志お仕事　　　ロボット ROS歴　　　5年くらい好きな分野　Kinematics 　　　　　　Dynamics Twitter @tera_koji FB terakoji

4. はじめに • TF便利ですよね・・・。 • でも結構はまりませんか？ • よく理解しなくっちゃ。

5. 発表趣旨 • TFはROSでもっとも重要なライブラリ（私見） – why? rviz,navigation,moveIT!等はすべて tfに依存 • ・・・しかし、ドキュメンテーションが不足か、　理解が難しいとの声も • 挙動がよく分からない(補間は何？とか、 lookupできる条件とか)、ハマること多数 • 今回はコードをとりあえず全部読んで（そんなに長くないです）理解してきました。

6. 今日のゴール • ROS初心者 – とりあえずTFをつかってみたい！！ • ROS中級者 – TFのハマりポイントをシェアできれば。 – TFでやってはいけないことが分かった。 – TF２とTFの違いが分かった！！ • ROS上級者 – TFのContributorになろう。

7. TFとは？ • 分散システム前提の座標変換のライブラリ tfシステム TFが目指すもののイメージ　（※注意：実際の実装とは異なります）親子の2フレーム間の位置姿勢と時間を非同期に報告常に更新 transform_broad_casterが報告任意の2フレーム間の位置姿勢を時間を指定して要求要求の位置姿勢を返す transform_listenerが受信して問い合わせ

8. Step1 ２フレーム間の関係を報告センサから見た物体の位置ロボットから見たセンサの位置フロアから見たロボットの位置 /tf /apple /sensor /sensor /robot /floor /robot

9. Step2 ツリーを構築 /apple /sensor /robot /floor

10. Step3 ツリーをたどって位置を取得 /apple /sensor /robot /floor floorからみた appleの位置姿勢

11. 実際のTF • すべての座標を解決したいノードに座標変換のツリーをもつ • SVNに対するGITのイメージ • リソースはいっぱい食うけど、ロバストで柔軟 Node1 Node2 Node3 /frame1 /frame2 /frame3 /frame4 /frame5 /frame1 /frame2 /frame3 /frame4 /frame5 /frame1 /frame2 /frame3 /frame4 /frame5

12. 位置姿勢の表現 • 位置はx,y,z(m) • 姿勢はQuaternion Why Quaternion? 空間表現の中でもコンパクトで数値計算に強い Quaternionを読む”コツ” ─＞　ある軸周りの回転に簡単に置き換えられる (x, y, z, w) 回転軸のベクトル。ノルムは気にしない。　　回転角度。ただし1.0で0°、0.0で180° 重要なWの値： 1.0 : 0° 　 0.0 : 180° 0.707: ９０° 0.5: 1２0°

13. TFの内部構造と実装

14. TFのROS依存 • TFはROS部分とROSに依存していないライブラリ部分がある。 • ただしパッケージは同じであまり分離できていない※ ※TF2ではパッケージ単位で完全に別れている tf::Transformer ROS（通信）に依存しない座標変換クラス。座標変換と時間を扱う。本質はここ。時間管理だけはros::Timeを利用。任意の分散システムで使える。非ROS ROS tf::TransformListener ROSで実装されたtf::Transformer。 ros::Subscriberで最新の座標系を受け取る。 tf::TransformBroadcaster ros::Publisherで座標の送信ツール群 tf_echo, view_frame, ...etc 継承

15. tf::Transfomer • TFの基本となる最重要クラス • やること – 座標系の更新を受け取り時間と共に管理※ – 座標系のツリーをたどって座標変換 ※TFの優れたところは時間をちゃんと管理しているところと、分散システムに対応し、更新の取りこぼしがあっても大きく破綻しないところ。

16. tf::Transformer　図解 tf::Transformer setTransform() 座標系の更新を通知以下の情報を伝える。 •自分のフレーム名 •子のフレーム名 •更新時間 •自分から子への座標変換 tf::TimeCache[]　※重要メンバキャッシュにストア lookupTransform() 時間を指定して、座標の読みだし。ツリーをたどって変換。フレーム毎に時間管理 /frame_1 /frame_2 /frame_3 /frame_4 WalkToTopParent()　※重要メソッドツリー構築のためのデータ収集 ※重要インターフェイス canTransform() 時間を指定して、ツリーがその時間に存在するかを調べる同じメソッドで解決 /frame_1 /frame_2 /root /frame_3 /frame_4 ツリーの構築と変換

17. tf::Transformer　図解 tf::Transformer setTransform() 座標系の更新を通知以下の情報を伝える。 •自分のフレーム名 •子のフレーム名 •更新時間 •自分から子への座標変換 tf::TimeCache[]　※重要メンバキャッシュにストア lookupTransform() 時間を指定して、座標の読みだし。ツリーをたどって変換。フレーム毎に時間管理 /frame_1 /frame_2 /frame_3 /frame_4 WalkToTopParent()　※重要メソッドツリー構築のためのデータ収集 ※重要インターフェイス canTransform() 時間を指定して、ツリーがその時間に存在するかを調べる同じメソッドで解決 /frame_1 /frame_2 /root /frame_3 /frame_4

18. TimeCache（クラス） • タイムスタンプで並んだTransformを管理。 – 離散的な時間のキャッシュから、任意の時間の座標変換を取得 – 正しくキャッシュにデータをストア – デフォルトで最新のデータより１０[s]以前のデータは消去 – 補間を行う位置は線形補間、姿勢はSlerp※ – 補外はしない時間 ↑ transform n-1 ↑ transform n ※Slerpは姿勢版の線形補間 getDataは2つのTransform間の線形補間を返す

19. TimeCache::getData() • データの得できる条件はそのデータを時間的に挟むデータがあること時間 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間 getDataが可能 getDataが不可能最新より10[s]たちgetDataが不可能な時間 10.0[s]

20. TransformStorage TimeCahceに蓄えられるデータ • フレームID • 子フレームのID • 位置 • 姿勢を持つ

21. tf::Transformer　図解 tf::Transformer setTransform() 座標系の更新を通知以下の情報を伝える。 •自分のフレーム名 •子のフレーム名 •更新時間 •自分から子への座標変換 tf::TimeCache[]　※重要メンバキャッシュにストア lookupTransform() 時間を指定して、座標の読みだし。ツリーをたどって変換。フレーム毎に時間管理 /frame_1 /frame_2 /frame_3 /frame_4 WalkToTopParent()　※重要メソッドツリー構築のためのデータ収集 ※重要インターフェイス canTransform() 時間を指定して、ツリーがその時間に存在するかを調べる同じメソッドで解決 /frame_1 /frame_2 /root /frame_3 /frame_4

22. walkToTopParent(src, target) • srcとtargetからgetData()ができる限り（＝データが利用可能である限り）ツリーの根元を目指す。 /frame_1 /frame_2 /frame0 /frame_3 /frame_4 ツリーの構築と変換位置姿勢を計算しながらたどるとlookupTransform() たどれるかだけを気にするとcanTransform()

23. lookupTransform() • たどるツリーのすべてのフレームが利用可能で無いと計算できない。 • ここがあまりドキュメントで触れていないので分かり難いと思う。 void tf::TransformListener::lookupTransform (std::string &W, std::string &A, ros::Time &time, StampedTransform &transform)

24. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 いつの時間のTFが取れるか？これがTFをしっかり理解するために非常に重要今frame_2とframe_4の変換を取りたい。どうすればいいだろうか？

25. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 パターン1. 時間をros::Time::now()で取得 ─＞　NG ros::Time::now() はここを見る

26. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 パターン2. 時間をros::Time(0)で取得 ─＞　NG ros::Time(0)はここを見る /frame_2と/frame_4の共通の最新

27. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 パターン3. 取れる時間の範囲の中で指定 ─＞　OK　・・・でもその時間をどうやってこの範囲が取れる時間

28. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 パターン4. 時間を決めて利用可能になるまで待つ ─＞　OK　これが基本この時間で取ると決めて waitForTransform

29. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 transformが入ってきた時間 getDataが可能な時間 getDataが不可能な時間最新より10[s]たちgetDataが不可能な時間時間 /frame_1 /frame_2 /frame_3 /frame_4 パターン4. 時間を決めて利用可能になるまで待つ ─＞　OK　これが基本この時間で取ると決めて waitForTransform()

30. パターン４の実際のコード try{ ros::Time now = ros::Time::now(); // ここでnowが利用可能になるまで待つ listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(1.0)); listener.lookupTransform("/turtle2", "/turtle1", now, transform);

31. tf::BroadCaster • tf/tfMessagesを通常のros::Publisherで投げているだけ。

32. tf::TransformListener • tf::Transformerを継承し、ROSの通信部分を付け加えたクラス • クラスを作成すると別Threadを立てて、Subscribe 専任にする（別Threadを立てない設定もあり）

33. tf vs tf2<http://wiki.ros.org/hydro/Migration#tf2.2BAC8-Migration.tf_and_tf2> • Hydroから本格的に移行（tfもtf2で実装されている=まぜて使える） • アルゴリズムはほとんど変わらない • 次の変更が大きなもの 1. tf_staticでツリーに静的なフレームを追加できる 2. ROSと綺麗に分かれた実装 3. テンプレートAPI transform()で直接native型で取り出せる 4. PythonがほぼNative実装に 5. クライアントにツリーを持たせないActionでの実装 6. /tf_prefixの削除 7. waitForTransformの機能がcanTransform,lookupTransformに統合 8. transform_listenerとbufferの分離

34. tf2の変更点 tf_static • /tfと/tf_staticのトピックがあり、tf_staticが受け取った変換は時間に関わらずに利用可能 • 補間とかはしない。すべて最新のデータを使う。 • 注意点：publisherを殺してはいけない。 latched topicで利用することを想定している。

35. /frame_1 /frame_2 /frame_0 /frame_3 /frame_4 時間 /frame_1 /frame_2 /frame_3 /frame_4 tf_static /static_frame1 /static_frame1 staticなフレームはいつでもOK

36. クライアントにツリーを持たせない Actionでの実装 • 従来 Node1 Node2 Node3 /frame1 /frame2 /frame3 /frame4 /frame5 /frame1 /frame2 /frame3 /frame4 /frame5 /frame1 /frame2 /frame3 /frame4 /frame5

37. クライアントにツリーを持たせない Actionでの実装 tf_server Node2 Node3 /frame1 /frame2 /frame3 /frame4 /frame5 Node1 actionlibでlookupTransform() /tf /tf_static 購読

38. ここからは • 私が経験したなかでTFでやっちゃいけないことを列挙していきます • クイズも交えながら進めます

39. 次のコードは何がまずいでしょう？

40. TFアンチパターン１ NG 現在の時間でlookupTransofrom（）前述の仕組みによりTFでは現在の時間は解決できません。本質的に過去の情報を取る必要があります。

41. 次のコードは何がまずいでしょう？

42. TFアンチパターン２ NG リスナーの生存期間を短く（狭く）するリスナーはすべてのtfを読んでいるようにすべきなので可能な限り長く生存期間を取りましょう。

43. TFアンチパターン3 NG ros::Time(0)=latestでlookupTransofrom （）ならwaitForTransform()やcanTransform() はいらない ros::Time(0)はsourceとtargetの共通時間のみを見て、問い合わせ時間を決めるので、その間の変換が可能かは気にしません。

44. TFアンチパターン４ NG 時計の合わせてないホスト通しでtfをやりとりする tfは時計を合わせてくれない。マルチホストでtfを使うときはchrony等で事前に合わせる必要がある。特に未来のタイムスタンプを持った/tfを受信するとTransformListenerは問答無用ですべてのキャッシュをリセットする。

45. TFアンチパターン５ NG 座標系のループは作らない tfは木構造しか許さない。ループがあるとTFは解決できなくなってしまう。

46. TFアンチパターン6 NG リスナーをリアルタイムプロセスで使う tfListenerは処理時間や使用メモリ量が予測できないことに注意しよう。

47. 結論 • tfをちゃんと理解して使おう • これから書くソフトはすべてtf2で • 時間の扱いには十分注意

tf,tf2完全理解

Koji Terada

Transcript