ロボットを制御する際に複数の座標系を扱う必要があります。これら複数の座標系間の変換等を行うためのライブラリに tf2 (transform2) があります。C++ と Python がサポートされていますが、ここでは Python を用いて簡単な使い方を把握します。

tf2 チュートリアルと同様に動作検証のためのパッケージを用意します。ROS ノードのスクリプトを格納する nodes ディレクトリも作成しておきます。

catkin_create_pkg learning_tf2 tf2 tf2_ros rospy
cd learning_tf2
mkdir nodes

Transform ブロードキャスト

nodes/static_tf2_broadcaster.py

#!/usr/bin/python
# -*- coding: utf-8 -*-
import rospy
import tf
import tf2_ros
import geometry_msgs.msg

def Main():

    # ROS ノードの初期化処理
    rospy.init_node('static_tf2_broadcaster')

    # ブロードキャスタ、Transform
    br = tf2_ros.StaticTransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    # Transform の時刻情報、Base となる座標系、world を Base とする座標系
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = 'world'
    t.child_frame_id = 'xxx'

    # 6D pose (位置 translation、姿勢 rotation)
    t.transform.translation.x = 0
    t.transform.translation.y = 0
    t.transform.translation.z = 0

    quat = tf.transformations.quaternion_from_euler(0, 0, 0)
    t.transform.rotation.x = quat[0]
    t.transform.rotation.y = quat[1]
    t.transform.rotation.z = quat[2]
    t.transform.rotation.w = quat[3]

    br.sendTransform(t)
    rospy.spin()

if __name__ == '__main__':
    Main()

実行例

chmod +x nodes/static_tf2_broadcaster.py
cd ../..
catkin_make
source devel/setup.bash
roscore
rosrun learning_tf2 static_tf2_broadcaster.py

トピックが作成されて Transform がパブリッシュされていることが確認できます。/world からみたときの /xxx 座標系の Transform も取得できます。/tf_static は /tf と異なり Transform 情報の保持に時間制限がありません。

rostopic echo /tf_static
rosrun tf tf_echo /world /xxx

Transform リスナ

nodes/tf2_listener.py

#!/usr/bin/python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros

def Main():

    # ROS ノードの初期化処理
    rospy.init_node('tf2_listener')

    # リスナーの登録
    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

    # 10 Hz
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():

        # /world に対する /xxx の Transform を取得
        try:
            t = tfBuffer.lookup_transform('xxx', 'world', rospy.Time())
            #t = tfBuffer.lookup_transform('mug', 'base_link', rospy.Time())
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            print(e)
            rate.sleep()
            continue

        print('{0:.2f}, {1:.2f}, {2:.2f}'.format(
            t.transform.translation.x,
            t.transform.translation.y,
            t.transform.translation.z
        ))
        print('{0:.2f}, {1:.2f}, {2:.2f}, {3:.2f}'.format(
            t.transform.rotation.x,
            t.transform.rotation.y,
            t.transform.rotation.z,
            t.transform.rotation.w
        ))

        rate.sleep()

if __name__ == '__main__':
    Main()

Gazebo シミュレータ内のロボットの関節値がパブリッシュされる /tfをサブスクライブする場合は world を base_link、xxx を mug に変更します。

時間を指定して異なる座標系間の Transform を取得

例えば 5 秒前の yyy 座標系から現在の xxx 座標系への Transform を取得するためには以下のようにします。fixed_frame には world を指定します。timeout 設定は任意です。

t = tfBuffer.lookup_transform_full(
    target_frame='xxx',
    target_time=rospy.Time.now(),
    source_frame='yyy',
    source_time=rospy.Time.now() - rospy.Duration(5.0),
    fixed_frame='world',
    timeout=rospy.Duration(1.0)
)

クォータニオン (Quaternion、四元数) の扱い方

sudo apt install python-ipython
python -m IPython

必要な関数や定数を import

from tf.transformations import quaternion_from_euler
from tf.transformations import euler_from_quaternion
from tf.transformations import quaternion_multiply
from numpy import pi
from copy import deepcopy

オイラー角の種別を axes で指定してクォータニオンを取得できます。

quaternion_from_euler(pi, 0, 0, axes='sxyz')

以下のような演算が可能です。

• 回転行列のように multiply できます。
• 逆行列のような inverse を得るためには q[3] の正負を反転させます。

q_new = q_rot * q_world という関係のクォータニオンを用意してみます。

q_world = quaternion_from_euler(0, 0, 0) # (0, 0, 0, 1)
q_rot = quaternion_from_euler(pi, pi/2, pi/3)
q_new = quaternion_multiply(q_rot, q_world)

q_rot = q_new * q_world_inv となることを確認してみます。

q_world_inv = deepcopy(q_world)
q_world_inv[3] = -q_world_inv[3]
print(euler_from_quaternion(quaternion_multiply(q_new, q_world_inv)))
print(euler_from_quaternion(q_rot))