タグ: ROS

現象

次のようなノードを書いたときにros::Time::now().toSec()の値が0になってしまって困りました。

int main(){
ros::init(“test”);
…
ros::Time t_start = ros::Time::now();
ROS_INFO(“%lf”, t_start.toSec());
…
return 0;
}

ちなみにこの現象はrosparam set use_sim_time trueをしている時だけで、しかもプログラムの後半で使ったときは普通にコンピュータ時間が出てきました。うーん、謎。

解決

原因はよくわからなかったけど、解決策としてはROS_INFOの前に

ros::Duration delay(1.0);
delay.sleep();

のように待ち時間を設けるか、

while( t_start.toSec() <= 0 ) t_start = ros::Time::now();

のように値が得られるまで待つかすることでちゃんと値が出てきました。
おそらくsim_timeを使う(=bagファイルの時間を使う)ときはクロックが遅くなる？のかな。ノードが起動してあまりにも早くros::Time::now()を呼びすぎると失敗するみたいです。

やりたいこと

ROSを使うとプレステのコントローラ「DualShock4」を使ってロボットの操作が簡単にできます。しかし、コントローラの操作をロボットに伝える事例はたくさんあっても、コントローラを振動させる（フォースフィードバック）やり方でハマったのでご紹介します。

王道は「ds4drv」と「joy_node」を使う。でも…

DS4コントローラとROSの連携で最も一般的なのはコントローラ用のドライバ「ds4drv」とROSのjoyパッケージのノードである「joy_node」を使うやり方です。（こちらのサイトを参考にしました）
ROSでPS4のコントローラ(DualShock4)を使う方法

ところが、振動機能を試してみると失敗します。
以下は失敗事例です。
振動のテストにはfftestコマンドを使います。まずはds4drvを起動して、コントローラとペアリング

$ sudo ds4drv 
[info][controller 1] Created devices /dev/input/js0 (joystick) /dev/input/event11 (evdev) 
[info][bluetooth] Scanning for devices
[info][bluetooth] Found device DC:0C:2D:5C:86:2B
[info][controller 1] Connected to Bluetooth Controller (DC:0C:2D:5C:86:2B)
[info][bluetooth] Scanning for devices
[info][controller 1] Battery: 37%
[warning][controller 1] Signal strength is low (31 reports/s)

一番上の行にあるevdevのイベント番号を見ます。今回は/dev/input/event11となっています。続いてfftestを実行します。

$ fftest /dev/input/event11
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event11 opened
…
（中略）
…

Uploading effect #0 (Periodic sinusoidal) ... Error:: Function not implemented
Uploading effect #1 (Constant) ... Error: Function not implemented
Uploading effect #2 (Spring) ... Error: Function not implemented
Uploading effect #3 (Damper) ... Error: Function not implemented
Uploading effect #4 (Strong rumble, with heavy motor) ... Error: Function not implemented
Uploading effect #5 (Weak rumble, with light motor) ... Error: Function not implemented
Enter effect number, -1 to exit

Uploading effectで始まる行ですべてError: Function not implementedが出てしまっています。この状態で0~5の数字キーを押しても全く振動してくれません。

確認すべきこと

カーネルのバージョン

まず第一に、linuxカーネルのバージョンを確認しましょう。linuxカーネルとはOSの中核を担うプログラムのことです。ds4の振動機能はlinuxではカーネル4.10以上でしかサポートされていないので、それ未満の場合は更新する必要があります。

$ uname -r 
4.9.0-43-generic

linuxカーネルはapt-get upgrade等では自動更新されないので、手動で更新する必要があります。といってもapt-getを使って簡単に更新できます。(※バージョンによっては更新後にエラーが起こりLinuxの起動に問題が出ることもあるので注意が必要ですが、最悪CUIでログインできれば元のバージョンに戻すとは可能です。)
カーネルのアップデートは以下コマンドでできます。

sudo apt-get install linux-generic-hwe-16.04

