目次
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型が無いので注意。
クラス | タイムスタンプなし | タイムスタンプ付き | 内部で保持している値 |
tf | Transform | StampedTransform | 回転行列(3×3)+並進ベクトル(3×1) |
tf2 | Transform | なし(tf2::Stamped<Transform>はあるが、child_frameを記述できない) | 回転行列(3×3)+並進ベクトル(3×1) |
geometry_msgs | Transform | TransformStamped | クォータニオン(x,y,z,w)+並進ベクトル(x,y,z) |
準備(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::StampedTransform | geometry_msgs::TransformStamped |
tf::broadcaster | ○ | ○ |
tf2::broadcaster | × | ○ |
broadcastできる型の一覧現在は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::Transform | tf2::Transform | geometry_msgs::Transform |
---|
xyzとロール、ピッチ、ヨー | ↑ | ↑ | ↑ |
xyzとクォータニオン | ↑ | ↑ | ↑ |
変換行列(4×4) | ↑ | ↑ | ↑ |
TF型以外からTF型を作る場合TF型→他のTF型、逆方向、合成等
| tf::Transform | tf2::Transform | geometry_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();
}
▲準備 ▲broadcaster ▲listener ▲変換一覧