ROS中quaternion四元數和歐拉角轉換

在移動小車導航中，ros系統通常使用四元數錶示裏程計orientation信息，但是很多時候需要歐拉角錶示更加直觀方便，因此在這裏記錄一下轉換方法。

include "tf/transform_datatypes.h"//轉換函數頭文件
#include <nav_msgs/Odometry.h>//裏程計信息格式

/****************四元數轉RPY歐拉角，以odomsub的回調函數為例*****************/
void odomCallback(const nav_msgs::Odometry &odom) {
    
    
      tf::Quaternion quat;
      tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
     
      double roll, pitch, yaw;//定義存儲r\p\y的容器
      tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //進行轉換

    }

/****************RPY歐拉角轉四元數*****************/
tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元數
tf::createQuaternionMsgFromYaw(double y);//只通過y即繞z的旋轉角度計算四元數，用於平面小車。返回四元數