欧拉角转四元数
#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;
};