ds4drvを使わない

前述の通りds4drvは振動機能に対応していません。Linuxカーネル4.10以降ではds4drvを使わなくてもBluetoothでPCとペアリングが可能です。ペアリングの方法は、ds4drvを使う場合と同じくPSボタンとshareボタンを5秒くらい長押しし、ds4の青いランプがチカチカッと光るようになったら、PCのBluetoothの設定から新規デバイス追加を行います。

ペアリングが完了したら何もしなくてもds4drvで接続したのと同じ状態になっています。ROSを使う際もそのままrosrun joy joy_nodeを実行すればジョイスティックの値を見ることが可能です。

この状態でもう一度fftestによる振動テストを実行してみましょう。

$ fftest /dev/input/event19
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event19 opened
…
（中略）
…

Setting master gain to 75% ... OK
Uploading effect #0 (Periodic sinusoidal) ... OK (id 0)
Uploading effect #1 (Constant) ... Error: Invalid argument
Uploading effect #2 (Spring) ... Error: Invalid argument
Uploading effect #3 (Damper) ... Error: Invalid argument
Uploading effect #4 (Strong rumble, with heavy motor) ... OK (id 1)
Uploading effect #5 (Weak rumble, with light motor) ... OK (id 2)
Enter effect number, -1 to exit
0
Now Playing: Sine vibration
Enter effect number, -1 to exit

OKとなっている、#0、#4、#5が使えます。数字を入力するとコントローラが振動します。やったね！。

pythonプログラムから振動

kharlashkin氏のpythonプログラムです。いろんな強さ＆時間で振動させることができます。python2系で動作します。python3系で動かすときはself.ff_joy = open(file, "r+")の部分をself.ff_joy = open(file, "r+b")に変えるといいみたいです。他のプログラムやROSから使うときはpythonで振動させられるのは便利ですね。

import fcntl, struct, array, time

EVIOCRMFF = 0x40044581
EVIOCSFF = 0x40304580
LOG_CLASS_ON = False
TIME_DELTA = 250

class Vibrate:

    def __init__(self, file):
        self.ff_joy = open(file, "r+")

    def close(self):
        self.ff_joy.close()

    def new_effect(self, strong, weak, length):
        effect = struct.pack('HhHHHHHxHH', 0x50, -1, 0, 0, 0, length, 0, int(strong * 0xFFFF), int(weak * 0xFFFF))
        a = array.array('h', effect)
        fcntl.ioctl(self.ff_joy, EVIOCSFF, a, True)
        return a[1]
        id = a[1]
        return (ev_play, ev_stop)

    def play_efect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_play = ''
            for i in id:
                ev_play = ev_play + struct.pack('LLHHi', 0, 0, 0x15, i, 1)
        else:
            ev_play = struct.pack('LLHHi', 0, 0, 0x15, id, 1)
        self.ff_joy.write(ev_play)
        self.ff_joy.flush()

    def stop_effect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_stop = ''
            for i in id:
                ev_stop = ev_stop + struct.pack('LLHHi', 0, 0, 0x15, i, 0)
        else:
            ev_stop = struct.pack('LLHHi', 0, 0, 0x15, id, 0)
        self.ff_joy.write(ev_stop)
        self.ff_joy.flush()

    def forget_effect(self, id):
        if type(id) == tuple or type(id) == list:
            for i in id:
                fcntl.ioctl(self.ff_joy, EVIOCRMFF, i)
        else:
            fcntl.ioctl(self.ff_joy, EVIOCRMFF, id)

f = Vibrate("/dev/input/event14")
p = f.new_effect(1.0, 1.0, TIME_DELTA )
f.play_efect((p))
time.sleep(TIME_DELTA / 1000.0)
f.stop_effect((p))
f.forget_effect((p))

https://github.com/chrippa/ds4drv/issues/91から引用

今回は以上です。今後はこれを使って危険をユーザに知らせるアラートシステムを作っていきたいと思っています。振動させるROSノードが完成したらそちらも公開しようと思います。論文で大変なので時間があればですが…。

