第五回ROS勉強会 @名古屋での資料です。 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では現在の時間は解決できません。本質的に過去の情報を取る必要があり
ます。

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

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

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

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

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

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

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