ROS：欧拉角转四元数、四元数转欧拉角

一.欧拉角转四元数

需要注意：有两种四元数类型，如下：

//初始化四元数
geometry_msgs::Quaternion q;//初始化四元数（geometry_msgs类型）
tf::Quaternion q1;//初始化四元数（tf类型）

可以通过ctrl点击Quaternion变量查看对应类型，在使用过程中一定要看变量是对应哪一种的Quaternion，否则会报错。

欧拉角转四元数转换代码：

#include 
#include 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "QuaternionToRPY");
    ros::NodeHandle node;
    double roll = 0.1, pitch = 0.2, yaw = 0.3; // 初始化欧拉角
    geometry_msgs::Quaternion q;//初始化四元数（geometry_msgs类型）
    tf::Quaternion q1;初始化四元数（tf类型）

    q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);//欧拉角转四元数（geometry_msgs::Quaternion）
    q1=tf::createQuaternionFromRPY(roll,pitch,yaw);欧拉角转四元数（tf::Quaternion）
    ROS_INFO("w=%.2f, x=%.2f, y=%.2f, z=%.2f", q.w,q.x,q.y,q.z);
    //tf::Quaternion不可以直接输出，因为类型不是double
    // ROS_INFO("w1=%.2f, x1=%.2f, y1=%.2f, z1=%.2f", q1.w,q1.x,q1.y,q1.z);
}

  如何相应使用对应方法：

二.四元数转欧拉角

#include 
#include 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "QuaternionToRPY");
    ros::NodeHandle node;
    
    double roll = 0.0, pitch = 0.0, yaw = 0.0; // 初始化欧拉角
    tf::Quaternion q(0.1, 0.2, 0.3, 0.4);      // 初始化四元数
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);// 四元数转欧拉角
    ROS_INFO("roll=%.2f, pitch=%.2f, yaw=%.2f", roll, pitch, yaw);
}