参考

問題

実験用にデータをcsvファイル出力するROSノードを作ったのに、単体(rosrun)ではファイルが作られるのにroslaunchでは作られない。

#これは作られる
rosrun imu imu_node

#これだと作られない
roslaunch imu imu.launch

<?xml version="1.0"?>
<launch>

  <node pkg="imu" type="imu_node" name="imu_node" />

</launch>

ちなみにROSのバージョンはKineticだが、全バージョン共通だと思う。たぶん。

原因と解決法

原因はlaunchファイルのオプションにあった。デフォルトではノードの実行場所は$ROS_HOMEになっていて、これはデフォルトで「~/.ros」になっている。ここを見てみるとちゃんと作られているようだ。

aaa@bbb:~$ ls .ros/
imu.csv                             rospack_cache_10391301543023691066
log                                 rospack_cache_11134725904490598093
roscore-11311.pid                   rospack_cache_15154294043039909020
rosdep                              rospack_cache_16685232255138339616
rospack_cache_04149954776256916034

隠しフォルダになっていると面倒なので、保存場所を変えたいと思って調べてみた。解決方法は３つ。

解決法①

launchファイルにオプションを追加して保存場所を変える。

<?xml version="1.0"?>
<launch>

  <node pkg="imu" type="imu_node" name="imu_node" cwd="node"/>

</launch>

nodeタグの中にオプション「cwd=”node”」を追加すると保存場所がプログラムの場所になる。つまり「~/catkin_ws/devel/lib/imu」になる。余計にわかりづらい。

解決法②

環境変数「ROS_HOME」を変える。デフォルトでは「~/.ros」になっているのを次のようにすると変えることができる。

export ROS_HOME=/home/aaa

そのまま同じターミナルでlaunchファイルを実行すると指定したフォルダにcsvファイルが作られる。しかし問題点が2つあり、ひとつはターミナルを起動する度に毎回上のコマンドを打たなければならないこと（.bashrcに上記を追記すれば大丈夫だけど…）。もうひとつは指定したパスにログファイルも作られてしまうことだ。ホームディレクトリとかにするとlogフォルダが出来ているとなんとなく嫌な感じがする。

解決法③

調べたところ、上2つの方法しか見当たらなかった…。やっぱり一番良いのはノードのソースファイル自体で保存場所をフルパスで指定すること。今までは

std::ofstream fout("imu.csv");

のように書いていたところを

std::ofstream fout("/home/aaa/imu.csv");

にすることで事無きを得た。
launchファイルって意外と融通きかないみたいで悲しい。

前回、2つの座標系間の位置関係が変化しない場合のTFの発信をしました。今回は常に位置関係が変化する時のTFについて説明します。

動的TF

ロボットを扱っていると、常に位置関係が変化する2つの座標系を表したい時がよくあります。例えば部屋から見たロボットの位置だったり、アームロボットの手先の位置を表すのにもTFを使います。今回は変化するTFをどう発信するか説明します。

といっても基本的な考え方は静的TFの時と同じです。まずはサンプルプログラムを載せます。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main (int argc, char** argv){
  ros::init(argc, argv, "tf_publisher");
  ros::NodeHandle nh;
  ros::Rate loop_rate(10);

  tf::TransformBroadcaster odom_broadcaster;

  double t = 0;

  while(ros::ok()){
    ros::spinOnce();

    geometry_msgs::TransformStamped odom_trans;

    //現在時刻、親フレーム、子フレームの指定
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";

    //親フレームからみた子フレームの位置
    odom_trans.transform.translation.x = t;
    odom_trans.transform.translation.y = sin(3.14*t);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

    //TFを一回発信
    odom_broadcaster.sendTransform(odom_trans);

    t += 0.01;

    loop_rate.sleep();
  }
}

順番に見ていくと、まず最初にtransform_broadcasterをインクルードします。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

