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

pomelo丶a

已于 2022-03-18 11:49:27 修改

阅读量4k

点赞数 6

CC 4.0 BY-SA版权

文章标签： c++

于 2022-03-16 14:17:26 首次发布

本文链接：https://blog.csdn.net/ws107845f/article/details/123524726

本文档介绍了一个ROS节点的实现，该节点订阅IMU传感器的四元数数据，将其转换为欧拉角，并通过自定义话题发布。主要涉及ROS节点创建、四元数到欧拉角的转换、数据发布和订阅。代码中使用了`tf/transform_datatypes.h`库进行转换，并展示了如何在类中定义回调函数处理数据。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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_publis