MENU

ROS – フレーム管理ライブラリ tf2 の基本

ROS
ROS
  1. ホーム
  2. ROS
  3. ROS – フレーム管理ライブラリ tf2 の基本
目次

概要

ROS のフレーム管理ライブラリ tf2 の基本について解説します。

tf

ロボットを動かすプログラムを ROS で作成しようとしたとき、ワールドフレーム、ロボットのフレーム、障害物のフレームといったように多くの3次元のフレームを考える必要があります。フレームの中には時刻変化するものもあるため、tf はこれらのフレームをタイムスタンプ付きで管理し、あるフレームから別のフレームへの変換を計算できるライブラリです。

tf と tf2

tf2 は tf を改良したライブラリです。今から開発する場合は tf ではなく、tf2 を使いましょう。

tf2 関係のパッケージ

ros/geometry2 に tf2 関係のパッケージ群があります。

tf2 及び tf2_ros が tf2 本体の C++ 実装です。tf2_msgs は tf2 で扱うメッセージの型を定義しており、それ以外のパッケージは他のライブラリのデータ型との変換機能を提供しています。

  • tf2: ROS に依存しない tf2 のライブラリ内で使用する型やインターフェースを定義
  • tf2_ros: tf2 の ROS 実装
  • tf2_msgs: tf2 のメッセージを定義
  • tf2_tools: tf2 のデバッグ用のコマンドラインツール
  • tf2_py: tf2 と PyObjects のデータ間の変換を提供
  • tf2_geometry_msgs: tf2 と geometry_msgs のデータ間の変換を提供
  • tf2_bullet: tf2 と bullet のデータ間の変換を提供
  • tf2_eigen: tf2 と Eigen のデータ間の変換を提供
  • tf2_kdl: tf2 と KDL のデータ間の変換を提供
  • tf2_sensor_msgs: tf2 と sensor_msgs のデータ間の変換を提供

tf を利用する場合、tf2, tf2_ros が必須であり、他はオプションです。

tf2 の特徴

木構造で管理

tf は、フレームを木構造で管理します。フレームを tf に登録する際は、あるフレームから別のフレームへの変換を時刻付きで登録する形で行います。 これは geometry_msgs/TransformStamped 型で表します。

geometry_msgs/TransformStamped

std_msgs/Header header
string child_frame_id
geometry_msgs/Transform transform
from geometry_msgs.msgs import TransformStamped

ts = TransformStamped()
ts.header.stamp  # その関係が観測された時刻
ts.header.frame_id  # 親フレームの ID
ts.child_frame_id  # 子フレームの ID
ts.transform.translation  # 親フレームから子フレームへ変換するための平行移動成分
trans.transform.rotation  # 親フレームから子フレームへ変換するための回転量

static なフレームと dynamic なフレーム

例えば、空間上に固定された障害物と移動しているロボットがいるとします。障害物のフレームは時間で変化しないので、static なフレームです。一方、ロボットは移動しておりフレームは時間ごとに変化するため、dynamic なフレームです。

tf2 では、フレームを管理するために time キャッシュ と static キャッシュという2つのキャッシュを持っています。

  • time キャッシュ: time キャッシュは、同じフレームでも時刻ごとに deque で管理します。2つのフレーム間で計算を行う場合はできるだけ同じ時刻のフレーム同士を検索して行うなど、時刻を考慮した処理が行われます。dynamic なフレームを tf に送信する場合は TransformBroadcaster インタフェースを使用します。
  • static キャッシュ: 時刻によって変化しない座標は static キャッシュで管理します。フレームは最新の時刻のものだけ保持するようになっており、計算を行う際に時刻は考慮されません。static なフレームを tf に送信する場合は StaticTransformBroadcaster インターフェースを使います。

tf のトピック

tf のフレームの登録は tf2_msgs/TFMessage 型のトピックを送信することで行います。このトピックは geometry_msgs/TransformStamped の配列になっており、複数のフレームを一度に登録できます。

tf2_msgs/TFMessage

geometry_msgs/TransformStamped[] transforms

StaticTransformBroadcaster.sendTransform() で送信した場合、/static_tf という名前のトピックで送信されます。一方、TransformBroadcaster.sendTransform() で送信した場合、/tf という名前のトピックで送信されます。

# 1つだけフレームを送信する
TransformBroadcaster.sendTransform(trans)
# 複数のフレームを送信する
TransformBroadcaster.sendTransform([trans1, trans2, trans3])

tf2 のフレームの管理システム

tf2 のフレームを管理する方法は次の2つがあります。

ローカルで管理: tf1 からある方式。フレーム間の変換を計算したい場合に TransformListener オブジェクトを作成し、/tf 及び /tf_static トピックを購読する。

tf2 において、フレームの登録は、ROS のトピックを使って行われます。 例えば、tf の送信元が100、受信元が100ある場合、1つの送信元と受信元で /tf 及び /tf_static という2つのトピックがやり取りされるため、合計で 100 x 100 x 2 = 20000 の TCP コネクションが張られます。そのため、PC のスペックによっては CPU に負荷がかかり、リアルタイムに処理できなくなる可能性があります。また、受信元でそれぞれフレームの情報を管理するため、メモリ容量の増大します。

ローカルで管理

