四元数转欧拉角函数

void getEulerYPR(const geometry_msgs::Quaternion &q, double &yaw, double &pitch, double &roll)
{
    double sqw;
    double sqx;
    double sqy;
    double sqz;

    sqx = q.x * q.x;
    sqy = q.y * q.y;
    sqz = q.z * q.z;
    sqw = q.w * q.w;

    // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
    double sarg = -2 * (q.x * q.z - q.w * q.y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
    if (sarg <= -0.99999)
    {
        pitch = -0.5 * M_PI;
        roll = 0;
        yaw = -2 * atan2(q.y, q.x);
    }
    else if (sarg >= 0.99999)
    {
        pitch = 0.5 * M_PI;
        roll = 0;
        yaw = 2 * atan2(q.y, q.x);
    }
    else
    {
        pitch = asin(sarg);
        roll = atan2(2 * (q.y * q.z + q.w * q.x), sqw - sqx - sqy + sqz);
        yaw = atan2(2 * (q.x * q.y + q.w * q.z), sqw + sqx - sqy - sqz);
    }
};