ROS 四元数和欧拉角互转例子

#include 
#include 
#include 
#include 
#include 

void publishOdom() {
  tf::Quaternion q;
  double roll, pitch, yaw;
  q.setRPY(roll, pitch, yaw);
  odom_msg.pose.pose.orientation.x = q.x();
  odom_msg.pose.pose.orientation.y = q.y();
  odom_msg.pose.pose.orientation.z = q.z();
  odom_msg.pose.pose.orientation.w = q.w();

   tf::Quaternion q2(odom_msg.pose.pose.orientation.x,
                         odom_msg.pose.pose.orientation.y,
                         odom_msg.pose.pose.orientation.z,
                         odom_msg.pose.pose.orientation.w);
   tf::Matrix3x3 m2(q2);
   m2.getRPY(roll, pitch, yaw);
}