ROSで座標を扱う方法(C++限定)(tf, tf2_ros, geometry_msgs)

# まえがき
ROS初心者向けです．
ROS(C++)で座標を扱う方法は主に２種類あると考えています．

- tf2_ros(tf2)ライブラリの型を使う
- geometry_msgsライブラリ(≒ROS package, namespace)の型を使う

この2つについてどのように使うのか、どんな特徴があるのかを紹介します．

## 要約(個人的見解)
- `tf2::Transform`型と`geometry_msgs::TransformStamped`型は別物
- 単一ノード内でのみ扱う場合はtf2_rosが便利
- 別ノードから得られた情報を扱う場合，
    - 座標変換を扱う, または要素(x,y,zなど)を単独で弄る場合
→geometry_msgs系+doTransform()
    - オイラー角の入出力がしたい，またはもうちょっと複雑なことをする場合
→SubscribeしたものをfromMsg()でgeometry_msgs->tf2_rosの変換を行う
    - いろいろ作業したあとpublishする必要がある場合
→toMsg()でtf2_ros->geometry_msgsに戻す

# tf2_ros(tf2)
## 特徴
1. このライブラリに属する型はclassなので多機能である
1. 特定の要素のみを扱うのは煩雑
1. 数学的演算が一行で完結
1. オイラー角での入出力が比較的容易

## tf2::Vector3
`tf2_geometry_msgs/tf2_geometry_msgs.h`をincludeして使う
本体は(ubuntuの場合)`/opt/ros/noetic/include/tf2/LinearMath/Vector3.h`
座標点ないしはベクトルを表すときに使用できる。
要素はx,y,zの3つ。（何故かメンバ変数はm_floats[4]）

### 初期化(コンストラクタ)
``` c++
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Vector3 a; //デフォルト
tf2::Vector3 b(1,2,0); //x,y,zを指定する
tf2::Vector3 c(b); //Vector3をそのままコピーする
```

### 値のセット・読み出し
直接メンバ変数(m_floats)を扱うことはできるが見通しが悪い(意味が分かりづらい)のでメンバ関数を利用する
``` c++
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Vector3 vec_x;

//値のセット
vec_x.setX(0.1);
vec_x.setY(0.2);
vec_x.setZ(0.0);
//または
vec_x.setValue(0.1, 0.2, 0.0);

//値の読み出し
ROS_INFO_STREAM(vec_x.getX()); //Y,Zも同様
ROS_INFO_STREAM(vec_x.x()); //上と同じ
```
### 演算
tf2::Vector3同士の加算・減算：各要素ごとに足す,引く
tf2::Vector3同士の乗算：各要素同士を掛ける
tf2::Vector3とtf2::tf2Scalar(つまりdouble)の剰余：Vector3 aの各要素をdouble b倍する

```c++
tf2::Vector3 a(1, 2, 3);
tf2::Vector3 b(8, 6, 4);
double p = 0.5;

tf2::Vector3 c = a + b; //c(9, 8, 7)
tf2::Vector3 d = a - b; //d(-7, -4, -1)
tf2::Vector3 e = a * b; //e(8, 12, 12)

tf2::Vector3 f = a * p; //f(0.5, 1, 1.5)
tf2::Vector3 g = a / p; //g(2, 4, 6)
```

### 主に使えるメンバ関数
紹介する以外にも様々な機能があります

#### length()
ベクトルの大きさ(ノルム, 長さ)

#### distance(tf2::Vector3& v)
vとの距離を出す

#### normalize() / normalized()
ベクトルの向きを変えずに大きさが1のベクトルにする．
__normalizeは自分自身も書き換えるが，normalizedは変わらない__
__ベクトルの大きさが0の場合エラー落ちするので注意__

```c++
tf2::Vector3 vec_x(2, 0, 0);
double a = 3;

tf2::Vector3 vec_y = a * vec_x.normalized();
ROS_INFO_STREAM(vec_y.x()); //3
ROS_INFO_STREAM(vec_x.x()); //2

vec_x.normalize();
ROS_INFO_STREAM(vec_x.x()); //1
```

