怎么在ROS中使用四元数旋转

https://quaternions.online/
四元数转欧拉角可视化

#include <tf/tf.h>

/*

q1 = w + xi + yj + zk

q1 * q2 =   (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
            (w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
            (w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
            (w1*z2 + x1*y2 - y1*x2 + z1*w2) k

i_2 = j_2 = k_2 = ijk = -1

x    1(w)  i   j   k
1(w)  1    i   j   k
i     i   -1   k  -j
j     j   -k  -1   i
k     k    j  -i   -1

*/

void test1(){
    // 求绕x or y or z轴旋转的四元数表示。

    tf::Vector3 v1(0,0,1);
    tf::Quaternion q1(0,0,0,1); //x,y,z,w
    q1.setRotation(v1, M_PI/2);
    std::cout<<std::setprecision(21)<<"从初始位(0,0,0,1)绕z轴旋转90度，转换到四元数q1: "<<"("<<q1[0]<<","<<q1[1]<<","<<q1[2]<<","<<q1[3]<<")"<<std::endl;
}

void test2(){
    tf::Quaternion q_x(-0.707,0,0,0.707);
    tf::Quaternion q_y(0,-0.707,0,0.707);
    tf::Quaternion q_z(0,0,-0.707,0.707);

    tf::Quaternion q2(0.5,0.5,0.5,0.5);
    tf::Quaternion q_t;
    q_t = q_y * q_x * q2;
    std::cout<<std::setprecision(21)<<"将q2(即从初始位绕x，y逆时针旋转90度)，再转换到初始位: "<<"("<<q_t[0]<<","<<q_t[1]<<","<<q_t[2]<<","<<q_t[3]<<")"<<std::endl;
}


int main(){
    test1();
    test2();
}