ros中有关欧拉角和四元数互转的C++写法

1. 四元数转欧拉角

包含的头文件：
#include “tf2/LinearMath/Matrix3x3.h”

tf2::Matrix3x3 mat(tf2::Quaternion( Imu_msg.orientation.x, Imu_msg.orientation.y, Imu_msg.orientation.z, Imu_msg.orientation.w ));
mat.getEulerYPR(robotAngle.z, robotAngle.x, robotAngle.y); // yaw pitch roll

2.偏航角转四元数

geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);