モーダルを閉じる工作HardwareHub ロゴ画像

工作HardwareHubは、ロボット工作や電子工作に関する情報やモノが行き交うコミュニティサイトです。さらに詳しく

利用規約プライバシーポリシー に同意したうえでログインしてください。

工作HardwareHub ロゴ画像 (Laptop端末利用時)
工作HardwareHub ロゴ画像 (Mobile端末利用時)

ROS/tf による座標変換

モーダルを閉じる

ステッカーを選択してください

モーダルを閉じる

お支払い内容をご確認ください

購入商品
」ステッカーの表示権
メッセージ
料金
(税込)
決済方法
GooglePayマーク
決済プラットフォーム
確認事項

利用規約をご確認のうえお支払いください

※カード情報はGoogleアカウント内に保存されます。本サイトやStripeには保存されません

※記事の執筆者は購入者のユーザー名を知ることができます

※購入後のキャンセルはできません

作成日作成日
2019/04/13
最終更新最終更新
2025/01/27
記事区分記事区分
一般公開

目次

    アカウント プロフィール画像 (サイドバー)

    機械系ですがコードも書きます

    0
    ステッカーを贈るとは?

    ロボットを制御する際に複数の座標系を扱う必要があります。これら複数の座標系間の変換等を行うためのライブラリに 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をサブスクライブする場合は worldbase_linkxxxmug に変更します。

    時間を指定して異なる座標系間の 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))
    
    0
    詳細設定を開く/閉じる
    アカウント プロフィール画像 (本文下)

    機械系ですがコードも書きます

    記事の執筆者にステッカーを贈る

    有益な情報に対するお礼として、またはコメント欄における質問への返答に対するお礼として、 記事の読者は、執筆者に有料のステッカーを贈ることができます。

    さらに詳しく →
    ステッカーを贈る コンセプト画像

    Feedbacks

    Feedbacks コンセプト画像

      ログインするとコメントを投稿できます。

      関連記事

      • ROS 環境構築および簡単な使い方 (Debian9)
        サムネイル画像-cca07ee896
        ロボットシステム開発のためのフレームワーク Robot Operating System (ROS) の環境構築および簡単な使い方について記載します。ここでは Debian9 を利用することにします。ROS は現行バージョン1 の Melodic Morenia ディストリビューションを扱います。 ROS 対応のロボット [TurtleBot3](http://emanual.robotis.co...
        ロボラボロボラボ1/27/2025に更新
        いいねアイコン画像0
      • ロボットシミュレータ Gazebo の簡単な使い方 (ROS)
        サムネイル画像-82838933fc
        ロボットシミュレータ Gazebo を ROS開発で利用するための簡単な手順を記載します。ここでは Debian9 を利用します。個別にインストールすることもできますが、ROS を ros-melodic-desktop-full でインストールすれば gazebo もインストールされます。 [URDF モデルの作成](http://wiki.ro
      • STL ファイルに Blender でテクスチャを適用して COLLADA を作成
        サムネイル画像-27ee777368
        ROS による開発で必要となる 3D モデルは STL や COLLADA データとして作成できます。FreeCAD と Blender を用いて直方体のデータを作成してみます。いずれの場合も、ROS 等で扱うために単位はメートルで出力します。 FreeCAD による直方体 STL の作成 STL はフェイスに三角形を利用した 3D モデルを表現するためのファイルフォーマットの一つです。多くのソフ...
        HARUTOHARUTO1/27/2025に更新
        いいねアイコン画像0
      • 輪郭に関連した画像処理 (OpenCV3 C++)
        サムネイル画像-2798a45608
        cv::Canny などで検出したエッジをもとに cv::findContours で輪郭を計算できます。輪郭に関連した処理の例を記載します。 輪郭の描画 #include <opencv2/opencv.hpp> #in
        あきらあきら8/22/2024に更新
        いいねアイコン画像0
      • Qt for Python (PySide2) の基本的な使い方
        サムネイル画像-3eccbf937d
        QT を Python から利用するためのライブラリには PyQt や PySide 等が存在します。PySide は元々 QT4 向けのライブラリでしたが、QT5 に対応するために新たに PySide2 が開発されました。PySide2 は Qt for Python ともよばれています。 Q: PySide? Qt for Python? what is the name?A: The nam...
        AWS NinjaAWS Ninja12/17/2023に更新
        いいねアイコン画像0