ROS中欧拉角，四元数，角度，弧度换算

ROS

#include "tf/transform_datatypes.h"
#include <tf/tf.h>
#include <tf2/LinearMath/Matrix3x3.h> //tf2::getYaw转换头文件
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>

#include <iostream>
#include <math.h>
#include <algorithm>
#define PI acos(-1)

int main(int argc, char** argv) {
    ros::init(argc, argv, "send_goals_node1");

    double y=45*PI/180.0; //角度换弧度

    //yaw换四元数
    geometry_msgs::Quaternion q;
    q=tf::createQuaternionMsgFromYaw(y);
    std::cout<<"q:"<<q.w<<std::endl;

    //四元数换欧拉
    double roll,pitch,yaw;
    tf::Quaternion RQ2; 
    tf::quaternionMsgToTF(q,RQ2);
    tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);
    std::cout<<"输出的欧拉角为：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;

    double z;
    z=yaw*180/PI;//弧度换角度
    std::cout<<"z="<<z<<std::endl;

    //四元数换yaw
    double c;
    c=tf2::getYaw(q);
    std::cout<<"c="<<c<<std::endl;

    ros::spin();
    return 0;
}