読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

世界で一番簡単なtfの使い方

Robot c++ ROS


はじめに


ROSを使う理由の一つに、

座標系管理&座標変換ライブラリである

tfが提供されていることがあると思います。


自律ロボットを開発する時に、

座標変換は重要な問題です。

ロボットに搭載されたレーザーセンサを使って、

周辺環境の地図を作成する時には、

レーザーの点群をグローバル座標系に変換しなければなりません。


また、ロボットの移動中心に

センサが取り付けられている時には良いのですが、

基本的にそのようなセンサは移動中心とは

別の位置に取り付けられているので、

その部分も座標変換する必要があります。


またそのセンサが時間と共に移動する場合、

(二次元のスキャンレーザーを回転させて

周辺環境の三次元形状を取得することは

一般的な方法です。)

座標変換の問題はより複雑になるでしょう。


また、この座標変換の問題は

単純に計算や実装が面倒なだけでなく、

バグが入りやすい部分でもあります。


オイラー角を使って座標変換を行う場合、

各軸の回転の順番が違うと、

意図しない回転を行ってしまう場合もありますし、

角度の原点や、正負の方向が一つ違うだけで、

全く違った場所に座標変換してしまいます。

加えて、そのようなバグは非常にデバックが難しく、

面倒なものです。


ROSが提供している座標変換ライブラリtf(transformの略だと思います)は,

このような問題を解決してくれるライブラリです。

自分はこのtfをソフトウェア開発で使用し始めてから、

書かなければならないコードが

おおよそ四分の一減ったと実感しています。


tfの概念を学ぶには下記の資料がおすすめです。



しかし、上記の資料でも言われていますが、

このtfには一つ問題があると思っていました。

それは使い方のマニュアルがわかりにくいということです。

ROSのwikiのページもあるのですが、

tf - ROS Wiki

非常にわかりにくく、

例えば、ある座標系の点群を

ある座標系に変換したい場合など,

非常に簡単な問題でありながら,

実際にどのようにすればいいのかがわかりませんでした。


結局、いろいろな資料を読み込んで理解したのですが、

これでは初学者にとっては不便なので、

今回はできるだけ簡単に,

tfによる座標変換の方法を説明したいと思います。


まず、今回したいことは

下記の図のように

/robotという座標系で取得されたレーザーの点群情報を

/globalという座標系に変換したいとします。


まず始めに、tfには大きく分けて二つのクラスがあります。

一つ目は座標系の登録を行う、Broadcaster

二つ目は登録された座標系を使用して座標変換などを行えるListenterです。


tfを使って座標変換を行いたい場合、

まず始めに各座標系の登録をしなければなりません。

今回の場合は/globalと/robotの座標系を登録します。

具体的にはロボットの位置を計算するノード内で

下記のように座標系を登録すればOKです。

#include "tf/transform_broadcaster.h"
 
//TF Broadcasterの実体化
tf::TransformBroadcaster glabal_robot_broadcaster;
 
//Robot位置と姿勢(x,y,yaw)の取得
double x=GetRobotPositionX();
double y=GetRobotPositionY();
double yaw=GetRobotPositionYaw();
 
//yawのデータからクォータニオンを作成
geometry_msgs::Quaternion robot_quat=tf::createQuaternionMsgFromYaw(yaw);
 
//robot座標系の元となるロボットの位置姿勢情報格納用変数の作成
geometry_msgs::TransformStamped robotState;
 
//現在の時間の格納
robotState.header.stamp = ros::Time::now();
 
//座標系globalとrobotの指定
robotState.header.frame_id = "global";
robotState.child_frame_id  = "robot";
 
//global座標系からみたrobot座標系の原点位置と方向の格納
robotState.transform.translation.x = x;
robotState.transform.translation.y = y;
robotState.transform.translation.z = z;
robotState.transform.rotation = robot_quat;
 
//tf情報をbroadcast(座標系の設定)
robotState_broadcaster.sendTransform(robotState);


これで,

先ほどの図のように,global座標系から見た

ロボットの位置と姿勢を中心としたrobot座標系という

