ROS2中如何从欧拉角转换成四元素

ROS1中使用from tf.transformations import quaternion_from_euler导入quaternion_from_euler()即可调用。而ROS2中默认没有安装，需要单独安装一下ros-galactic-tf-transformations(我使用的ROS2是galactic，根据版本名不同做相应修改即可)：

sudo apt-get update
sudo apt-get install ros-galactic-tf-transformations

然后导入包，不过包名改成了tf_transformations

import tf_transformations
 
# 将欧拉角转换为四元数（roll, pitch, yaw）
q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
 
# 将四元素转换成欧拉角 
euler = tf_transformations.euler_from_quaternion([x, y, z, w])

不安装tf_transformations的话，还可以安装其它工具包来替代，例如python的第三方quaternions包:

pip install quaternions  -i http://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.com

from quaternions import Quaternion as Quaternion 
euler = [0.98012,0.5232,0.2105] # ROLL PITCH HEADING
q = Quaternion.from_euler(euler, axes = ['y', 'y', 'x']) # a= (w, x, y, z)
e = Quaternion.get_euler(q) # e = (Heading, Pitch, Roll)

需要注意的是ROS在gemetry_msgs包里已定义有Quaternion类，另外ROS1里tf2里也定义有Quaternion，同时使用时注意用别名或包名区分。

另外，也可以安装trimesh工具包后使用里面的quaternion_from_euler

pip3.8 install trimesh -i http://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.com

from trimesh.transformations import quaternion_from_euler
from geometry_msgs.msg import Quaternion as GeoQuaternion
 
def createQuaternionFromYaw(yaw):
  q = quaternion_from_euler(0, 0, yaw)
  return GeoQuaternion(x=q[0],y=q[1],z=q[2],w=q[3])

看一下trimesh里/usr/local/lib/python3.8/dist-packages/trimesh/transformations.py里的quaternion_from_euler()的源码和/opt/ros/galactic/lib/python3.8/site-packages/tf_transformations/__init__.py里的quaternion_from_euler()代码，发现功能实现基本一致：

# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
 
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
    'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
    'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
    'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
    'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
    'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
    'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
    'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
    'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
 
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
 
...
 
def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
    """Return quaternion from Euler angles and axis sequence.
    ai, aj, ak : Euler's roll, pitch and yaw angles
    axes : One of 24 axis sequences as string or encoded tuple
    >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
    >>> np.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
    True
    """
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
    except (AttributeError, KeyError):
        _TUPLE2AXES[axes]  # validation
        firstaxis, parity, repetition, frame = axes
 
    i = firstaxis + 1
    j = _NEXT_AXIS[i + parity - 1] + 1
    k = _NEXT_AXIS[i - parity] + 1
 
    if frame:
        ai, ak = ak, ai
    if parity:
        aj = -aj
 
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci * ck
    cs = ci * sk
    sc = si * ck
    ss = si * sk
 
    q = np.empty((4, ))
    if repetition:
        q[0] = cj * (cc - ss)
        q[i] = cj * (cs + sc)
        q[j] = sj * (cc + ss)
        q[k] = sj * (cs - sc)
    else:
        q[0] = cj * cc + sj * ss
        q[i] = cj * sc - sj * cs
        q[j] = cj * ss + sj * cc
        q[k] = cj * cs - sj * sc
    if parity:
        q[j] *= -1.0
    return q
 
def euler_from_quaternion(quaternion, axes='sxyz'):
    """Return Euler angles from quaternion for specified axis sequence.
    >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
    >>> np.allclose(angles, [0.123, 0, 0])
    True
    """
    return euler_from_matrix(quaternion_matrix(quaternion), axes)

def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
    """
    Return quaternion from Euler angles and axis sequence.
    ai, aj, ak : Euler's roll, pitch and yaw angles
    axes : One of 24 axis sequences as string or encoded tuple
    >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
    >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953])
    True
    """
    return _reorder_output_quaternion(
        transforms3d.euler.euler2quat(ai, aj, ak, axes=axes)
    )
 
...
 
def euler_from_quaternion(quaternion, axes='sxyz'):
    """
    Return Euler angles from quaternion for specified axis sequence.
    >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
    >>> numpy.allclose(angles, [0.123, 0, 0])
    True
    """
    return euler_from_matrix(quaternion_matrix(quaternion), axes)

另外，ros2默认安装的spawn_entity代码/opt/ros/galactic/lib/python3.8/site-packages/scripts/spawn_entity.py里的提供了一个简单实现：

def quaternion_from_euler(roll, pitch, yaw):
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)
 
    q = [0] * 4
    q[0] = cy * cp * cr + sy * sp * sr  #w
    q[1] = cy * cp * sr - sy * sp * cr  #x
    q[2] = sy * cp * sr + cy * sp * cr  #y
    q[3] = sy * cp * cr - cy * sp * sr  #z
 
    return q

可以看到这个实现没有提供坐标轴顺序参数axes，按默认的sxyz来的。

pyquaternion包里的Quaternion只提供了使用q[w,x,y,z]创建实例以及使用matrix创建实例或使用由3维向量指定的旋转轴和旋转角度(弧度)创建实例，当使用nuScenes数据集时，里面涉及的3Dbox的roation都是pyquaternion.Quaternion类型的(标注文件sample_annotations.json的rotation的值就是[w,x,y,z]形式)，需要把3D检测模型输出的yaw角的弧度值转换成Quaternion后计算nuScenes指标时就需要使用上面的转换获得q[w,x,y,z]后，再直接使用Quaternion(q)创建实例即可。

那么axes参数值都有哪些呢？意思是什么，查了一下资料这里汇总一下：

根据三次基本转动选取的坐标轴的不同，欧拉角共有12种组合，再考虑到可选取原始坐标系的坐标轴，也可选取“新”坐标系的坐标轴，则共有24种欧拉角表示。一般规定原始坐标系为静坐标系(static)，每个基本转动后形成的新坐标系为动坐标系(rotating)。

24 种欧拉角表示列举如下：

• 静轴(即转轴选静坐标系的坐标轴):

sXYZ,sXZY,sXYX,sXYZ,sXZY,sXYX,sXYZ,sXZY,sXYX,
sXZX,sYXZ,sYZX,sXZX,sYXZ,sYZX,sXZX,sYXZ,sYZX,
sYXY,sYZY,sZXY,sYXY,sYZY,sZXY,sYXY,sYZY,sZXY,
sZYX,sZXZ,sZYZsZYX,sZXZ,sZYZsZYX,sZXZ,sZYZ

• 动轴(即转轴选动坐标系的坐标轴):

rZYX,rYZX,rXYX,rZYX,rYZX,rXYX,rZYX,rYZX,rXYX,
rXZX,rZXY,rXZY,rXZX,rZXY,rXZY,rXZX,rZXY,rXZY,
rYXY,rYZY,rYXZ,rYXY,rYZY,rYXZ,rYXY,rYZY,rYXZ,
rXYZ,rZXZ,rZYZrXYZ,rZXZ,rZYZrXYZ,rZXZ,rZYZ

静轴欧拉角和动轴欧拉角有如下规律:
     绕静轴 XYZXYZXYZ 分别 转 α,β,γα,β,γα,β,γ 角度的转动与绕动轴 ZYXZYXZYX分别转 γ,β,αγ,β,αγ,β,α 角度的转动等价，其他形式的欧拉角亦有此类似规律。

axes的值和firstaxis, parity, repetition, frame的含义解释：