【ROS】tf座標変換をためす

前回の記事（【ROS】tfを理解する（フレームのつながり）)でフレームのつながりについて書きました。
今回は、tfの座標変換ライブラリを使って、座標変換の計算を実際にやってみようと思います。また、行列を使った計算も試しにやってみましたので紹介します。

やること

map->odom->base_linkとtfが繋がっている時、mapフレームからみたbase_linkの位置を算出してみます。
mapフレームの位置(0,0,0)、mapフレームからみたodomフレームの位置(2,1,0)、odomフレームからみたbase_linkフレームの位置（1,1,0)とします。
※簡略化するため姿勢情報は変化なし
※rosは、macに入れたdocker環境で試しました。記事(【Docker】MacにDocker+ROS環境を入れる)

mapフレームからみたbase_linkの位置(tf2ライブラリ)

まずは、tf2ライブラリを使って、mapフレームからみたbase_linkの位置を取り出してみました。
（実装はpythonです）

まずは、テスト用のrosパッケージを作ります。

mapフレームからbase_linkの座標をプリントする以下のスクリプトを実装し、~/catkin/src/tf_test/src/配下に保存します。(ファイル名：tf_test.py)

#!/usr/bin/env python
## coding: UTF-8

import rospy
from tf2_ros import Buffer, TransformListener
import tf2_ros

if __name__ == '__main__':
    try:
        rospy.init_node("tf_test")
        tf_buffer = Buffer()
        tf_listener = TransformListener(tf_buffer)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                trans = tf_buffer.lookup_transform("map", "base_link", rospy.Time(0), rospy.Duration(5.0))
                rospy.loginfo("Translation: x: {}, y: {}, z: {}".format(trans.transform.translation.x,
                                                                       trans.transform.translation.y,
                                                                       trans.transform.translation.z))

            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                rospy.logwarn("TF Exception: {}".format(e))

            rate.sleep()

    except rospy.ROSInterruptException:
        pass

コード解説
17行目のlookup_transformで、mapフレームからbase_linkの座標を計算して取得しています。
取得結果は、TransformStamped型のメッセージとしてtransに保存されます。
今回は、位置情報のみをデバッグしましたが、姿勢については、trans.transform.rotationで取得できます。

動作検証

早速動作検証してみます。
まず、mapフレームからみたodomフレームの位置(2,1,0)、odomフレームからみたbase_linkフレームの位置（1,1,0)を以下コマンドで手動でコマンド送信します。
その前にまずroscoreを立ち上げます。

今回作ったtf_testを動かします。

以下にように、mapフレームからみたbase_linkの座標がコンソールに表示されてます。
座標はx: 3.0, y: 2.0, z: 0.0　です。
想定通りです。

mapフレームからみたbase_linkの位置(tfライブラリ)

先ほどはtf2ライブラリを使ってみましたが、古いライブラリtfを使ってもできるようですのでやってみました。
tf_test2.pyという新しいファイルを作成し、以下のスクリプトを実装しました。

#!/usr/bin/env python
## coding: UTF-8

import rospy
import tf

if __name__ == '__main__':
    try:
        rospy.init_node("tf_test")
        tf_listener = tf.TransformListener()

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                tf_listener.waitForTransform("map", "base_link", rospy.Time(), rospy.Duration(5.0))
                (trans, rot) = tf_listener.lookupTransform("map", "base_link", rospy.Time(0))
                rospy.loginfo(trans)

            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                rospy.logwarn("TF Exception: {}".format(e))

            rate.sleep()

    except rospy.ROSInterruptException:
        pass

コード解説
tf2ライブラリでは、tf情報をBufferに格納し座標変換を計算していましたが、tfライブラリでは、Bufferには格納せずに取得します。
具体的には、16行目lookupTransformで取得しています。
過去のtfを蓄積させて何か制御を行いたい場合は、tf2ライブラリのほうがよいかもしれませんが、そうではない場合はtfライブラリでもよいかもしれません。

動作検証

先ほどと同じ様に検証してみます。
roscoreを立ち上げます。

今回作ったtf_test2を動かします。

結果は、[3.0, 2.0 ,0.0]が表示されました。想定通りですね。

mapフレームからみたbase_linkの位置(同時変換行列を使う)

最後に行列の計算式から算出してみました。
今回のような単純なパターンでは、上記のtfのライブラリから簡単に算出できるので行列で計算する必要はありませんが、行列式による算出についても理解したかったのでやってみました。
tf_test3.pyという新しいファイルを作成し、以下のスクリプトを実装しました。

#!/usr/bin/env python
## coding: UTF-8

import rospy
import tf

if __name__ == '__main__':
    try:
        rospy.init_node("tf_test")
        tf_listener = tf.TransformListener()

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                tf_listener.waitForTransform("map", "odom", rospy.Time(), rospy.Duration(5.0))
                (trans, rot) = tf_listener.lookupTransform("map", "odom", rospy.Time(0))

                tf_listener.waitForTransform("odom", "base_link", rospy.Time(), rospy.Duration(5.0))
                (trans2, rot2) = tf_listener.lookupTransform("odom", "base_link", rospy.Time(0))

                # map->odom matrix
                odom_in_map = tf.transformations.quaternion_matrix(rot)
                odom_in_map[:3, 3] = trans

                # for debug
                # rospy.loginfo(" map->odom matrix")
                # for i in odom_in_map:
                #     rospy.loginfo(i)

                # odom->base_link matrix
                base_link_in_odom = tf.transformations.quaternion_matrix(rot2)
                base_link_in_odom[:3, 3] = trans2

                # for debug
                # rospy.loginfo(" map->odom matrix")
                # for i in base_link_in_odom:
                #     rospy.loginfo(i)

                # cal map->base_footprint 
                map_to_basefootprint = tf.transformations.concatenate_matrices(odom_in_map,base_link_in_odom)
                map_to_basefootprint_trans = tf.transformations.translation_from_matrix(map_to_basefootprint)
                map_to_basefootprint_rot = tf.transformations.quaternion_from_matrix(map_to_basefootprint)

                # for debug
                # rospy.loginfo(" map->base_footprint matrix")
                # for i in map_to_basefootprint:
                #     rospy.loginfo(i)

                rospy.loginfo(map_to_basefootprint_trans)

            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                rospy.logwarn("TF Exception: {}".format(e))

            rate.sleep()

    except rospy.ROSInterruptException:
        pass

コード解説
21行目〜22行目で、mapフレームからみたodomフレームの同時変換行列を作成しています。
同様に31行目〜32行目で、odomフレームからみたbase_linkフレームの同時変換行列を作成しています。
※同時変換行列は、座標ベクトルの並進（平行移動）、拡大縮小、回転を実現することができる変換行列のことです。

40行目のtf.transformations.concatenate_matricesで、２つの同時変換行列の積から、mapフレームからみたbase_linkフレームの同時変換行列を算出しています。
41行目で、同時変換行列から、並進情報（x,y,z)を取得、42行目で回転情報を取得しています。

動作検証

先ほどと同じ様に検証してみます。
roscoreを立ち上げます。

今回作ったtf_test3を動かします。

結果は、[3. 2. 0.]が表示されました。想定通りですね。

最後に

今回は、tfの座標変換を色々試してみました。
tfのライブラリを使うと簡単に座標変換できることがわかりました。
ROSは公開されているパッケージを使えば、中身を知らなくても簡単に動かせるので便利ですが、技術要素について深堀りしないと理解が追いつかないことがあるので、こうやって少しずつ理解を深めていこうと思います。
それでは！！