## tf2::Quaternion
`tf2_geometry_msgs/tf2_geometry_msgs.h`をincludeして使う
回転を表す
クォータニオン自体の説明は省略

### 初期化(コンストラクタ)
値を直接指定して初期化する方法は不便そう
``` c++
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q; //デフォルト
```

### 値のセット・読み出し
オイラー角から代入する場合はメンバ関数のsetRPY()を，読み出しは`tf2/utils.h`のincludeで使える`tf2::getEulerYPR()`や`tf2::getYaw()`を利用する
``` c++
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q;

double roll, pitch, yaw;

//値のセット
q.setRPY(roll, pitch, yaw); //roll pitch yawをdoubleで指定できる

//値の読み出し

#include <tf2/utils.h>

double roll, pitch, yaw;

tf2::getEulerYPR(q, yaw, pitch, roll); //後ろ3つが参照渡しで値が帰ってくる
ROS_INFO("%f %f %f", yaw, pitch, roll);

ROS_INFO_STREAM(tf2::getYaw(q)); //yawだけ欲しいとき
```
### 演算
tf2::Quaternion同士の乗算：回転の加算みたいなことができる
注１：Quaternionの大きさは基本1が良いので，演算後normalize()を実行しておくと良い
注２：回転の順番は重要，可換ではない

```c++
tf2::Quaternion p(0, 0, M_PI/2);
tf2::Quaternion q(0, 0, M_PI/4);

tf2::Quaternion r = q * p; //pの回転を行ったあとにqの回転を行う
r.normalize();
ROS_INFO_STREAM(tf2::getYaw(r)); //M_PI*3/4程度の値
```

### 主に使えるメンバ関数
#### inverse()
回転方向を逆にする
```c++
tf2::Quaternion p(0, 0, M_PI/2);
tf2::Quaternion q(0, 0, M_PI/4);

tf2::Quaternion r = q.inverse() * p; //pの回転のあと，qの逆回転を行う
r.normalize();
ROS_INFO_STREAM(tf2::getYaw(r)); //M_PI/4程度の値
```

## tf2::Transform
`tf2_geometry_msgs/tf2_geometry_msgs.h`をincludeして使う
要素はtf2::Vector3とtf2::Quaternion
このため，2種類の使い方ができてしまう
1. いわゆる回転行列を表す
1. 姿勢(Pose)を表す

### 初期化(コンストラクタ)
``` c++
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Vector3 pos(0, 0, 0);
tf2::Quaternion quat;
quat.setRPY(0, 0, yaw);

tf2::Transform a; //ただの宣言
tf2::Transform b(quat, pos); //position orientationを指定する
tf2::Transform c(b); //Transformをそのままコピーする
```

### 値のセット・読み出し
``` c++
tf2::Transform A;
tf2::Vector3 vec_x;
tf2::Quaternion q;

// set
A.setOrigin(vec_x);
A.setRotation(q);

// get
tf2::Vector3 y = A.getOrigin();
double x = A.getOrigin().getX(); //直接xを得る
tf2::Quaternion r = A.getRotation();

```

### 演算
よくある「ベクトルxを回転行列Aでyに変換する」
```math
y=Ax
```
をほぼそのまま表現可能
``` c++
tf2::Transform A;
tf2::Vector3 x, y;

y = A * x;
// または
y = A(x);

```
また，
- 回転行列Aで姿勢Bから姿勢Cを演算
- 回転行列Aと回転行列Bで回転行列Cを演算
することも可能
```math
C=AB
```
``` c++
tf2::Transform A, B, C;
C.multi(A, B);
C = A * B;
```
*=なども使える

### 主に使えるメンバ関数
#### inverse()
逆変換が可能
```math
z=A^{-1}y
```
``` c++
tf2::Transform A;
tf2::Vector3 x, y, z;

y = A * x;

z = A.inverse() * y;

if(x == z) //true
```


# [geometry_msgs](http://wiki.ros.org/geometry_msgs)

