ROS中的四元数和欧拉角转换实例（转载）

链接: 转载博客.

#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::Quaternion q;
q = tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
 
 
tf::createQuaternionMsgFromYaw(double y);//只通过y即绕z的旋转角度计算四元数，用于平面小
 
//调用回调函数将四元数转RPY欧拉角
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    quat_subscriber = n.subscribe<nav_msgs::Odometry>("Odometry", 1000, odomCallback);
 
    // check for incoming quaternions untill ctrl+c is pressed
    ROS_INFO("waiting for quaternion");
    ros::spin();
    return 0;
}

定位系统中，ros系统通常使用四元数表示里程计（imu）旋转的信息，但是很多时候需要rpy表示更加直观方便，因此在这里记录一下转换方法。