この問題を解決する方法として tf2 ではサーバーを用意してそこで /tf 及び /tf_static のトピックを購読します。フレーム間の変換を計算したい場合にはそのサーバーに問い合わせて行います。この方式の場合、受信元はサーバー1つになるため、前者の方式に比べて TCP コネクションの数が合計で 100 x 2 = 200 となります。コネクションが減ったため、CPU に対する負荷が減少します。

サーバーで管理: tf2 で追加された方式。まず、/tf 及び /tf_static トピックを購読するためのサーバー tf2_buffer_server を起動する。フレーム間の変換を計算したい場合に BufferClient オブジェクトを作成し、サーバーに問い合わせて情報を取得する。サーバーとのやり取りには ROS の actionlib が使われます。

サーバーで管理

tf2 のツール

tf2_tools パッケージに付属している以下のツールで tf2 のデバッグを行えます。

view_frames

起動してから5秒間だけ tf トピックを受信し、tf の木構造を dot 形式及び pdf 形式で出力します。

rosrun tf2_tools view_frames.py

実行すると、dot フォーマットの frames.gv と木構造の画像化結果 frames.pdf が出力されます。

tf_echo

reference_frame から target_frame の変換を計算し、出力します。

rosrun tf tf_echo <reference_frame> <target_frame>

static_transform_publisher

static_transform_publisher は static なフレームを送信するためのコマンドラインツールです。実行すると、フレームを /tf_static トピックで1度だけ送信します。

回転をクォータニオンで指定する場合

rosrun tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

回転をオイラー角で指定する場合

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

static_transform_publisher で送信したあとに起動した TransformListener も Publisher の latch=True 機能を使用して最新のトピックを受信できるようにこのプログラムは起動したままにします。

tf2 の設計

tf2 のライブラリ構造は以下のようになっています。

TransformStorage: フレームの情報を格納するデータクラス
Cache はあるフレームの情報を記録するデータ構造です。
TimeCacheInterface: キャッシュのインターフェースを定義
├── StaticCache: static キャッシュの実装
└── TimeCache: time キャッシュの実装

Buffer は複数のフレームを木構造で管理し、フレーム間の計算機能を提供します。
BufferCore: Buffer の基本機能を定義
BufferInterface: Buffer のインターフェースを定義
└── Buffer: Buffer のインターフェースの実装

TransformListener: 送信されたフレームを受信するインターフェース
TransformBroadcaster: dynamic なフレームを送信するインターフェース
StaticTransformBroadcaster: static なフレームを送信するインターフェース
BufferServer: 送信されたフレームを受信するためのサーバー
BufferClient: サーバーに問い合わせるためのクライアント

tf2 の API

StaticTransformBroadcaster – static なフレームの登録

  1. フレームを送信するインターフェイスである StaticTransformBroadcaster オブジェクトを作成します。
  2. TransformStamped オブジェクトを作成し、時刻、親フレームの ID、子フレームの ID、親フレームから子フレームへの変換を設定します。
  3. TTransformBroadcaster.sendTransform() で TransformStamped を /tf_static トピックとして送信します。

TransformBroadcaster – dynamic なフレームの登録

  1. フレームを送信するインターフェイスである TransformBroadcaster オブジェクトを作成します。
  2. TransformStamped オブジェクトを作成し、時刻、親フレームの ID、子フレームの ID、親フレームから子フレームへの変換を設定します。
  3. TransformBroadcaster.sendTransform() で TransformStamped を /tf トピックとして送信します。

TransformListener – tf を受信する

  1. 受信したフレームを記録するための Buffer オブジェクトを作成します。
  2. Buffer オブジェクトをコンストラクタ引数に渡して、/tf 及び /tf_static トピックを受信するための TransformListener オブジェクトを作成します。受信したフレーム情報は Buffer オブジェクトに蓄積されます。

TransformBroadcaster と TransformListener

ある時刻において、フレーム間の変換

例: 5秒前のフレームAから5秒前のフレームBへの変換を求める

lookup_transform() は時刻 time のフレーム source_frame からフレーム target_frame への変換を取得します。

lookup_transform(target_frame, source_frame, time, timeout=rospy.Duration(0.0))

can_transform() は時刻 time のフレーム source_frame からフレーム target_frame への変換が存在するかどうかを取得します。

can_transform(target_frame, source_frame, time, timeout=rospy.Duration(0.0))
  • timeout はトピックの受信を待機する制限時間です。
  • time = rospy.Time(0) を指定した場合、source_frame と target_frame の共通で利用できる最新の時刻を探します。

異なる時刻のフレーム間の変換

例: 5秒前のフレームAから10秒前のフレームBへの変換を求める

lookup_transform_full() は時刻 target_time のフレーム target_frame から時刻 source_time のフレーム target_frame への変換を取得します。

lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0))

can_transform_full() は時刻 target_time のフレーム target_frame から時刻 source_time のフレーム target_frame への変換が存在するかどうかを取得します。

can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0))
  • 異なる時間のフレーム同士を比較するため、時間変化しない static なフレームを fixed_frame に指定する必要がいあります。

例外

変換できない場合は以下の例外が発生します。

  • ConnectivityException: フレームAとフレームBが同じ木構造のノードでないため、辿ることができない場合に発生する例外です
  • ExtrapolationException: 閾値を超えた補間が生じる場合に発生する例外です
  • LookupException: フレームAからフレームBへの変換が見つからない場合に発生する例外です

参考

ROS
ROS

関連記事

コメント

コメントする

目次