2つの座標系を登録できます.


ちなみに,

tfではいずれかのノードが座標系を設定すれば、

他のノードは通常のトピックを読むように

座標系の情報を取得したり、

座標変換を行うことができます。


続いて、

robot座標系で得られるレーザの点群のデータを

global座標系に変換したい場合,

座標変換を行いたいノード内で、

下記のようにします。

#include "tf/transform_listener.h"
 
tf::TransformListener tflistener;
 
//それぞれの座標系におけるレーザの点群のデータ変数
sensor_msgs::PointCloud laserPointsRobot;
sensor_msgs::PointCloud laserPointsGlobal;
 
//robot座標系におけるレーザ点群の情報の取得
laserPointsRobot=getLaserPoints();
 
//レーザの点群をrobot座標系からglobal座標系に変換
//変換されたデータはlaserPointsGlobalに格納される. 
tflistener.transformPointCloud("robot",laserPointsRobot.header.stamp,laserPointsRobot,"global",laserPointsGlobal);

非常に簡単で便利ですね。

PointCloud以外のデータタイプである、

vector,point,pose

でも同様にそれぞれの座標変換の関数を使うと

座標変換を行うことができます。

tf/Overview/Using Published Transforms - ROS Wiki


このようにtfを使えば、

一度座標系の登録をしてしまえば、

その座標系の登録名を元に座標変換をすることができます。


自律ロボットの開発で

座標変換に悩ませられている人は、

ぜひ一度、ROSとtfを使ってみることをオススメします。


ROSでクオータニオンからロール・ピッチ・ヨー角を取得する方法

nav_msg::Odometryなどの姿勢クオータニオンデータから、

ロール・ピッチ・ヨー角を取得したい時は、下記のような関数を作成し、

使用すると楽です。


関数:

/**
 *  @brief ROSのトピックのクオータニオンの構造体から
 *         Roll,Pitch,Yaw角を取得する関数
 *  @param q トピックのクオータニオン
 *  @param[out] roll [rad]
 *  @param[out] pitch [rad]
 *  @param[out] yaw [rad]
 */
 void GetRPY(const geometry_msgs::Quaternion &q,
  double &roll,double &pitch,double &yaw){
  //bulletのクオータニオンに変換
  btQuaternion btq(q.x,q.y,q.z,q.w);
  tf::Matrix3x3(btq).getRPY(roll, pitch, yaw);
 }

使う時

nav_msgs::Odometry odom=GetData();
double roll,pitch,yaw;
GetRPY(odom.pose.pose.orientation,roll,pitch,yaw);
cout<<roll<<" "<<pitch<<" "<<yaw<<endl;

Bagファイルを再生させた時にtfがoldだと言われた時

1. roscoreを起動

2. 下記のコマンドでROS時間をsimulator時間を使用することをONにする

rosparam set use_sim_time true

3. 下記のコマンドでbagファイルをplayする

(--clockオプションをオンする)

rosbag play --clock hoge.bag

tfを使う時の注意点

前述のslideshareの資料にも書いてありますが、

tfを使う上でよく間違えてしまうことが2つあります。

1. ros::time::now()を使ってLookupTransformしてはいけない。

tfはtf_broadcasterによって登録されたtf間の間の時間しか、座標変換できません。

ですのでros::time::now()で時間指定すると、座標変換できません。

つまり、基本的にtfは過去の時間を指定してLookupTransformする必要があります。


そんな時は下記のリンクの通り、tfが登録されるまで時間を待つようにすると、

きちんと座標変換してくれます。

ja/tf/Tutorials/tf and Time (C++) - ROS Wiki

2. listenerのオブジェクトをローカル変数にしてはいけない

tfではlistenerオブジェクトがtfのデータを受信し、貯めこむ形になるので、

座標変換をしたい度にlistenerオブジェクトを作っていると、

tfのframeのデータが蓄積されずにエラーになることになります。


対策としてはノードに一つlistenerオブジェクトを

ノード起動時に生成し、座標変換時はそのオブジェクトを毎回使うようにします。