将imu_data中获取到的四元数数据转换成欧拉角，并通过话题发布，使用rqt_plot工具查看yaw角是否存在零漂误差

1.如题，该贴是想记录本人在实习时编写的一些程序与代码，供大家参考。

2.我们先来看看通过imu得到imu_data话题中的数据类型，在头文件<sensor_msgs/Imu.h>中，声明了以下的数据类型：

 Imu_()
    : header()
    , orientation()
    , orientation_covariance()
    , angular_velocity()
    , angular_velocity_covariance()
    , linear_acceleration()
    , linear_acceleration_covariance()  {
      orientation_covariance.assign(0.0);

      angular_velocity_covariance.assign(0.0);

      linear_acceleration_covariance.assign(0.0);

其中较为重要的就是imu的四元数orientation()，四元数数据格式为（x,y,z,w），网络上某些帖子提供了四元数转换为欧拉角的方法，但没有具体解析，下面笔者阐述一下整个订阅imu数据、转换数据、再通过话题发布的流程。

3.先来看下完整代码：

#include <ros/ros.h>
#include "tf/transform_datatypes.h"//转换函数头文件
#include <sensor_msgs/Imu.h>//imu数据信息
#include <geometry_msgs/Point32.h>//geometry_msgs消息数据类型

double Pi = 3.14;
class Quat_to_angle
{
    public: 
     //创建需要的发布对象
     geometry_msgs::Point32 rpy_pt;

     //创建发布者
     ros::Publisher rpy_publisher;

     //创建订阅者，订阅IMU.orientation转换后的欧拉角信息
     ros::Subscriber rpy;

     //创建句柄节点
     ros::NodeHandle nh;
     
     //创建构造函数，在构造函数里给订阅者和发布者初始化
     Quat_to_angle(ros::NodeHandle nh)
     {      
       rpy_publisher = nh.advertise<geometry_msgs::Point32>("/pub",1000); 

       //在调用类内部的回调函数时，callback前要加入命名空间，并且要在后面加this指针，表示调用自己内部函数
       rpy = nh.subscribe<sensor_msgs::Imu>("/imu_data", 1000, &Quat_to_angle::ImuCallback, this);

     };
     
     //创建成员回调函数，实现具体转换功能
     void ImuCallback(const sensor_msgs::ImuConstPtr& imu)  
        {   //传入数据时，使用常量指针的形式，让数据不可改变，这样比较规范
        
            //定义一个四元数quadf
            tf::Quaternion quat;
            
            //把msg形式的四元数转化为tf形式,得到quat的tf形式
            tf::quaternionMsgToTF(imu->orientation, quat);

            //定义存储r\p\y的容器
            double roll, pitch, yaw;

            //进行转换得到RPY欧拉角
            tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
            
            //定义将要发布的欧拉角数据类型           
            rpy_pt.x = roll*180/ Pi;
            rpy_pt.y = pitch*180/ Pi;
            rpy_pt.z = yaw*180/ Pi;

            std::cout<<"roll="<<rpy_pt.x<<"\t pitch="<<rpy_pt.y<<"\t yaw="<<rpy_pt.z<<std::endl;

            rpy_publisher.publish(rpy_pt);  
        }       
};
int main(int argc, char **argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "quat_to_angle");
    //创建句柄节点
    ros::NodeHandle nh;
    //创建实例化对象q
    Quat_to_angle q(nh);
    // q.rpy_publisher = nh.advertise<geometry_msgs::Point32>("/pub",1000); 
    // //如果是在类外进行订阅，则需要将this，换为&q(实例化的对象)，表示调用q对象上的回调函数
    // q.rpy = nh.subscribe<sensor_msgs::Imu>("/imu_data", 1000, &Quat_to_angle::ImuCallback, &q);

    // 检查传入的四元数直到按下crtl+c
    ROS_INFO("waiting for quaternion");
    ros::spin();
    return 0;
}