続いてmain関数の最初の4行目でgeometry_msgs::TransformStamped型の変数を作っています。そんでそのヘッダ部分に現在時刻と親フレーム、子フレームの情報を入れます。

geometry_msgs::TransformStamped odom_trans;

//現在時刻、親フレーム、子フレームの指定
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = "base_link";

そのあとが今回の本題、TFの座標を入れる部分です。x,y,zと回転を入れます。回転はクオタニオン(四元数)で表現します。クオタニオン表現では3次元ベクトルとそのベクトル周りの回転量で姿勢を表しますが、2次元平面だけで考える場合はヨー角からクオタニオンを作ってくれる便利な関数があるので非常に楽です。

//親フレームからみた子フレームの位置
odom_trans.transform.translation.x = t;
odom_trans.transform.translation.y = sin(3.14*t);
odom_trans.transform.translation.z = 0.0;

//ヨー角からクオタニオンを作ってrotationに入れる
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

TFの準備ができたところで次の関数で発信します。

//TFを一回発信
odom_broadcaster.sendTransform(odom_trans);

これを一定時間ごとに繰り返すだけで、動的TFの発信は終わりです。
今回はsin曲線を描きつつ進行方向（＝微分方向＝cos）を向くようにしたのでこんな感じの軌跡ができるはずです。

ROSを最初から勉強したい方はこちら

はじめに

実験とかのためにROSトピックの値をcsvファイルに出力したいことが良くあるんですが、ROSの時刻を出力するのにはまったので解決までのメモです。

問題点

ROSの時間を数値（double）に変換するには

double t = ros::Time::now().toSec();

でOK。そんでcsvファイルに出力する

#include <ros/ros.h>
#include <fstream>

int main(int argc, char** argv){
  ros::init(argc, argv, "record_to_csv");
  ros::NodeHandle nh;
  ros::Rate loop_rate(20);
  
  std::ofstream fout("data.csv");
  fout << "Time[s]" << std::endl;

  while(ros::ok()){

    double t = ros::Time::now().toSec();
    fout << t << std::endl;

    loop_rate.sleep();
  }
}

と思ったら、全部同じ値になってしまった！

Time
0
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
...

慌てて色々試したところ、ROS_ERRORではちゃんと表示される模様。

ROS_INFO("%lf",ros::Time::now().toSec());

[ INFO] [1530608049.263032619]: 0.000000
[ INFO] [1530608049.566322356, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.566409625, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.617069613, 1529991633.667305116]: 1529991633.667305
[ INFO] [1530608049.667780029, 1529991633.717981430]: 1529991633.717981
[ INFO] [1530608049.718522891, 1529991633.768572181]: 1529991633.768572

ところがstd::coutでは出力されない。

std::cout << ros::Time::now().toSec() << std::endl;

0
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09

解決

表示がROS_INFOと違うので疑っていたが、やはり桁数の問題だった。ファイル書き込みではデフォルトで6桁以降は切り捨てられるようだ。ROS関係のほうを疑ったり、ros::Time(0)を試してみたりしたけど全然だめで、ros::Time::now().secとros::Time::now().nsecを読んで（どちらもuint型）合計するとかやってしまった…。
std::coutもfoutも次のようにすれば桁数の設定ができる。

fout << std::setprecision(20) << ros::Time::now().toSec() << std::endl;

0
1529991603.1314094067
1529991603.1314094067
1529991603.1819009781
1529991603.2322974205
1529991603.2829723358
1529991603.3336381912
1529991603.3842408657

プログラミング上手い人にとっては常識なのかも知れないけど、そんなこともあるのか。勉強になりました。

はじめに

研究でデプスセンサを使うことがよくあるのですが、デプスセンサからの送られてくるsensor_msgs::PointCloud2形式のデータの扱いについて困った事がありました。例えばあるxy座標でのz座標を取り出したり、ある範囲のデータだけ取り出したりするのが意外と難しかったので、つまずいたことをまとめておきます。

