【ROS】TFのbroadcast,lookupと型変換まとめ(C++)

目次

• C++におけるTF型
• 準備(CMakeLists.txt, package.xml, ヘッダファイル)
• broadcaster
• listener
• TF、geometry_msgsの変換表

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としてbroadcastするにはタイムスタンプ型でなければならないが、tf2にはStampedTransform型が無いので注意。

準備(CMakeLists.txt, package.xml, ヘッダファイル)

ROSで使うため、CMakeLists.txt, package.xmlに以下の記述が必要。

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  ︙
  tf2
  tf2_ros
  tf2_geometry_msgs
)

︙

catkin_package(
  INCLUDE_DIRS
  LIBRARIES tf_test
  CATKIN_DEPENDS roscpp ... tf2 tf2_ros tf2_geometry_msgs
  #DEPENDS system_lib
)

package.xml

<package format="2">
  <name>tf_test</name>
   ︙
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>

  <depend>roscpp</depend>
  <depend>rospy</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

ヘッダファイル

大体以下があれば大丈夫なはず。

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

TF Broadcaster

TFクラスとTF2クラスのbroadcasterの違い

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

現在はtfクラスを使うことは少ないと思われるので、以降はtf2を例に説明します。

基本のbroadcaster

tf2::broadcasterはgeometry_msgs::TransformStamped型しか発信できないため、tf2::Transform型等を発信したければ一度geometry_msgs::TransformStampedに変換する必要がある。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

void pub_tf(const geometry_msgs::TransformStamped trans_msg){
  // geometry_msgs::TransformStamped型のTFを発信
  static tf2_ros::TransformBroadcaster br;

  br.sendTransform(trans_msg);
}

void pub_tf(const tf2::Transform trans_tf2){
  // tf2::Transform型のTFを発信
  // tf2::Transform型は姿勢しか持っていないので、タイムスタンプやフレーム名を追加する必要がある
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped trans_msg;
  trans_msg.header.stamp = ros::Time::now();
  trans_msg.header.frame_id = "map";
  trans_msg.child_frame_id = "robot";
  trans_msg.transform = tf2::toMsg(trans_tf2);
  br.sendTransform(trans_msg);
}

なお、関数の最初で

static tf2_ros::TransformBroadcaster br;

という部分があるが、恥ずかしながらstaticを付ける効果がわからなかったので調べた。どうやらstaticを付けた変数は関数の実行時に毎回初期化されるのではなく、最初の一回だけ初期化された後は関数を抜けても残り続けるらしい。高速化のためなのか必ず必要なのかわからないが、つけておいたほうが良いと思う。

複数のTFを一度に発信する

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

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

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 listener

TFは現在の値を取得することはできない(ツリー状につながったTFがそれぞれ一定周期で更新されるため、現時点よりもちょっと古い情報しか持っていない)ため、二通りの方法がある。

下記のサイトがとても参考になった。

tf,tf2完全理解

方法① すべてのTFが更新されるまで待ってから取得する(基本)

現時点でのparentからchildまでのTFを取るために、その間のすべてのTFが更新されるのを待って取得する。この方法ではTFの時間的な補間がされるため正確なTFが取得できるが、現時点から少し経ってからしか結果が得られない。

bool get_tf(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame, const float timeout){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // 時刻を記録する
  ros::Time now = ros::Time::now();

  // すべてのTFが補間可能になるまで待つ
  if(!tfBuffer.canTransform(parent_frame, child_frame, now, ros::Duration(timeout))){
    ROS_WARN("Could not lookup transform from %s to %s, in duration %f [sec]", parent_frame.c_str(), child_frame.c_str(), timeout);
    return false;
  }

  // nowの時刻でのtransformを取得する
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, now, ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

方法② とにかく今のTFを取得する

とにかく今登録されている情報を使ってparentからchildまでのTFを取得する。この方法では即座に結果が帰ってくるが、parentからchild間に存在するそれぞれのTFが更新されたタイミングはバラバラなため、時間的にずれたTFをつなぐことになり正確ではない。

bool get_tf_immediate(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // ros::Time(0)＝parentとchildの最新TFが登録された時刻のうち早い方
  // とりあえず今登録されているTFを取得するが、古い情報を使うため現在のTFとは異なる
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, ros::Time(0), ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

TFの型変換

tf以外のもの→TF型

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();
}

▲準備 ▲broadcaster ▲listener ▲変換一覧