# 座標の管理 --- ロボットは多くのパーツで構成されています。多くの場合、パーツは移動できます。そのため、多くの座標系を追跡する必要があります。 <img src="./v2_media/tf_pr2.png" alt="" style="width: 39%;"/> --- ## TF2 - 座標系（フレームとも呼ぶ）を管理・処理するツール - ツリー構造の座標系間の接続を維持する - ユーザーは、位置を表すポイントやベクトルをある座標系から別の座標系に変換できる - （内部的にトピック`/tf`および`/tf_static`の出版/購読モデルを使って実装されている） <img src="./v2_media/tf0.png" alt="" style="width: 40%;"/> https://wiki.ros.org/tf2 （tfライブラリも存在していますが、tf2のほうが新しいです。） --- ### TFツリー - 全ての座標系（フレーム）がツリーに繋がっています。 - 位置データは、同じツリー内にある任意の2つのフレーム間で変換可能です。 ツリーの例： <img src="./v2_media/tf_tree.png" alt="" style="width: 40%;"/> --- ### TFツール *ターミナル* 現在の変換ツリーに関する情報をプリント ```bash rosrun tf tf_monitor ``` 2つの座標系間の変換情報をプリント ```bash rosrun tf tf_echo <座標系1> <座標系2> ``` *可視化* TFツリー：`rqt`を実行してから、メニューから Plugins → Visualization → TF Treeを選択 #### turtlesimで試してみよう 以下のコマンドを使って2つの亀ロボットのturtlesimを起動しましょう。 ```bash roslaunch turtle_tf2 turtle_tf2_demo.launch ``` ```bash rosrun turtlesim turtle_teleop_key ``` その後、tfのコマンドを使ってみてください。 ```bash rosrun tf tf_echo turtle1 turtle2 ``` rqtを使って、TFツリーを表示してみてください。 --- ### TFとRViz - Addボタンを使って、「TF」の表示を選択 <img src="./v2_media/tf_rviz.png" alt="" style="width: 95%;"/> **注意**：RVizがメインな座標系として使っているフレーム（Global OptionsのFixed Frame ⇒ 上の図で赤い矢印がついているところ）を必ず実際にTFツリーに入っているフレームに設定する必要があります。 --- ### TF APIの使用 まず、TF用のバッファーと購読者を作成します。 ```python tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) ``` （購読者を1つだけ使ってください。使用するたびに新しい購読者を作成しないように注意してください。） 変換を取得するには ```python trans = tfBuffer.lookup_transform(目的のフレーム, 本来のフレーム, rospy.Time(0)) ``` （rospy.Time(0)の場合、最新の変換が得られる） --- Pythonコードの例（`base`フレームから`odom`フレームへの変換） ```python #!/usr/bin/env python import rospy import math import tf2_ros import geometry_msgs.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('tf2_listener') tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform("odom", "base", rospy.Time()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue rate.sleep() ``` https://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29 tf2_geometry_msgsパッケージの関数を使えば、lookupTransformで上記のように得られた変換パラメータを使用して、点、ポーズなどの座標変換が行えます。 点の座標変換の例： ```python import tf2_geometry_msgs from geometry_msgs.msg import PointStamped, Point #変換したい点 pt = Point(1,2,0) #（do_transform_pointはPointStampedを使うので一旦PointをPointStampedに変換します） pt2 = tf2_geometry_msgs.do_transform_point(PointStamped(point=pt), trans).point rospy.loginfo(pt) rospy.loginfo(pt2) ``` --- - 自分のプログラム内でTF変換を公開する（任意） → 以下のチュートリアルを参照 [tf2静的ブロードキャストの作成](https://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29) [tf2ブロードキャストの作成]( https://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28C%2B%2B%29) --- ### GazeboとTF Gazeboでシミュレートされているロボットのフレームをブロードキャスト → `robot_state_publisher`ツールを使います - `rosrun robot_state_publisher robot_state_publisher`、またはlaunchファイルに追加する --- → [課題](v2_task.html) ← [実習その1](v2_1.html) ↑ [ホームページ](index.html)