データ形式

ROSの公式ドキュメントを見ればデータ形式そのものはわかるのですが、意味を理解するのにちょっと時間がかかりました。

まず、ポイントクラウドのデータ本体は「data」にあります。これは8ビット(0~255)の数字の列になっていますが、このデータの読み方を定義しているのが「fields」です。ROSのトピックを見てみると、次のようなデータになっていることがわかります。

rostopic echo /point_cloud/fields
- 
  name: "x"
  offset: 0
  datatype: 7
  count: 1
- 
  name: "y"
  offset: 4
  datatype: 7
  count: 1
- 
  name: "z"
  offset: 8
  datatype: 7
  count: 1
--

fieldsは配列になっていて、各データの見方を表しています。
各fieldの見方について説明すると、

• [name]：値の名前
• [offset]：先頭から何番目に始まっているか
• [datatype]：データ型
• [count]：何個のデータが入っているか（？）

を表します。つまりデータの入れ方は次のようになります。

point_stepは1つの点を何個のデータで表すか、で今回は12となります。
さて、xyz各データの読み方ですが、これはfieldのdatatypeで読むことになります。ドキュメントによると今回のデータ型「7」はfloat32を意味するとのことなので、この4バイトをfloat型で読み込めばいいということになります。(4バイトをfloatにする方法はネットに資料が沢山ありますので割愛します。)

data[]とfields[]以外のデータは主にポイントクラウドの大きさなどを表す情報で、

• [height]：高さ
• [width]：幅
• [is_bigendian]：datatypeがビッグエンディアンかどうか
• [point_step]：1つの点を何個のデータで表すか
• [row_step]：1行を何個のデータで表すか
• [is_dense]：すべての値が正常かどうか

などがあります。

実験時のセンサデータとかをすべて記録しておけるrosbagはROSの便利機能の中でもかなり有用な機能です。センサが手元に無くても記録したデータを再生することで、システムがちゃんと動作するか確認することが出来ます。
一方でrosbagには独自の使い方があったりするので、ここで一度出来ること&出来ないことを確認してみたいと思います。

事前に今流れているトピックを確認する

rostopic list

#例
rostopic list | grep cmd #「cmd」を含むものだけ表示

まず最初に記録すべきトピック、記録しなくてもいいトピックを見極めましょう。grepによる検索と合わせて使うことも多いです

record関連

特定のトピックだけrecord

rosbag record (トピック名) (トピック名) ...

#例
rosbag record /joy /cmd_vel
rosbag record -O input_data /joy #「input_data.bag」ファイルに「/joy」トピックを記録

特定のトピックだけを狙って記録する方法です。すべてのデータを記録するとファイルサイズがGB単位になって大変な時はこのやり方もありです。
しかし、あとになってこのデータ取っておけば良かった、となること多しなので慎重に決めましょう。

すべてのトピックをrecord

rosbag record -a

すべてのトピックを記録する方法です。後で完全にシミュレート出来ますが、ファイルサイズが膨大になります。小さなシステムならむしろおすすめです。再生したときにTFが上手く表示されない時は、下のplay –clockを使いましょう。

正規表現にマッチするトピックだけrecord

rosbag record -e "(正規表現)"

#例

rosbag record -e "/MyRobot/.*" #「/MyRobot/」で始まるトピックのみ記録する
rosbag record -e ".*(joy|lidar|imu).*"  #「joy」「lidar」「imu」のいずれかを含むトピックのみ記録する

〇〇で始まるトピック等、一部のトピックだけ記録したいときや、センサからのメッセージだけ記録したいときによく使います。

正規表現にマッチするトピックだけ除外する

rosbag record -x "(正規表現)"

#例
rosbag record -a -x ".*robot.*" #「robot」を含むトピックを除外する
rosbag record -a -x ".*(image|velodyne).*" #「image」または「velodyne」を含むトピックを除外する

