環境
本記事は以下の環境を想定して記述している。
| 項目 | 値 |
|---|---|
| OS | Ubuntu 22.04 |
| ROS | ROS 2 Humble |
本記事の概要
本記事では、tf2の概要とbroadcaster, listenerの作成方法について解説する。
本記事の内容は、ROS講座17 tf-2broadcast、listen
の内容をROS 2公式チュートリアル：tf2を参考にしてROS 2対応させたものである。
tf2について
tf2とはロボットの座標変換ライブラリの名称である。
tf2では各座標系をフレームと呼び、このフレーム同士の関係をツリー構造で管理する。
また、2つのフレーム間の位置と姿勢を追跡でき、時間の管理も可能である。
ツリー構造について
フレームは親子関係を持つ事ができ、原点となるフレームを親、
そこから派生するフレームを子として扱う。
例えば、以下のようなフレームがあるとする。
- 原点となるフレーム
base_link -
base_linkを親とするフレームa_frame -
base_linkを親とするフレームb_frame -
a_frameを親とするフレームc_frame
上記をツリー構造で可視化すると以下のようになる。
このようにtf2では各フレームがツリー構造で示される。
フレームの変換について
座標はその物の位置を示すが、どこを原点とするかによってその位置情報に差が生まれる。
先ほどの例を用いて説明すると、原点がbese_linkの場合とb_frameの場合ではa_frameの位置情報が異なる。
この差の変換を容易に行えるようにしたのがtf2である。
時間について
時間を記録することで、どの座標がどの時間にどこにいたかを認識できるようになる。
例えば、a_frameが5秒前にいた位置を認識しc_frameを移動させることが可能になる。
補足1：tfとtf2
tf2はtfというライブラリをさらに改善したものである。
ROS 1ではtfとtf2の二つのライブラリがあり、ROS Hydro以降はtf2が推奨されていた。(参考：[http://wiki.ros.org/tf])
ROS 2では使用ライブラリはtf2に統一されている。
補足2：tfの概要について
tfの概要については、以下のサイトでも解説されているため合わせて確認するとよい。
tf2のbroadcast
broadcastとは以下の情報を/tf or /tf_staticトピックに送信することである。
- 親フレーム(parent)のフレーム名
- 子フレーム(child)のフレーム名
- 親フレームから見た子フレームの位置と姿勢情報
- この関係が存在した時間
broadcasterには以下の2種類がある。
- static
- 親フレームと子フレームの位置関係が常に同じであるもの。
-
/tf_staticに情報が配信される。 - 常に関係性が同じであるため、broadcastを行うのはノード起動時の一回のみである。
- dynamic
- 親フレームと子フレームの位置関係が時間によって異なるもの。
- 例えば、親フレームが地図・子フレームが走行している車だった場合、位置関係が時間によって変わるためdynamicとなる。
-
/tfに情報が配信される。 - 位置関係や時間が変わるため、正しい情報を得るために常にbroadcastする必要がある。
- 親フレームと子フレームの位置関係が時間によって異なるもの。
tf2のlisten
listenはbroadcastされた/tf もしくは /tf_staticを受信してその情報を保持する。
保持時間は標準で最大10秒間である。
ここで得た情報を用いて座標変換結果の取得を行える。
ソースコード作成
実際にbroadcasterとlistenerを作成する。
パッケージの作成
まず初めにパッケージの作成を行う。
ワークスペースはPub&Sub通信等に使用したros2_lecture_wsを今回も使用する。
パッケージ作成は以下のようにして行う。
cd ~/ros2_lecture_ws/src
ros2 pkg create --build-type ament_cmake tf2_lecture
broadcasterの実装
broadcasterを実装する。ソースコードへのリンクはこちら。
このコードでは、親フレームをbase_linkとし、子フレームとして以下二つを作成している。
- staticなフレームである
static_frame-
base_linkからz軸の正方向に1.0離れた位置で静止している。 - ノード起動時に一度だけbroadcastされる。
-
- dynamicなフレームである
dynamic_frame-
base_linkの周りを周回する。 - 1秒おきにbroadcastされる。
-
コードの細かい解説についてはコード内のコメントを参照することとし、コメントで扱いきれなかった情報について以下にまとめる。
- 姿勢の設定には四元数が使用されている。
四元数とは、姿勢を表すための表現方法である。- 四元数については、以下のサイトが参考になる
listenerの実装
listenerを実装する。ソースコードへのリンクはこちら
このコードでは、broadcastされたデータを受け取りbase_linkからdynamic_frameへの変換情報を出力している。
- lookupTransform()関数で2つのフレーム間の変換結果を取得する。
- この関数からは、
source frameにおける座標をtarget frameに変換するための座標変換が得られる。 - 変換は、第二引数の
source frameから第一引数のtarget frameへと行われる。 - 第三引数には時刻を指定するが、基本的には
tf2::TimePointZeroを使えばよい。(※1)
- この関数からは、
- 座標変換が正常に行われなかった場合lookupTransform()関数は例外を出すため、座標変換をする際は必ずtry-catch構文による例外処理を使用する。
※1
tf2::TimePointZeroを使うことで変換可能な最新の時刻を設定できるが、他の時間を指定することもできる。
本記事では説明を省略するが、下記サイトが参考になる。
ビルドの設定
以下、launchファイルとビルドの設定を行う。
launch
各フレームを可視化するため、今回はbroadcaster, listenerの実行に加え、RVizも同時に実行させる。
よって、上記3つのノードを同時起動できるlaunchファイルを作成する。
ソースコードはこちら
CMakeLists.txt
tf2_lecture/CMakeLists.txtに以下を追記する。
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2 REQUIRED) # tf2の使用のために必要
+find_package(tf2_ros REQUIRED) # tf2の使用のために必要
+add_executable(tf2_broadcaster
+ src/tf2_broadcaster.cpp
+)
+add_executable(tf2_listener
+ src/tf2_listener.cpp
+)
+ament_target_dependencies(tf2_broadcaster
+ geometry_msgs
+ rclcpp
+ tf2
+ tf2_ros
+)
+ament_target_dependencies(tf2_listener
+ geometry_msgs
+ rclcpp
+ tf2
+ tf2_ros
+)
+install(TARGETS
+ tf2_broadcaster
+ tf2_listener
+ DESTINATION lib/${PROJECT_NAME}
+)
+install(
+ DIRECTORY
+ launch
+ DESTINATION share/${PROJECT_NAME}/
+)
package.xml
tf2_lecture/package.xmlに以下を追記する。
+ <depend>geometry_msgs</depend>
+ <depend>rclcpp</depend>
+ <depend>tf2</depend>
+ <depend>tf2_ros</depend>
+ <exec_depend>ros2launch</exec_depend>
ビルド
以下のコマンドでビルドを行う。
colcon build --packages-select tf2_lecture
なお、ビルドを行う前に下記コマンドで不足している依存関係がないか確認するとよい。
rosdep install -i --from-path src --rosdistro humble -y
実行と確認
ビルドが終われば、以下のコマンドを実行してlaunchファイルを起動する。
なお、ros2_lecture_wsで. install/setup.bashコマンドを実行していないと起動できないので注意。
ros2 launch tf2_lecture tf2_lecture.launch.py
実行後、ターミナルにbase_linkからdynamic_frameへの座標変換結果が以下のように出力される。
[tf2_listener-3] [INFO] [1674105583.846797874] [listener1]: base_link->dynamic_frame: -0.714269 0.699871 0.000000
[tf2_listener-3] [INFO] [1674105584.846805414] [listener1]: base_link->dynamic_frame: 0.714264 -0.699876 0.000000
[tf2_listener-3] [INFO] [1674105585.846810339] [listener1]: base_link->dynamic_frame: -0.714236 0.699904 0.000000
(以下省略)
ただし、RVizが初期設定のままだと何も表示されないので下記に示す設定を行う必要がある。
RVizの設定
以下、今回作ったフレームをRViz上で可視化させるための操作手順である。
なお、rviz2でRVizを起動しモニター上にRVizが起動されている前提とする。
- "Add"ボタン(RViz画面左下にある)を押す。
- 新しくウィンドウが表示されるので、"By display type"タブから
TFという項目を探してクリックし右下の"✓OK"ボタンを押す。- これによって、tf関係のトピックが可視化される。
- "Displays"タブの中にある"Fixed Frame"で
mapが選択されている場合、base_linkに変更する。-
mapをクリックして右の▼ボタンをクリックすると選択できるフレームのリストが表示されるので、そこから選ぶ。- ここで選択したフレームが原点となり、そのフレームに基づく各座標が可視化されるようになる。
-
mapというフレームは今回使用していないため、親フレームであるbase_linkに変更している。
-
- "TF"横にある▶ボタンを押して"Shown Names"にチェックを入れる。
- これにより各フレームの名前を表示できる。
以上の操作を行うと下記のような画面になる。
以下のような表示がなされていることが期待動作である。
-
base_link,static_frame,dynamic_frameの三つの座標が見える。 -
static_frameが静止している。 -
dynamic_frameが、base_linkの周りを一定の距離/速さで移動している。