LoginSignup
17
10
@nabeya11(おなべ士)

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

Posted at

まえがき

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なので多機能である
  2. 特定の要素のみを扱うのは煩雑
  3. 数学的演算が一行で完結
  4. オイラー角での入出力が比較的容易

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]）

初期化(コンストラクタ)

#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)を扱うことはできるが見通しが悪い(意味が分かりづらい)のでメンバ関数を利用する

#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倍する

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の場合エラー落ちするので注意

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して使う
回転を表す
クォータニオン自体の説明は省略

初期化(コンストラクタ)

値を直接指定して初期化する方法は不便そう

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

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

値のセット・読み出し

オイラー角から代入する場合はメンバ関数のsetRPY()を，読み出しはtf2/utils.hのincludeで使えるtf2::getEulerYPR()やtf2::getYaw()を利用する

#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()を実行しておくと良い
注２：回転の順番は重要，可換ではない

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

回転方向を逆にする

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. いわゆる回転行列を表す
  2. 姿勢(Pose)を表す

初期化(コンストラクタ)

#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をそのままコピーする

値のセット・読み出し

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に変換する」

y=Ax

をほぼそのまま表現可能

tf2::Transform A;
tf2::Vector3 x, y;

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

また，

  • 回転行列Aで姿勢Bから姿勢Cを演算
  • 回転行列Aと回転行列Bで回転行列Cを演算
    することも可能
C=AB

tf2::Transform A, B, C;
C.multi(A, B);
C = A * B;

*=なども使える

主に使えるメンバ関数

inverse()

逆変換が可能

z=A^{-1}y

tf2::Transform A;
tf2::Vector3 x, y, z;

y = A * x;

z = A.inverse() * y;

if(x == z) //true

geometry_msgs

特徴

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

よく使う型一覧

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

座標変換

geometry_msgsシリーズは特にメンバ関数を持ちませんが，座標変換のツールが提供されています.
座標変換の関数は

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を用意

で使用可能です．

みなさんがきっと参考にしているtf2 tutorialのListenerのページを，「座標系1からみた座標系2の原点の姿勢がほしいんだよね」という感じで見てしまうとlookupTransform()関数のsource_frameとtarget_frameが逆になりがちです．
ちなみにbroadcastでは「座標系1での座標系2の原点の姿勢」を扱います．

#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);

変換可能な組み合わせはここにあるとおりです

tf2<->geometry_msgs間の型変換

外のROSnodeからsubscribeした値をtf2_rosの便利な機能で扱いたいんだけど…という場合には

  • fromMsg()
  • toMsg()

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

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

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

例えば，とある座標系で原点から点pの方向に二倍距離の位置である点qが欲しい場合

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

まとめ

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

Pythonにはtf2_rosが存在しません

17
10
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
17
10