正規表現にマッチしたトピックだけを除外することができます。必ず-aオプションか-eオプションと共に使う必要があります。データが大きすぎてrecordするとbagファイルの容量が大きくなってしまうときに有効です。

play関連

ループ再生

rosbag play test.bag -l
# または
rosbag play test.bag --loop

bagファイルをリピート再生します。センサデータを使ってシステムのチェックを行いたい時等、何回も再生しなおすのが面倒な時に便利です。

ちなみに通常の方法でも同様ですが、再生中にスペースキーを押すとポーズが出来ます。さらに、ポーズ中に「s」キーを押すと0.1秒ずつ再生をすすめることができます。

bagファイルのクロックを使用

rosparam set /use_sim_time true #roscoreから出力されるクロックを停止
rosbag play test.bag --clock #bagファイルのクロックを出力

通常bagファイルを再生するときはroscoreのクロック(現在の時間)を使用しますが、時間が重要な場合(TFなど)はbagファイル記録時の時間を使うほうが良いです。一行目でroscoreのクロックを無効にし、二行目でバグファイルの時間を基準の時間として再生します。

倍速・スロー再生

rosbag play -r 0.5 test.bag

データを0.5倍速で再生します。データをゆっくり見たいときに。

途中から再生

rosbag play -s 45 -u 10 test.bag

45秒からスタートして10秒間再生します。一部だけ集中して見たい時や途中から見たいときに使います。-rや-lと合わせて使うことも多いです。

トピック名をリネームして再生

rosbag play test.bag tf:=tf_old

tfという名前のトピックをtf_oldにリネームして再生します。同じ名前のトピックがあってエラーが起きるときに使います。

検証関連

bagファイルの情報を表示

rosbag info test.bag

バグファイルにどんなトピックが記録されているか等の情報を見ることが出来ます。

ROS初心者の方はまずはこちらの本がオススメです。

はじめに

ROSプログラムをコンパイルする時はcatkin_makeを使っているけれど、仕組みはいまいち分かっていない。（そもそもmakeって何かも良くわかっていない）

そんなあなたも取り敢えず試せる便利なコンパイルオプションがこちら。

msg.hが無いと言われる他

ちゃんとCMakelist.txtに書いてるのに作られてない…

これはおそらくメッセージを作るより前にコンパイルエラーで止まってるのが原因。次のオプションで、エラーが起きたプログラムはとりあえず無視して最後までコンパイルできる。

catkin_make -i

もう一度コンパイルするとエラー出ない。

warningめっちゃ出るんだけど…

前はコンパイル出来たやつがあるときからめっちゃwarning出ることがある。またはerror[warning]みたいになって止まる。

そんなとき、エラーを全無視するオプションがこちら。

catkin_make --cmake-args -DCMAKE_CXX_FLAGS="-w"

エラーを放置することになるのであんまりよくないかも。

パッケージを除外する

他の人が作ったパッケージは動かないことが往々にしてある。その人が帰ってくるまで待つとして、その間そのパッケージは無視してコンパイルしたい時は

catkin_make -DCATKIN_BLACKLIST_PACKAGES="package1;package2"

はじめに

ROSの便利機能にして基本機能TF（Transform、トランスフォーム）について、出来るだけ難しい言葉を使わずに説明していきたいと思います。

static transformの書き方

TFとは？

TF（transform）とは「ある座標系」と「他の座標系」の位置関係を表すものです。

座標系とは基準点のことで、例えば部屋の基準点なら部屋の隅、ロボットの基準点なら重心、センサの基準点ならセンサ底面などです。

例として次のような車輪ロボットを考えてみます。

このとき、ロボットの重心位置においたbase_link座標系からセンサのlaser座標系に行くには「x方向に20cm、z方向に10cm」と表現できます^※。これが「TF」です。

※ 正確には、2つの座標系の関係を3次元で表すには、位置(x,y,z)と姿勢(ロール、ピッチ、ヨー)の6つのパラメータが必要です。また、姿勢を4つのパラメータで表す方法(4元数、クォータニオン)もあります。

