【ROS】TF、Broadcasterと各種変換まとめ(C++)

目次

C++におけるTF型

ROSで使うTF型は一つではなく、TF,TF2,geometry_msgsの3つのクラスが使われている。TFクラスは現在使用が推奨されていないので、新しいクラスであるTF2を可能な限り使う。

よく使うTFメッセージは以下のようなものがある。

  • tf2::Transform
    変換や逆行列の計算に便利な型
  • tf2::StampedTransform
    tf2::Transformにタイムスタンプがついた型
  • geometry_msgs::Transform
    ROSトピックでよく使われる型
  • geometry_msgs::TransformStamped
    上記にタイムスタンプがついた型

TFの型まとめ

一覧はこのようになっている。TFとして発信するためにはタイムスタンプ型でなければならないが、tf2にはStampedTransform型は無いので注意。

クラスタイムスタンプなしタイムスタンプ付き内部で保持している値
tfTransformStampedTransform回転行列(3×3)+並進ベクトル(3×1)
tf2Transformなし(tf2::Stamped<Transform>はあるが、child_frameを記述できない)回転行列(3×3)+並進ベクトル(3×1)
geometry_msgsTransformTransformStampedクォータニオン(x,y,z,w)+並進ベクトル(x,y,z)

TF Broadcaster

TFを発信する際はタイムスタンプ付きの型(geometry_msgs::TransformStamped等)を使う。各クラスのBroadcasterが発信できる型は以下の通り。

tf::StampedTransformgeometry_msgs::TransformStamped
tf::broadcaster
tf2::broadcaster×
broadcastできる型の一覧

また、発信できる型ならば、std::vectorで複数をまとめて同時に発信することも可能。

void publish_all(){
  // tf2でgeometry_msgsのTFを複数まとめて送信する例
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped tf1, tf2, tf3;
  std::vector<geometry_msgs::TransformStamped> all_tf;

  all_tf.push_back(tf1);
  all_tf.push_back(tf2);
  all_tf.push_back(tf3);

  br.sendTransform(all_tf);
}

変換表

tf以外のもの→TF型

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
TF型以外からTF型を作る場合

TF型→他のTF型、逆方向、合成等

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
tf::Transform
tf2::Transform
geometry_msgs
逆方向のTF(なし)
2つのTFの合成(なし)
2つのTFの差分(なし)
TF型を変換したり、データを取り出す場合

xyzとロール、ピッチ、ヨーからgeometry_msgs

ロール、ピッチ、ヨーから直接geometry_msgs::Quaternionを作る関数は無いので、tf::Quaternionで作ってから代入する。

