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);