TFの特徴

複数のTFをつなげることが出来る

TFは2つの座標系の位置関係を表すものですが、普通ロボットや自動車は沢山の部品で出来ています。それらを1つの基準点（たとえば上の図のロボット座標系）からすべて表すのは大変です。
したがってTFは数珠つなぎに記述することが一般的です。例えば先程のロボットの例で、車体の上面の中心にtop_center座標系を作るとレーザセンサやカメラまでの位置を計測しやすくなるため便利になります。base_link座標系→top_center座標系→laser、camera座標系と数珠つなぎになっているのが分かります。

ROSでは座標系のことを「frame(フレーム)」と呼びます。rvizに表示される黄色の線は「あるフレーム」から「他のフレーム」へのTF、つまり一方のフレームから見たもう一方のフレームの位置と姿勢がROSに登録されている事を示しています。元になるフレームを「parent frame（親フレーム）」または単に「frame」、目的のフレームを「child frame（子フレーム）」と呼びます。

2フレーム間のTFをROSに登録する

ROSで利用するすべてのTFはROSシステムに登録されます。親フレーム名、子フレーム名、位置関係、タイムスタンプ等の情報をメッセージにまとめてROSに送信します。またTFには有効期限があるため、1度登録したら終わりではなく10ms毎など定期的に登録し直す必要があります。(例外として、有効期限が無いstatic_transformというものもあります)

つながっているTF同士の位置関係は自動計算される

すべてのフレーム間の位置関係をROSに登録しておくことで、任意の2フレーム間のTFをROSが自動計算してくれます。例えば上の例でbase_linkフレームとcameraフレームは直接繋がっていませんが、これを自動計算できる仕組みがROSに備わっています。

逆に、TFが途中で切れていると、2フレーム間のTFが取得できず、エラーになります。

ROSに登録されてから一定時間経つと無効になる

TFは一度誰かが発信してROSシステムに登録された後、一定時間経つと無効になります。ずっと表示させたければすべてのTFを定期的に再発信する必要があります。

静的TFと動的TF

TFには例えばロボットのセンサのように位置関係が永久に変わらないものと、部屋からみたロボットの位置のように変わり続けるものがあります。前者を静的TF、後者を単にTFと言います。

TFを発信する

静的TFを発信するコードを書いてみます。最も簡単なのは次のようなlaunchファイルを作ることです。

<launch>

  <node pkg="tf" type="static_transform_publisher" name="robot_to_sensor" args="0.2 0 0.1 0 0 0 robot sensor 10" />

</launch>

「<node」 から 「/>」までがTFを発信するプログラムを実行する部分です。それぞれの引数の意味は次のとおりです。

nameが同じプログラムを同時に作るとエラーになります。これを自分のパッケージ→lanchフォルダの中に入れて実行すると、argsで指定したTFが発信されます。

roslaunch my_package static_tf.launch

発信されているTFをrvizで見てみましょう。rvizを起動して、左下部あたりにある「Add」をクリック、「By display type」から「TF」を追加して下さい。

2つの座標軸が表示されていれば成功です！
rvizの左側の「Fixed Frame」を「robot」や「sensor」に切り替えることで、どのフレームを中心に据えるか選ぶ事が出来ます。lanchファイルで複数のプログラムを起動すれば、TFの数珠つなぎも簡単に実現できます。

<launch>

  <node pkg="tf" type="static_transform_publisher" name="robot_to_sensor" args="0.2 0 0.1 0 0 0 robot sensor 10" />
　<node pkg="tf" type="static_transform_publisher" name="map_to_robot" args="1.0 0.5 0 0 0 0 map robot 10" />

</launch>

しかし、これだと常に同じTFしか発信できません。
次回は常に変化する位置関係の発信方法をご紹介したいと思います。

ROS初心者の方はまずはこちらの本がオススメです。

動的TFの書き方＞＞