Yura YuLife

ITエンジニアの覚え書き。

ROS Pythonでオイラー角とクォータニオンの相互変換

ROS python

ROSでオイラー角とクォータニオンを変換するには、tfパッケージの関数を利用すれば良いのですが、 単体の関数として使うには少々使いづらいので、簡単なラッパー関数を作りました。

オイラー角からクォータニオンへの変換

import tf
from geometry_msgs.msg import Quaternion

def euler_to_quaternion(euler):
    """Convert Euler Angles to Quaternion

    euler: geometry_msgs/Vector3
    quaternion: geometry_msgs/Quaternion
    """
    q = tf.transformations.quaternion_from_euler(euler.x, euler.y, euler.z)
    return Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])

実行例

>>> import math
>>> euler_to_quaternion(Vector3(0.0, 0.0, 0.0))
x: 0.0
y: 0.0
z: 0.0
w: 1.0

>>> euler_to_quarternion(Vector3(0.0, 0.0, math.pi / 2.0))
x: 0.0
y: 0.0
z: 0.707106781187
w: 0.707106781187

クォータニオンからオイラー角への変換

import tf
from geometry_msgs.msg import Vector3

def quaternion_to_euler(quaternion):
    """Convert Quaternion to Euler Angles

    quarternion: geometry_msgs/Quaternion
    euler: geometry_msgs/Vector3
    """
    e = tf.transformations.euler_from_quaternion((quaternion.x, quaternion.y, quaternion.z, quaternion.w))
    return Vector3(x=e[0], y=e[1], z=e[2])

実行例

>>> quaternion_to_euler(Quaternion(0.0, 0.0, 0.0, 1.0))
x: 0.0
y: -0.0
z: 0.0

>>> quaternion_to_euler(Quaternion(0.0, 0.0, 0.7071, 0.7071))
x: 0.0
y: -0.0
z: 1.57079632679

参考URL