geometry_msgs::Quaternion q转化为欧拉角

将一个四元数（geometry_msgs::Quaternion）转换为欧拉角需要使用一些数学知识和函数。以下是一个简单的C++代码示例，可以将一个四元数转换为对应的欧拉角：

#include <ros/ros.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "quaternion_to_euler");
    ros::NodeHandle nh;

    // 定义一个四元数
    geometry_msgs::Quaternion q;
    q.x = 0.5;
    q.y = 0.5;
    q.z = 0.5;
    q.w = 0.5;

    // 将四元数转换为欧拉角
    double roll, pitch, yaw;
    tf::Quaternion tf_q(q.x, q.y, q.z, q.w);
    tf::Matrix3x3(tf_q).getRPY(roll, pitch, yaw);

    ROS_INFO("Quaternion: (%f, %f, %f, %f)", q.x, q.y, q.z, q.w);
    ROS_INFO("Euler angles: (%f, %f, %f)", roll, pitch, yaw);

    return 0;
}

其中，`tf::Quaternion` 是 ROS 中的一个四元数类，`tf::Matrix3x3` 是一个旋转矩阵类，`getRPY` 函数可以将旋转矩阵转换为欧拉角。在输出中，`roll`、`pitch`、`yaw` 分别代表绕 x、y、z 轴的旋转角度。