位置姿勢計算にtfを使う

ROSには位置姿勢を表現するためにtfという便利な仕組みがあります．
チュートリアルやサンプルを見ていても，すでに発行されている複数のtfからtfを取得したりする例はよく見るのですが，プログラム内の計算でもtfが使えるのでここにまとめます．
ただし，自分はkineticしか使っていないので，その範囲でしか確認していません．

ちなみに，こういう計算の例とかが載っているチュートリアルがあったら教えてください…
Class referenceあれば十分だろ，って姿勢ですかね…

tfとtf2

tfには，tfとtf2が存在し，チュートリアルなんかも未だにtfで書かれているものが多いんですが，tfのwikiをみると

Migration: Since ROS Hydro, tf has been "deprecated" in favor of tf2. tf2 is an iteration on tf providing generally the same feature set more efficiently. As well as adding a few new features.

なんて書かれています．
なので，なにか理由がない限りはtf2を使うことにしましょう．

ただ，面倒なので，このページではこのあともtf2をさしてtfと書くことにします．

tf2の準備

インストール

sudo apt install ros-kinetic-geometry2

geometry2がメタパッケージになっていて，tf2関連をまとめてインストールできます．

実際に使うパッケージ類

上記でインストールすると，次のパッケージ群がインストールされるはずです．

tf2
tf2_bullet
tf2_eigen
tf2_geometry_msgs
tf2_kdl
tf2_msgs
tf2_py
tf2_ros
tf2_sensor_msgs
tf2_tools

この中から必要なものを依存関係に書いてコンパイルします．
基本的にはtf2とtf2_geometry_msgsとtf2_rosがあればいいかと思います．
構成としては，tf2が基本で，それ以外がtf2の拡張パッケージという感じです．
特に各型の変換が必要な場合には，使う型によって依存パッケージを追加します．

計算してみる

メッセージとtfの変換

便利にtfで計算するためには，tfで使われるクラスになっていると楽です．
そのため，まずはROSでやり取りされるメッセージをtfで使われるクラスに変換します．

各種定義は，公式ドキュメントを参照しましょう．
convert，fromMsg, toMsg などが見どころです．
基本的にどの関数もテンプレートとオーバーロードを使いまくって定義されているので，いろいろな型の間で変換が可能です．
(それゆえ何が何に変換できるのか見通しが悪いともいう)
tf2以外のパッケージ(tf2_geometry_msgsとか)にも各変換関数が定義されているので，そちらも要確認です．

メッセージからtfに変換

#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

//geometry_msgs::Transform msg を受け取るコールバックだとして．
tf2::Transform tf;
tf2::convert(msg, tf);    // msg から tf に変換される

tfからメッセージに変換

なんでかこれはconvertがうまくできない．ヒントを渡してもダメ．教えて偉い人．

#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

geometry_msgs::Transform msg;
tf2::Transform tf;
tf2::toMsg(tf, msg);    // tf から msg に変換される

stampedなんちゃらへの変換

tfの各型にはタイムスタンプを持つ機能がないので，タイムスタンプだけ持つ型がその他の位置姿勢等のデータ型をもてる仕組みになっています．

たとえば，タイムスタンプありのtransformは次のように定義できます．

tf2::Stamped<tf2::Transform> stamped_transform;

これを使うと，次のような変換もできます．

geometry_msgs::TransformStamped msg = tf2::toMsg(tf2::Stamped<tf2::Transform>(tf, ros::Time::now(), frame_id));
msg.child_frame_id = child_frame_id;

sendTransformをするときなんかに便利です．が，child_frame_idの設定をし忘れないように．

tfを使った計算

Transform 同士の計算

表現したいものがtfになったら勝ちです．
あとはひたすら計算するだけです．

例えば，2つのtfの姿勢の差をとりたいときはこんな感じです．

tf2::Transform tf1, tf2, tf_delta;
tf_delta = tf2.inverse() * tf1;

tfの差なんてlookupTransformすればいいじゃん，となるかもしれませんが，先述の通り，geometry_msgsから変換したtfでこの計算ができるので，まだtfになっていないもので計算ができます．

quaternion の線形補間

tfのそれぞれの型は，演算子のオーバーロードも多くされているので，スカラーとの計算も楽です．

2つのquaternionを補間する例は次のようにかけます．

double d = 0.1;
tf2::Quaternion q1, q2;  // 入力のQuaternion
tf2::Quaternion q = q1 * d + d2 * (1.0 - d);

Vector3のほうも同様にスカラーと計算できるので，前述のtfの差の位置と姿勢をそれぞれ線形補間する，なんてこともできます．

おまけ

quaternionからオイラー角への変換

ネットを見ていると、tf時代によく使われていたmatrixに変換する方法がよく紹介されていますが、tf2には一発で変換できるツールがあるのでそれを使うと便利です。

double roll, pitch, yaw;
tf2::getEulerYPR(q, yaw, pitch, roll);  // q入力のQuaternion

ここで、入力のQuaternionはconvertで変換できるものであれば入力できます。tf2::Quaternionやgeometry_msgs::quaternionなど。
公式ドキュメントをはこのあたり。

まとめ

ロボットの位置姿勢の計算には必須な同次変換が簡単に表現できるうえ，内部はQuaternionで実装されているので，ジンバルロック問題もありません．
ROSでプログラムを書くときにわざわざ自分で位置姿勢の計算を要素ベースでしなくてはならないことはほぼないでしょう．
2次元の場合でも，tfを使ってやると便利だと思います．

ROSのtf2で位置姿勢を計算する