ROS中欧拉角和四元数转换

欧拉角转四元数

#include <ros/ros.h>
#include <tf/tf.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "Euler2Quaternion");
  ros::NodeHandle node;
  geometry_msgs::Quaternion q;
  double roll=0.5,pitch=4.3,yaw=5.3;
  //输入欧拉角，转化成四元数在终端输出
  q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
  ROS_INFO("输出的四元数为：w=%d,x=%d,y=%d,z=%d"，q.w,q.x,q.y,q.z);
  return 0;
};

四元数转欧拉角

#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include <tf/tf.h>
int main(int argc, char** argv)
{
  ros::init(argc, argv, "Quaternion2Euler");
  ros::NodeHandle node;
  nav_msgs::Odometry position;
  tf::Quaternion RQ2;  
  double roll,pitch,yaw;
  sposition.pose.pose.orientation.w=0.012;
  position.pose.pose.orientation.x=0.076;
  position.pose.pose.orientation.y=0.012;
  position.pose.pose.orientation.z=0.098;
  //输入四元数，转化成欧拉角数在终端输出
  tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2); 
  tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);  
  ROS_INFO("输出的欧拉角为：roll=%f pitch=%%f yaw=%f",roll,pitch,yaw);
  return 0;
};