ROS2 使用 tf2的过程的四元数变换，需要通过python的第三方包进行四元数旋转关系转换

在机器人系统中，经常需要用到坐标转换，比如一个机器人的手臂要去拿一个运动的物体，控制的时候我们需要手臂的当前坐标，同时还需要知道手臂和身体的坐标，这个时候就需要用到坐标转换，把手臂的坐标转换为身体的坐标系，这样控制起来就方便一点。当然这里是动态的情况，也存在静态的情况，比如机器人头顶的摄像头，转换到身体的坐标系，那么位置关系是相对固定的，所以可以一开始就写到固定的位置。

• 有固定转换关系的文件放在哪里？个比较好的方法是各个节点自己广播自己的转换关系，而其实静态的转换关系只需要发送一次就可以了，因为不会变化。
• 有动态转换关系的节点，需要实时动态发布自己的转换关系，这样会涉及到时间戳，以及过时。
• 转换关系的拓扑结构如何确定？是树型还是网络型的，这涉及到转换关系传递的问题。

安装

sudo apt install ros-dashing-tf2
sudo apt install ros-dashing-tf2-ros

Demo 实例：
https://github.com/ros2/geometry2/tree/ros2/examples_tf2_py/examples_tf2_py

我用的ros2 dashing 发行版，感觉一直无法导入tf2_ros 最后的方法是重新安装了更新的版本eloquent，安装过程严格对照官网教程进行即可。

在ROS1里面，

# tf.transformations alternative is not yet available in tf2
from tf.transformations import quaternion_from_euler
if __name__ == '__main__':
# RPY to convert: 90deg, 0, -90deg
	q = quaternion_from_euler(1.5707, 0, -1.5707)
	print "The quaternion representation is %s %s %s %s." % (q[0], q[1], q[2], q[3])

quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
#type(pose) = geometry_msgs.msg.Pose
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]

#type(pose) = geometry_msgs.msg.Pose
quaternion = (
    pose.orientation.x,
    pose.orientation.y,
    pose.orientation.z,
    pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]

在ROS2里面，目前四元数的转换没有集成到tf2_ros里面，四元数的定义放在了gemetry_msgs里
使用python的第三方quaternions包进行四元数转换。

在Python中进行转换的方法：

pip install quaternions

from quaternions import Quaternion as Quaternion # 用于求解欧拉角到四元数的变换结果
b = [0.21467549,0.32245,0.44535] # ROLL PITCH HEADING
a = Quaternion.from_euler(b,axes = ['y', 'y', 'x']) # 返回的是 w x y z
print(a)
c = Quaternion.get_euler(a) # 返回的结果是 Heading Pitch Roll
c = c[::-1]
print(c)

输出结果：
<0.9609193895916198, 0.19994791727967973, 0.17901508090292767, 0.06788488706409596>
[0.21467548999999994, 0.32245000000000007, 0.4453499999999999]

还有另外的方法：

from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx',[-1 * self.heading_radians, self.pitch_radians, self.roll_radians])
orientation = r.as_quat()

这个和C++版本的tf2里的变换顺序是保持一致的。