| Note: このチュートリアルは ROS Tutorials と tf basic tutorials を読み終えたことを想定しています．また，このチュートリアルで用いるコードは robot_setup_tf_tutorial パッケージから入手可能です. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
tf を用いたロボットのセットアップ
Description: このチュートリアルでは tf を使ってロボットをセットアップする方法を学びます．Tutorial Level: BEGINNER
Next Tutorial: tf に対してロボットの状態を publish する方法を学びます： using the robot state publisher on your own robot
Transform の構造
多くの ROS パッケージはロボットの transform tree が tf ソフトウェアライブラリで publish されることを想定しています．概念的には，異なる coordinate frame 間の移動と回転をもとに transform tree がオフセットを定義しています．具体例として一番上にレーザーを1つ備えた簡単な移動ロボットを考えてみましょう．このロボットを考えるにあたって2つの coordinate frames を定義します．1つ目の coordinate frame は基準面の中心点に対応していて，2つ目の coordinate frame は一番上に取り付けられたレーザーの中心点に対応しています．簡単のためにこの2つの coordinate frame に名前をつけましょう．1つ目の coordinate frame を "base_link"，2つ目の coordinate frame を "base_laser"とします（ナビゲーションではロボットの回転中心を "base_link" に取ることが大切です）． frame の命名規則についてはREP 105を参照して下さい．
ここで，レーザー中心点からの距離データを取得していると想定しましょう．つまり "base_link" の coordinate frame 上にデータを取得したということになります．私たちはこのデータを使って移動ロボットが障害物を回避することに役立てたいとします．そうするためには "base_laser" の frame から受け取ったレーザースキャンを "base_link" の frame に変換する方法が必要です．
この関係を定義するために，レーザーは移動ロボットの中心から前に10cm・上に20cm離れたところに付けられていると仮定します．このようにすれば"base_link" の frame を "base_laser" の frame に関連付けるために必要な移動量のオフセットが分かります．具体的に言うと "base_link" の frame 上のデータから "base_laser" の frame 上のデータを得るときは (x: 0.1m, y: 0.0m, z: 0.2m) の移動をさせなくてはならず，その逆のときは (x: -0.1m, y: 0.0m, z: -0.20m) の移動をさせなくてはいけません．
この調整を自分たちでやりくりすることもできます．つまり，必要ならば frame 間の移動量を保存し適用していくということが可能です．しかしこうしていては coordinate frames の数が増えていくにつれてかなり面倒な作業になっていくでしょう．幸運なことにこの作業を自分たちの手でやる必要はありません．tf を使って一旦 "base_link" と "base_laser" の関係を定義し，その2つの coordinate frames 間の移動は tf に任せれば良いのです．
tf を使って "base_link" と "base_laser" の関係性を定義・保存するためには，それらを transform tree に追加する必要があります．概念的には， transform tree の各ノードは coordinate frame に対応し，各エッジ（枝）は親ノードから子ノードへ移動する際に必要な変換に対応しています．どの2つの coordinate frame でも互いに結びつける橋渡しがただ1つしか存在しないことを保証する木構造を tf は使用しており，すべてのエッジ（枝）は親ノードから子ノードに向けられていると想定しています．
簡単に transform tree を作るため，"base_link" coordinate の frame と "base_laser" coordinate の frameの2つを作りましょう．エッジを作るためにはどちらが親ノードでどちらが子ノードであるかを最初に決めることが必要です．この「親」と「子」の区別が大切であることを忘れないで下さい． tf はあらゆる変換は親ノードから子ノードに移動すると想定しているからです．"base_link" の coordinate fram を親に決めます．こうすることで新たに部品やセンサが搭載されたときに， "base_link" の frame を介して "base_laser" の frame と結びつけてあげる事ができます．ここでは "base_link" と "base_laser" を結びつけるエッジ（枝）を表す変換は (x: 0.1m, y: 0.0m, z: 0.2m) になります．このように transform tree を設定してあげることで， "base_link" の frame 上で得たレーザースキャンを "base_laser" の frame 上のものに変換することが tf ライブラリを使うことのように簡単になります． ロボットはこういった技法を駆使して， "base_link" の frame 上で得たレーザースキャンを論理的に考えることで周囲の障害物を安全に避けるプランを立てることができます．
コードの作成
上記の例で tf を概念的には理解してくれたと思います．それでは transform tree のコードを書いてみましょう．この例では ROS に慣れ親しんでいることを想定しています．もし用語や概念が分からなければ ROS Documentation を見てみて下さい．
"base_laser" の frame 上で得た点群を "base_link" の frame に変換することを考えます．最初に transform を publish するノードを作ります．次に ROS を介して transform を listen し，その transform を用いてポイントを変換するためのノードを作ります．では"robot_setup_tf" のようなシンプルな名前のパッケージを作成してください．そのパッケージはroscpp と tf と geometry_msgs に依存しています．
$ cd %TOP_DIR_YOUR_CATKIN_WS% $ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs
$ roscreate-pkg robot_setup_tf roscpp tf geometry_msgs
上記のコマンドは適切な場所で実行して下さい（例えば前のチュートリアルで作成したであろう ~/ros など）．
代替案： navigation_tutorials スタックの中に robot_setup_tf_tutorial というパッケージがあります．次のコマンドでインストールすることが可能です（ %YOUR_ROS_DISTRO% は { fuerte, groovy } など）:
$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials
Transform の Broadcast
パッケージを作成したあとは ROS を介して base_laser から base_link への transform を broadcast するノードを作成します．お気に入りのエディターを起動し，今作ったばかりの robot_setup_tf パッケージに次のコードを src/tf_broadcaster.cpp として保存しましょう．
1 #include <ros/ros.h>
2 #include <tf/transform_broadcaster.h>
3
4 int main(int argc, char** argv){
5 ros::init(argc, argv, "robot_tf_publisher");
6 ros::NodeHandle n;
7
8 ros::Rate r(100);
9
10 tf::TransformBroadcaster broadcaster;
11
12 while(n.ok()){
13 broadcaster.sendTransform(
14 tf::StampedTransform(
15 tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
16 ros::Time::now(),"base_link", "base_laser"));
17 r.sleep();
18 }
19 }
では base_link から base_laser への transform を publish する部分を中心にコードを見ていきましょう．
tf パッケージは transform をより簡単に publish できるように tf::TransformBroadcaster を提供しています．TransformBroadcaster を使うために tf/transform_broadcaster.h インクルードしなくてはなりません．
10 tf::TransformBroadcaster broadcaster;
ここで TransformBroadcaster のオブジェクトを作成します．あとで wire を介して base_link から base_laser への transform を送るときに使います．
ここがミソです． TransformBroadcaster を使って transform を送る際は引数を5つとります．第一引数は2つの coordinate frame の間に生じる回転を表す回転行列 btQuaternion です．今回は回転させたくはないのでピッチ・ロール・ヨーがすべて0で構成される btQuaternion を使います．第二引数は2つの coordinate frame の間に生じる移動を表す btVector3 です．ロボットの基準面から見てレーザーはx方向に10cm，z方向に20cmの位置にあるので，そうなるよう btVector3 を作ります．第三引数として transform が publish されたタイムスタンプを渡す必要があります．単に ros::Time::now() とすれば大丈夫です．第四引数に現在作ろうとしているリンクの親ノードの名前，ここでは "base_link" を渡し，第五引数には子ノードの名前， "base_laser" を渡します．
Transform の利用
ROS を介して base_laser から base_link への transform を publish するノードを作成することができました．では，その transform を用いて "base_laser" の frame 上のポイントを "base_link" の frame に変換するノードを作りましょう．下記のコードを robot_setup_tf の中の src/tf_listener.cpp に貼り付けてください．その後解説に移ります．
1 #include <ros/ros.h>
2 #include <geometry_msgs/PointStamped.h>
3 #include <tf/transform_listener.h>
4
5 void transformPoint(const tf::TransformListener& listener){
6 //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
7 geometry_msgs::PointStamped laser_point;
8 laser_point.header.frame_id = "base_laser";
9
10 //we'll just use the most recent transform available for our simple example
11 laser_point.header.stamp = ros::Time();
12
13 //just an arbitrary point in space
14 laser_point.point.x = 1.0;
15 laser_point.point.y = 0.2;
16 laser_point.point.z = 0.0;
17
18 try{
19 geometry_msgs::PointStamped base_point;
20 listener.transformPoint("base_link", laser_point, base_point);
21
22 ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
23 laser_point.point.x, laser_point.point.y, laser_point.point.z,
24 base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
25 }
26 catch(tf::TransformException& ex){
27 ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
28 }
29 }
30
31 int main(int argc, char** argv){
32 ros::init(argc, argv, "robot_tf_listener");
33 ros::NodeHandle n;
34
35 tf::TransformListener listener(ros::Duration(10));
36
37 //we'll transform a point once every second
38 ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
39
40 ros::spin();
41
42 }
では重要なところを少しずつ見ていきましょう．
tf/transform_listener.h をインクルードします．これは tf::TransformListener を使う際に必要なヘッダファイルです． TransformListener オブジェクトはROSを介して送られる transform のメッセージトピックを自動的に subscribe し， wire を介して入ってくるすべての transform データを管理します．
5 void transformPoint(const tf::TransformListener& listener){
引数として与えられた TransformListener を元に関数を作ります．この関数は "base_laser" の frame 上にあるポイントを "base_link" の frame に変換する関数です． main() の中で作られる ros::Timer のコールバック関数として一秒ごとに起動します．
6 //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
7 geometry_msgs::PointStamped laser_point;
8 laser_point.header.frame_id = "base_laser";
9
10 //we'll just use the most recent transform available for our simple example
11 laser_point.header.stamp = ros::Time();
12
13 //just an arbitrary point in space
14 laser_point.point.x = 1.0;
15 laser_point.point.y = 0.2;
16 laser_point.point.z = 0.0;
ここで geometry_msgs::PointStamped のポイントを作ります．名前の最後についている "Stamped" というのは，タイムスタンプと frame_id の2つをそのメッセージと関連付けることができるヘッダの存在を意味しています．そして laser_point メッセージのスタンプ field を ros::Time() に設定します．これによって TransformListener に最新の transform を要求することができるようになります．ヘッダの frame_id には "base_laser" を設定します． "base_laser" の frame 上でのポイントを作成しているからです．最後にいくつかのデータを設定します． x に1.0， y に0.2， z に0.0を入れて下さい．
18 try{
19 geometry_msgs::PointStamped base_point;
20 listener.transformPoint("base_link", laser_point, base_point);
21
22 ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
23 laser_point.point.x, laser_point.point.y, laser_point.point.z,
24 base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
25 }
ここまでで "base_link" の frame 上へと移したいポイントを "base_laser" の frame 上に作りました．実際に移動させるためには TransformListener のオブジェクトを用いて transformPoint() を呼び出します．引数は「ポイントを移動する先の frame の名前（この例では "base_link" ）」「移動しようとしているポインタの名前」「移動後の格納先」の3つです． transformPoint() の呼び出し後， laser_point が "base_link" の frame 上で持っていた情報と同じ情報を base_point が所有します．
もし何らかの原因（ tf_broadcaster が動いていない，など）により， base_laser から base_link への transform が使用できなければ， transformPoint() を呼んだときに TransformListener が例外を throw しているかもしれません．これを上手いこと扱うために，例外を catch しエラーを表示するコードを書いておきましょう．
ビルド
ここまででサンプルコードは書き終えました．次はビルドします． roscreate-pkg によって自動生成された CMakeLists.txt を開き，次のコードを一番下に付け加えて下さい．
add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(tf_listener ${catkin_LIBRARIES})忘れずに保存したら，パッケージをビルドします．
$ cd %TOP_DIR_YOUR_CATKIN_WS% $ catkin_make
rosbuild_add_executable(tf_broadcaster src/tf_broadcaster.cpp) rosbuild_add_executable(tf_listener src/tf_listener.cpp)
Next, make sure to save the file and build the package.
$ rosmake robot_setup_tf
実行
さて，ビルドも完了しました．次に ROS の中で何が起きているのか見てみましょう．このセクションでは3つのターミナルを開くことになります．
1つ目のターミナルではcoreを立ち上げます．
roscore
2つ目のターミナルでは先ほど作成した tf_broadcaster を立ち上げます．
rosrun robot_setup_tf tf_broadcaster
いい調子です．では，3つ目のターミナルで先ほど作成した tf_listener を立ち上げます．これを使って "base_laser" の frame 上 から "base_link" の frame 上へとポイントを移動させます．
rosrun robot_setup_tf tf_listener
すべてが上手くいけば次のような出力を見ることでしょう．これは2秒に1回 "base_laser" の frame 上 から "base_link" の frame 上へと移動されたポイントを表示しています．
[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19 [ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19 [ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19 [ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19 [ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19
おめでとうございます！平面レーザーの transform をしっかりと broadcast するプログラムを書くことができました．次のステップは上記の例で用いた PointStamped を ROS 由来のセンサデータに置き換えることです．このプロセスの資料はここで見ることができるでしょう here.
