ROS四元数、欧拉角互转

#include
#include //amcl_pose
#include //转换函数头文件
#include 

class RobotMove
{
public:
    RobotMove();
    void poseCallBack(geometry_msgs::PoseWithCovarianceStampedConstPtr &pose);

private:
    ros::NodeHandle n;
    ros::Subscriber pose_sub; //订阅amcl位置
    float pose_x,pose_y,pose_z;
};
//构造函数
RobotMove::RobotMove()
{  
	pose_sub=n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose",10,&RobotMove::poseCallBack,this);
}

//位置回调函数
void RobotMove::poseCallBack(geometry_msgs::PoseWithCovarianceStampedConstPtr &pose)
{
    //四元数转欧拉角
    tf::Quaternion quat; //定义一个四元数
    tf::quaternionMsgToTF(pose->pose.pose.orientation,quat); //取出方向存储于四元数
 
    double roll,pitch,yaw; //定义存储r\p\y的容器
    tf::Matrix3x3(quat).getRPY(roll,pitch,yaw); //进行转换
    pose_x=roll;
    pose_y=pitch;
    pose_z=yaw;

	//欧拉角转四元数,类型可以不同
	geometry_msgs::Quaternion quaternion;//定义四元数
	quaternion=tf::createQuaternionMsgFromRollPitchYaw(0,0,0); //欧拉角
}
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "robotMoveAction");
    RobotMove robotmove;
    ros::spin();
    return 0;
}