#################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = このチュートリアルは [[ROS/Tutorials | ROS Tutorials]] と [[tf/Tutorials#Learning_tf|tf basic tutorials]] を読み終えたことを想定しています．また，このチュートリアルで用いるコードは [[robot_setup_tf_tutorial | robot_setup_tf_tutorial]] パッケージから入手可能です ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = tf を用いたロボットのセットアップ ## multi-line description to be displayed in search ## description = このチュートリアルでは tf を使ってロボットをセットアップする方法を学びます． ## the next tutorial description (optional) ## next = tf に対してロボットの状態を publish する方法を学びます： ## links to next tutorial (optional) ## next.0.link=[[ja/robot_state_publisher/Tutorials/Using the robot state publisher on your own robot|using the robot state publisher on your own robot]] ## next.1.link= ## what level user is this tutorial for ## level= BeginnerCategory ## keywords = #################################### <> <> <> == 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 の命名規則については[[http://www.ros.org/reps/rep-0105.html|REP 105]]を参照して下さい． ここで，レーザー中心点からの距離データを取得していると想定しましょう．つまり "base_link" の coordinate frame 上にデータを取得したということになります．私たちはこのデータを使って移動ロボットが障害物を回避することに役立てたいとします．そうするためには "base_laser" の frame から受け取ったレーザースキャンを "base_link" の frame に変換する方法が必要です． {{attachment:simple_robot.png}} この関係を定義するために，レーザーは移動ロボットの中心から前に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 は使用しており，すべてのエッジ（枝）は親ノードから子ノードに向けられていると想定しています． {{attachment:tf_robot.png}} 簡単に 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|ROS Documentation]] を見てみて下さい． "base_laser" の frame 上で得た点群を "base_link" の frame に変換することを考えます．最初に transform を publish するノードを作ります．次に ROS を介して transform を listen し，その transform を用いてポイントを変換するためのノードを作ります．では"robot_setup_tf" のようなシンプルな名前のパッケージを作成してください．そのパッケージは[[roscpp]] と [[tf]] と [[geometry_msgs]] に依存しています． {{{{{#!wiki buildsystem catkin {{{ $ cd %TOP_DIR_YOUR_CATKIN_WS% $ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs }}} }}}}} {{{{{#!wiki buildsystem rosbuild {{{ $ 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` として保存しましょう． {{{ #!cplusplus block=broadcaster #include #include int main(int argc, char** argv){ ros::init(argc, argv, "robot_tf_publisher"); ros::NodeHandle n; ros::Rate r(100); tf::TransformBroadcaster broadcaster; while(n.ok()){ broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), ros::Time::now(),"base_link", "base_laser")); r.sleep(); } } }}} では `base_link` から `base_laser` への transform を publish する部分を中心にコードを見ていきましょう． <> tf パッケージは transform をより簡単に publish できるように `tf::TransformBroadcaster` を提供しています．`TransformBroadcaster` を使うために `tf/transform_broadcaster.h` インクルードしなくてはなりません． <> ここで `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` に貼り付けてください．その後解説に移ります． {{{ #!cplusplus block=listener #include #include #include void transformPoint(const tf::TransformListener& listener){ //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "base_laser"; //we'll just use the most recent transform available for our simple example laser_point.header.stamp = ros::Time(); //just an arbitrary point in space laser_point.point.x = 1.0; laser_point.point.y = 0.2; laser_point.point.z = 0.0; try{ geometry_msgs::PointStamped base_point; listener.transformPoint("base_link", laser_point, base_point); ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); } catch(tf::TransformException& ex){ ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); } } int main(int argc, char** argv){ ros::init(argc, argv, "robot_tf_listener"); ros::NodeHandle n; tf::TransformListener listener(ros::Duration(10)); //we'll transform a point once every second ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); ros::spin(); } }}} では重要なところを少しずつ見ていきましょう． <> `tf/transform_listener.h` をインクルードします．これは `tf::TransformListener` を使う際に必要なヘッダファイルです． `TransformListener` オブジェクトはROSを介して送られる transform のメッセージトピックを自動的に subscribe し， wire を介して入ってくるすべての transform データを管理します． <> 引数として与えられた `TransformListener` を元に関数を作ります．この関数は "base_laser" の frame 上にあるポイントを "base_link" の frame に変換する関数です． `main()` の中で作られる `ros::Timer` のコールバック関数として一秒ごとに起動します． <> ここで `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を入れて下さい． <> ここまでで "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` を開き，次のコードを一番下に付け加えて下さい． {{{{{#!wiki buildsystem catkin {{{ 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 }}} }}}}} {{{{{#!wiki buildsystem rosbuild {{{ 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 由来のセンサデータに置き換えることです．このプロセスの資料はここで見ることができるでしょう [[navigation/Tutorials/RobotSetup/Sensors|here]].