4.接下来我们逐步分析整个流程，先来看main函数中做的事情，初始化ros节点，创建句柄，最重要的是创建了一个Quat_to_angle类的实例化对象q（对于类和对象不了解的同学可以再去看看c++的内容）,当实例化q后，我们便可以实现所有功能。

int main(int argc, char **argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "quat_to_angle");
    //创建句柄节点
    ros::NodeHandle nh;
    //创建实例化对象q
    Quat_to_angle q(nh);

    // q.rpy_publisher = nh.advertise<geometry_msgs::Point32>("/pub",1000); 
    // //如果是在类外进行订阅，则需要将this，换为&q(实例化的对象)，表示调用q对象上的回调函数
    // q.rpy = nh.subscribe<sensor_msgs::Imu>("/imu_data", 1000, &Quat_to_angle::ImuCallback, &q);

    // 检查传入的四元数直到按下crtl+c
    ROS_INFO("waiting for quaternion");
    ros::spin();
    return 0;
}

接下来我们回到定义的Quat_to_angle类，看类中都做了什么。

首先包含必要的头文件，这里包含<geometry_msgs/Point32.h>，因为我想发布这个数据类型的欧拉角。

#include <ros/ros.h>
#include "tf/transform_datatypes.h"//转换函数头文件
#include <sensor_msgs/Imu.h>//imu数据信息
#include <geometry_msgs/Point32.h>//geometry_msgs消息数据类型

然后创建类，定义成员属性

class Quat_to_angle
{
    public: 
     //创建需要的发布对象
     geometry_msgs::Point32 rpy_pt;

     //创建发布者
     ros::Publisher rpy_publisher;

     //创建订阅者，订阅IMU.orientation转换后的欧拉角信息
     ros::Subscriber rpy;

     //创建句柄节点
     ros::NodeHandle nh;

接着，在构造函数中给发布者和订阅者初始化，实现发布欧拉角和订阅IMU的四元数数据。

//创建构造函数，在构造函数里给订阅者和发布者初始化
     Quat_to_angle(ros::NodeHandle nh)
     {      
       rpy_publisher = nh.advertise<geometry_msgs::Point32>("/pub",1000); 

       //在调用类内部的回调函数时，callback前要加入命名空间，并且要在后面加this指针，表示调用自己内部函数
       rpy = nh.subscribe<sensor_msgs::Imu>("/imu_data", 1000, &Quat_to_angle::ImuCallback, this);

     };

接下来创建成员回调函数，实现四元数到欧拉角的转换过程，并发布出去。

//创建成员回调函数，实现具体转换功能
     void ImuCallback(const sensor_msgs::ImuConstPtr& imu)  
        {   //传入数据时，使用常量指针的形式，让数据不可改变，这样比较规范
        
            //定义一个四元数quadf
            tf::Quaternion quat;
            
            //把msg形式的四元数转化为tf形式,得到quat的tf形式
            tf::quaternionMsgToTF(imu->orientation, quat);

            //定义存储r\p\y的容器
            double roll, pitch, yaw;

            //进行转换得到RPY欧拉角
            tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
            
            //定义将要发布的欧拉角数据类型           
            rpy_pt.x = roll*180/ Pi;
            rpy_pt.y = pitch*180/ Pi;
            rpy_pt.z = yaw*180/ Pi;

            std::cout<<"roll="<<rpy_pt.x<<"\t pitch="<<rpy_pt.y<<"\t yaw="<<rpy_pt.z<<std::endl;

            rpy_publisher.publish(rpy_pt);  
        }

5.最后来看看实现的结果，打开rqt_plot工具，查看实际底盘or仿真环境下四元数转换称欧拉角后的信息，/pub是我创建的话题名，其下有x,y,z分别对应roll,pitch.yaw角，

另外也可以将类直接写进头文件中，在源程序只写一个main函数，直接实例化Quat_to_angle这个类便可以实现想要的功能。

到此结束，大家对于代码实现有问题的话可以直接评论。