## 特徴
1. このライブラリに属する型はstructよりなのでメンバ関数はない
1. 要素(メンバ変数)を直接操作可能
1. 数学的演算の定義はない
1. オイラー角の入出力が(直接は)サポートされていない
1. 標準の型(Xxxx)と，それに`std_msgs/Header`を含めたXxxxStamped型が存在
1. **この型でtopicにできる(pub/subできる)**

## よく使う型一覧

- Point(Stamped)
地点を表す
- Pose(Stamped)
姿勢を表す
- PoseArray
PoseのArray型(PointArrayも欲しいなあ)
- Transform(Stamped)
変換行列ないしは座標系同士の関係を表すのに使う
いわゆる「TF」に使用される
- Twist(Stamped)
速度を表す

## 座標変換

geometry_msgsシリーズは特にメンバ関数を持ちませんが，座標変換のツールが提供されています.
座標変換の関数は
``` c++
tf2::doTransform(A in, A out, geometry_msgs::TransformStamped transform);
```
で
- `tf2_geometry_msgs/tf2_geometry_msgs.h`をinclude
- 変換したいgeometry_msgsシリーズの型が属する**座標系`source_frame`から変換したい座標系`target_frame`への変換**行列`geometry_msgs::TransformStamped transform`を用意

で使用可能です．

:::note info
みなさんがきっと参考にしている[tf2 tutorialのListener](http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29
)のページを，「座標系1からみた座標系2の原点の姿勢がほしいんだよね」という感じで見てしまうとlookupTransform()関数の`source_frame`と`target_frame`が逆になりがちです．
ちなみにbroadcastでは「座標系1での座標系2の原点の姿勢」を扱います．
:::
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29

``` c++
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

// 変換行列の用意(例)
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

geometry_msgs::TransformStamped transform;
try {
    transform = tfBuffer.lookupTransform("map", "base_link", ros::Time(0));
    //mapからみたbase_link原点の座標=base_link座標系からmap座標系への変換行列
} catch (tf2::TransformException& ex) {
    ROS_WARN("%s",ex.what());
}

#incliude <tf2_geometry_msgs/tf2_geometry_msgs.h>

//base_link座標系のpointをmap座標系の値に変換
geometry_msgs::point point_from_baselink, point_from_map;
tf2::doTransform(point_from_baselink, point_from_map, transform);

```

変換可能な組み合わせは[ここ](https://docs.ros.org/en/melodic/api/tf2_geometry_msgs/html/c++/namespacetf2.html
)にあるとおりです

https://docs.ros.org/en/melodic/api/tf2_geometry_msgs/html/c++/namespacetf2.html

# tf2<->geometry_msgs間の型変換
外のROSnodeからsubscribeした値をtf2_rosの便利な機能で扱いたいんだけど…という場合には
- fromMsg()
- toMsg()

関数を使います．
扱える型と関数の引数/戻り値はここを参照してください

https://docs.ros.org/en/melodic/api/tf2_geometry_msgs/html/c++/namespacetf2.html

:::note warn
toMsg()は結果が戻り値にくるか，それとも参照渡しした第二引数なのかが型によって揺れているので注意
:::

ちなみにfrom,toを気にしたくないって人に`convert(in, out)`関数がありますが，from,toがサポートしていた型の一部が使えないという問題を抱えています(toMsgの結果の戻り方の揺れが原因かも？)

例えば，とある座標系で原点から点pの方向に二倍距離の位置である点qが欲しい場合
``` c++
#incliude <tf2_geometry_msgs/tf2_geometry_msgs.h>

//publisher, subscriberの宣言まわり(main)は省略
void callback(geometry_msgs::Point sub_msg){
  tf2::transform p, q;
  tf2::fromMsg(sub_msg, p);

  if(p.length() != 0)
  {
    q = 2 * p.normalized();
  }

  geometry_msgs::Point pub_msg;
  tf2::toMsg(q, pub_msg);
  point_pub.publish(pub_msg);
}
```

# まとめ
~~いかがでしたか~~ 書きやすくて，修正しやすくて，見やすいコーディングであればどれでもよいのでは

:::note warn
Pythonにはtf2_rosが存在しません
:::