ROS講座15 tf-1:概要

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

#TFの概要
TFはROSの重要なシステムの1つで、ロボットシステム中の3次元座標の管理をしている。今回は具体的なプログラムはおいておいて概要を説明します。tfの特徴として

* 3次元の座標の連鎖関係を木構造で管理している
* 3次元の姿勢をquaternionで管理している
* 時間を管理している

というものがある。
以下簡易化のために2次元での例を交えて解説する。

# 座標管理の連鎖関係
座標は物体の位置を表すことができるが、位置を比較するためには座標の

* 原点の位置
* x軸の方向
* y軸の方向

がそろっていないといけない。
ロボット中のすべての物体の位置の記述が1つの座標系（ワールド座標系：以下/world）で管理できれば良いが現実的に無理があります。
例えば/worldにあるrobot1とrobot2のそれぞれのカメラで認識した物体は、基本的にはカメラから見てどの方向にいるのかと出力される。もしロボットの動作が認識した画像の方向に動くのなら、これをわざわざこの時にロボットは/worldから見てどこにあるのかと変換するのは面倒です。
こんな時には使えるのが座標の連鎖です。例えば以下のような状況の時

* ロボット(/base_link)から見て認識した物体Pが位置(x2、y2)にあった。
* この時ワールド座標(/world)から見てロボットの座標(/base_link)は(x1、y1、θ1)にあった。
![tf連鎖1.jpg](https://qiita-image-store.s3.amazonaws.com/0/254442/9b33ec77-f85b-833f-1f8d-533da7f75930.jpeg)

上の2つの情報をしっかり保存しておけばPがワールド座標(/world)から見たらどこにあるのかを計算から求めることはできる。
このようにtfとは座標の関係が以下のようにツリー状になったものです。

![tf連鎖2.jpg](https://qiita-image-store.s3.amazonaws.com/0/254442/e5138bcb-6016-ae6c-fcac-be2eab36e2ea.jpeg)

#quaternionでの3次元中での姿勢管理
2次空間でしたら上記の説明のように座標間の関係は原点の位置(x、y)と角度θで表されますが、3次元になるとだいぶ話は面倒になります。3次元中での角度の表し方として大きく分けて

* オイラー表記（roll、yaw、pitch）
* quaternion表記(x,y,z,w)

があります。オイラー表記のほうが直感的です。以下の画像のように各軸に対して何度回っているかを表します。

![オイラー.png](https://qiita-image-store.s3.amazonaws.com/0/254442/f04a3a55-9991-364d-0201-b98a344f0a67.png)
(https://devforum.roblox.com/t/take-out-pitch-from-rotation-matrix-while-preserving-yaw-and-roll/95204 より)

しかし以下のような欠点があるためにコンピューター上で演算するのに不都合です。

* 軸を回す順番を変えると結果が変わってしまうこと(X-Y-Zの順に回すものが一般的によく使われる)
* ジンバルロック現象が起きる 
* オイラー表記で回した後さらに回した状態を計算するのが煩雑

このためにQuaternion表記が求められます。詳しく語ると長くなりますが、値を見ても直感的にわからない代わりに、コンピューター上で演算しやすい形式になっています。

#座標の時間管理
上記の説明と同じようにワールド座標(/world)中にロボット座標(/base_link)があって、そのロボットがある時(t=1.0)に物体を認識したとします。また別の時間(t=2.0)に物体を認識したとします。たとえこれらがロボットから見て別の場所にあったとしてもロボット自体が動いているために以下の図のように同じ物体である可能性があります。
このように過去に認識した座標はその時間のtf(座標の関係)を使って計算をしなくてはなりません。これに対応できるようにtfはその座標関係があった時間も一緒に記録することができて、またある程度過去の座標データを内部で保持しています。

![tf連鎖3時間.jpg](https://qiita-image-store.s3.amazonaws.com/0/254442/baebcff6-f11c-654f-4c35-8c83d1dc9afe.jpeg)

# /tfと/tf_static
tf関連のrosを起動している時に`$rostopic list`をする`/tf`と`/tf_static`という2つのトピックがあるのが見えます。このうち/tf_staticは時間変化しないtfをやり取りして、/tfは時間変化するものを扱います。そのために/tf_staticはROSが立ち上がった時に1回だけpublishされます（正確にはlatchedなpublisherになっているので、新しいsubscriberが立つと最初に1回だけsubscribeできます。）。

# Rvizの中のtf

実はRvizにはtfを表示するという項目があり、それをONにするとRviz上に以下の画像のように座標軸が現れます。実はRviz上のモデルはただtfに張り付いているだけで、Rviz上でロボットを動かしているときはtfが動くことによって実現されています。
ちなみに赤がX軸、緑がY軸、青がZ軸です。

![RViz_Mover6.png](https://qiita-image-store.s3.amazonaws.com/0/254442/6e13e0c1-702e-17da-b885-f79496664ffa.png)
https://www.cpr-robots.com/products/ros.html より

# robot_state_publisher
ユーザーの作ったROSノードが直接tfのpub・subをすることもできますが、`/robot_state_publisher`を使うことが一般的です。
`/robot_state_publisher`はまずrobotの構造を表現したurdfファイルがロードされている`/robot_description`というrosparamをロードしてfixedなlinkの関係を`\tf_static`にpublishします。
稼働するjointについては、ユーザーがpublishしたjoint名とその値の組の配列である`/joint_states`をsubscribeしてそれを解釈して`/tf`をにpublishします。

![robot_state_publisher.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/eeac5461-d0ef-096e-944b-a6e7cdd1f026.png)


# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)