【ROS】ros::Time::now()ができない話

現象

次のようなノードを書いたときに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】DS4コントローラを振動させる

やりたいこと

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の設定から新規デバイス追加を行います。

よくある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ノードが完成したらそちらも公開しようと思います。論文で大変なので時間があればですが…。

参考

【ROS】launchファイルでcsvファイルが出力されない問題

問題

実験用にデータを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

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

解決法①

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ファイルって意外と融通きかないみたいで悲しい。

ROSのTFとは?簡単に説明(サンプルコード付き)②

前回、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)を向くようにしたのでこんな感じの軌跡ができるはずです。

発信されたTFをRVizで確認。fixed_frameをmapに変えるのを忘れずに!

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

ros::Time::now()を数値にしてcsvに書き込もうとしても同じ値が出る件

はじめに

実験とかのために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

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

ROSのsensor_msgs:PointCloud2データの構造と扱い方

はじめに

研究でデプスセンサを使うことがよくあるのですが、デプスセンサからの送られてくる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]:何個のデータが入っているか(?)

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

data[]のデータ構造

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]:すべての値が正常かどうか

などがあります。

ROSの勉強…

rosbagの便利オプション

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

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

rostopic list

まず最初に記録すべきトピック、記録しなくてもいいトピックを見極めましょう。

record関連

rosbag record /xxx /yyy

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

rosbag record -a

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

play関連

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

bagファイルをリピート再生します。センサデータを使ってシステムのチェックを行いたい時等、何回も再生しなおすのが面倒な時に便利です。ちなみに通常の方法でも同様ですが、再生中にスペースキーを押すとポーズが出来ます。

rosparam set /use_sim_time true
rosbag play test.bag --clock

TFなどが上手く表示されない時に使います。一行目で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にリネームして再生します。同じ名前のトピックがあってエラーが起きるときに使います。

検証関連

rosbag info test.bag

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

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

catkin_make時の便利なコンパイルオプション

はじめに

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とは?簡単に説明(サンプルコード付き)

はじめに

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

TFとは?

TF(transform)とは「ある座標系」と「他の座標系」の位置関係を表すものです。
座標系は部品と部品の位置関係のように変化しないものでもいいですし、部屋のある一点から見たロボットの位置のように常に変化するものでも良いです。
例として次のようなロボットを考えてみます。

車輪ロボットとセンサ

このとき、ロボットの重心位置においた座標系からセンサの座標系に行くには「右に20cm、上に10cm」等と表現できます。これが「TF」です。

TFの特徴

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

TFは「あるもの」から「他のもの」への位置関係を表すものですが、普通ロボットなり自動車なりは沢山の部品で出来ています。それらを1つの基準点(たとえば上の図のロボット座標系)からすべて表すのは大変です。
そこで、TFを数珠つなぎに書くことで、関係を分かりやすくすることが出来ます。例えば先程のロボットなら次のような座標系が作れそうです。

ロボットの座標系

座標系の名前は「frame(フレーム)」と呼びます。「map」フレームは常に動かない座標系です。ロボットなら部屋のある点、自動車なら道路などがこれにあたります。
黄色の線は「あるフレーム」から「他のフレーム」へのTFがある事を示しています。元になるフレームを「parent frame(親フレーム)」または単に「frame」、目的のフレームを「child frame(子フレーム)」と呼びます。

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

繋がっている2つのフレーム間のTFは、ROSが自動で計算してくれます。任意の位置関係をいつでも取得することが可能です。

※逆に、途中で切れていると目的のフレームが表せなくなります

一定時間経つと捨てられる

これは重要な特徴なのですが、TFは一度誰かが発信して一定時間経つと無効になります。ずっと表示させたければすべての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を発信するプログラムを実行する部分です。それぞれの引数の意味は次のとおりです。

引数値の例備考
pkg“tf” (固定)パッケージの名前
type“static_transform_publisher” (固定)プログラムの名前
name“(好きな名前)”実行時の名前
args(例)”0.2 0 0.1 0 0 0 robot sensor 10″x方向、y方向、z方向、ヨー、ピッチ、ロール、親フレーム、子フレーム、発信の間隔[Hz]

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

roslaunch my_package static_tf.launch

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

静的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の書き方>>