ROS TF2 中的 四元数 基础部分
这篇博客主要讲解 ROS中四元数用法的基础知识。
1、四元数的组成
ROS使用四元数来跟踪和应用旋转。
一个四元素有4个成员（x,y,z,w）。
注意： w 是最后一个，但是一些库像 Eigen 把 w 放在第一的位置。
不绕x / y / z轴旋转的 常用单位 四元数为（0,0,0,1）：
在一个节点 加入下面 代码 setRPY() 会 把 绕 x、y、z轴旋转的角度 转化 为 四元数
设置不绕x / y / z轴旋转的setRPY( 0, 0, 0 ) 打印出四元数 则 为 （0,0,0,1）
#include <tf2/LinearMath/Quaternion.h>
...
tf2::Quaternion myQuaternion;
myQuaternion.setRPY( 0, 0, 0 ); // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f %f %f %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w()); // Print the quaternion components (0,0,0,1)
四元数的大小应为1。 如果数值错误导致四元数的大小不为1，ROS将显示警告。 为避免这些警告，需要对四元数进行标准化：
q.normalize();对于四元数 ROS 有两种 数据类型：msg 和 tf
msg geometry_msgs::Quaternion quat_msg 这种声明的就是 msg 类型
tf tf2::Quaternion myQuaternion 这种声明的就是 tf 类型
在 tf2_geometry_msgs.h 文件 中 提供 了 数据类型 转换的 函数
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;
tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);convert() 即 将 msg 类型 转换 为 tf 类型
fromMsg() 即 将 msg 类型 转换 为 tf 类型
toMsg() 即 将 tf 类型 转换 为 msg 类型
2、将 RPY坐标系 下的 角度 转换为 四元数
对于人来说，绕轴旋转很容易，直接写出四元数则很难。
在求 一个 物体 姿态的时候 可以 如下
先以（围绕X轴的滚动）/（围绕Y轴的后续俯仰）/（随后围绕Z轴的偏航）计算目标旋转，然后转换为四元数：
setRPY() 会 把 绕 x、y、z轴旋转的角度 转化 为 四元数 注意 形参 是 弧度
#include <tf2/LinearMath/Quaternion.h>
tf2::Quaternion myQuaternion;
myQuaternion.setRPY(1.5707, 0, -1.5707); // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f %f %f %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w()); // Print the quaternion components (0,0,0,1)结果如下
3、如何通过四元数 做 旋转
将一个姿态（用四元数表示） 做一个 旋转（用四元数表示） ，只需要将 姿态的四元数 乘以旋转的四元数。这个乘法的顺序很重。
例子
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
//q_orig 是原姿态转换的tf的四元数
//q_rot 旋转四元数
//q_new 旋转后的姿态四元数
tf2::Quaternion q_orig, q_rot, q_new;
// commanded_pose.pose.orientation 这个比如说 是 订阅的别的节点的topic 是一个 姿态的 msg 四元数
//通过tf2::convert() 转换成 tf 的四元数
tf2::convert(commanded_pose.pose.orientation , q_orig);
// 设置 绕 x 轴 旋转180度
double r=3.14159, p=0, y=0;
q_rot.setRPY(r, p, y);//求得 tf 的旋转四元数
q_new = q_rot*q_orig; // 通过 姿态的四元数 乘以旋转的四元数 即为 旋转 后的 四元数
q_new.normalize(); // 归一化
// 将 旋转后的 tf 四元数 转换 为 msg 四元数
tf2::convert(q_new, commanded_pose.pose.orientation);4、四元数转置
四元数 转置 直接 把 w 参数 就 负 即 可
5、求两个姿态（四元数）的旋转
假如在一个坐标系下有两个 姿态 用四元数 表示的q_1和q_2，那如何 求这两个姿态的旋转四元数q_r呢。
q_2 = q_r*q_1
可以类似于求解矩阵方程来求解q_r。 颠倒q_1并将两边右乘。 同样，乘法的顺序很重要：
q_r = q_2*q_1_inverse
举个例子
q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w //注意这个负号
q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w
qr = tf.transformations.quaternion_multiply(q2, q1_inv)