geometry_msgs::Transform xyz_rpy_to_geometry(double x, double y, double z, double roll, double pitch, double yaw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

xyzとクォータニオンからgeometry_msgs

xyzとクォータニオン(x,y,z,w)が分かっている場合、geometry_msgs::Transformにそのまま代入するだけ。

geometry_msgs::Transform xyz_quat_to_geometry(double x, double y, double z, double qx, double qy, double qz, double qw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  gt.rotation.x = qx;
  gt.rotation.y = qy;
  gt.rotation.z = qz;
  gt.rotation.w = qw;

  return gt;
}

変換行列からgeometry_msgs

あまり使うことは無いかもしれないが、変換行列を並進ベクトル(3次元ベクトル)と回転行列(3×3行列)で表していた場合、geometry_msgsを作るにはこうする。

  geometry_msgs::Transform matrix_to_geometry(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  geometry_msgs::Transform gt;
  gt.translation.x = trans.getX();
  gt.translation.y = trans.getY();
  gt.translation.z = trans.getZ();
  tf2::Quaternion q;
  rot.getRotation(q);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

geometry_msgsからxyzとロール、ピッチ、ヨー

geometry_msgsからロール、ピッチ、ヨーを直接得ることはできないので、geometry_msgs::Quaternion→tf::Quaternion→tf::Matrix3x3に変換してからgetRPY関数で取得する。

std::tuple<double, double, double, double, double, double> geometry_to_xyz_rpy(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;

  double roll, pitch, yaw;
  tf::Quaternion q(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);

}

geometry_msgsからxyzとクォータニオン

geometry_msgsはxyzとクォータニオンを持つ型なので、そのまま取得する。

std::tuple<double, double, double, double, double, double, double> geometry_to_xyz_quat(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;
  double qx = gt.rotation.x;
  double qy = gt.rotation.y;
  double qz = gt.rotation.z;
  double qw = gt.rotation.w;

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

geometry_msgsから変換行列

geometry_msgsから並進ベクトル(3次元)と回転行列(3×3行列)に変換する。

std::tuple<tf2::Matrix3x3, tf2::Vector3> geometry_to_matrix(geometry_msgs::Transform gt){
  tf2::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf2::Matrix3x3 rot(tf2::Quaternion(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w));

  return std::make_tuple(rot, trans);
}

geometry_msgsからtf::Transform

tf::Transform geometry_to_tf(geometry_msgs::Transform gt){
  tf::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf::Quaternion quat(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Transform transform(quat, trans);

  return transform;
}

geometry_msgsからtf2::Transform

tf2では便利なconvert関数が使えるので、シンプルに変換できる。

tf2::Transform geometry_to_tf2(geometry_msgs::Transform gt){
  tf2::Transform tf2;
  tf2::convert(gt, tf2); // geometry_msgs::Transform -> tf2::Transform

  return tf2;
}

geometry_msgsの逆方向TF

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの合成

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの差分

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

xyzとロール、ピッチ、ヨーからtf2::Transform

コンストラクタを使ってtf2::Quaternion q(roll, pitch, yaw)とせず、一旦tf2::Quaternion q;で作ってからsetRPY関数で設定していることに注意。

tf2::Transform xyz_rpy_to_tf2(double x, double y, double z, double roll, double pitch, double yaw){
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf2::Transform tf2(q, tf2::Vector3(x, y, z));

  return tf2;
}

xyzとクォータニオンからtf2::Transform

クォータニオンから作るときはtf2::Quaternionのコンストラクタを使える。

tf2::Transform xyz_quat_to_tf2(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

変換行列からtf2::Transform

これは簡単。

tf2::Transform matrix_to_tf2(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  tf2::Transform tf2(rot, trans);
  return tf2;
}

tf2::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf2::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf2_to_xyz_rpy(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double roll, pitch, yaw;
  tf2::Quaternion q = tf2.getRotation();
  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf2::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

  std::tuple<double, double, double, double, double, double, double> tf2_to_xyz_quat(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf2::Transformから変換行列

とてもシンプル。

std::tuple<tf2::Matrix3x3, tf2::Vector3> tf2_to_matrix(tf2::Transform tf2){
  tf2::Matrix3x3 rot = tf2.getBasis();
  tf2::Vector3 trans = tf2.getOrigin();

  return std::make_tuple(rot, trans);
}

tf2::Transformからtf::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf::Transform tf2_to_tf(tf::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();
  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

tf2::Transformからgeometry_msgs

とてもシンプル。

geometry_msgs::Transform tf2_to_geometry_msgs(tf2::Transform tf2){
  geometry_msgs::Transform gt = tf2::toMsg(tf2);
  return gt;
}

tf2::Transformの逆方向TF

TF2使っててよかった!

tf2::Transform tf2_invert(tf2::Transform tf2){
  return tf2.inverse();
}

tf2::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf2::Transform tf2_add(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_A* tf2_B;
}

tf2::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf2::Transform tf2_sub(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_B * tf2_A.inverse();
}

xyzとロール、ピッチ、ヨーからtf::Transform

tf2::Transformの場合とほぼ同様のやり方でできる。

tf::Transform xyz_rpy_to_tf(double x, double y, double z, double roll, double pitch, double yaw){
  tf::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf::Transform t(q, tf::Vector3(x, y, z));

  return t;
}

xyzとクォータニオンからtf::Transform

クォータニオンから作るときはtf::Quaternionのコンストラクタを使える。

tf::Transform xyz_quat_to_tf(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

変換行列からtf2::Transform

これは簡単。

tf::Transform matrix_to_tf(tf::Matrix3x3 rot, tf::Vector3 trans){
  tf::Transform t(rot, trans);
  return t;
}

tf::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf_to_xyz_rpy(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double roll, pitch, yaw;
  tf::Quaternion q = t.getRotation();
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

std::tuple<double, double, double, double, double, double, double> tf_to_xyz_quat(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf::Transformから変換行列

とてもシンプル。

std::tuple<tf::Matrix3x3, tf::Vector3> tf_to_matrix(tf::Transform t){
  tf::Matrix3x3 rot = t.getBasis();
  tf::Vector3 trans = t.getOrigin();

  return std::make_tuple(rot, trans);
}

tf::Transformからtf2::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf2::Transform tf_to_tf2(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();
  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

tf::Transformからgeometry_msgs

tf2の時と関数名が違うので注意。こういう微妙な違いがあったりするのが怖い。

geometry_msgs::Transform tf_to_geometry_msgs(tf::Transform t){
  geometry_msgs::Transform gt;
  tf::transformTFToMsg(t, gt);
  return gt;
}

tf::Transformの逆方向TF

tf::Transform tf_invert(tf::Transform t){
  return t.inverse();
}

tf::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf::Transform tf_add(tf::Transform t_A, tf::Transform t_B){
  return t_A * t_B;
}

tf::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf::Transform tf_sub(tf::Transform t_A, tf::Transform t_B){
  return t_B * t_A.inverse();
}

▲変換一覧に戻る

コメントを残す

メールアドレスが公開されることはありません。