使用WTGAHRS2(JY-GPSIMU)在ROS中读取数据并发布话题

IMU简介

 十轴惯性导航传感器WTGAHRS2传感器集成高精度的陀螺仪、加速度计、地磁场传感器、GPS 模块，采用高性能的微处理器和先进的动力学解算与卡尔曼动态滤波算法，能够快速求解出模块当前的实时运动姿态。
 采用先进的数字滤波技术，能有效降低测量噪声，提高测量精度。传感器内部集成了姿态解算器，配合动态卡尔曼滤波算法，能够在动态环境下准确输出模块的当前姿态，姿态测量精度静态 0.05 度，动态 0.1 度，稳定性极高，性能甚至优于某些专业的倾角仪！
内部自带电压稳定电路，工作电压 3.3v~5v，引脚电平兼容 3.3V/5V 的嵌入式系
统，连接方便。支持串口 TTL/232，方便用户选择最佳的连接方式。串口速率 2400bps~921600bps
可调。GPS 信息、姿态传感器信息同步输出。最高 200Hz 数据输出速率。输入内容可以任意选择，输出速率 0.1～200HZ 可调节。
资料下载：https://pan.baidu.com/s/1kUxka08OW2x1Ej-7jvEgWw
验证码：0eug
官网：http://wit-motion.cn/guandaodaohang/26.html

驱动程序

在ROS中的驱动是以驱动文件的形式存在的，需要我们自己编写驱动程序。在这里我们参考https://blog.csdn.net/qqliuzhitong/article/details/114384297文章编写自己的驱动程序。

IMU串口通信协议

IMU使用的是JY901芯片，在官方说明文档里可以找到串口通信协议

程序

关于自定义话题及ros话题发布接受机制请参考：https://blog.csdn.net/qq_43569735/article/details/108065179
串口程序+话题发布

//Created by ljm
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "agv_imu/imu_data.h"
int main(int argc, char** argv)
{
    ros::init(argc, argv, "agv_imu");
    ros::NodeHandle n;
    //创建imu数据话题发布
    ros::Publisher pub_imu_data;
    pub_imu_data = n.advertise<agv_imu::imu_data>("AGV/imu",50);

    //创建一个serial对象
    serial::Serial sp;
    //创建timeout
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    //设置要打开的串口名称
    sp.setPort("/dev/ttyUSB0");
    //设置串口通信的波特率
    sp.setBaudrate(9600);
    //串口设置timeout
    sp.setTimeout(to);

    double ax,ay,az;
    double wx,wy,wz;
    double angle_roll,angle_pitch,angle_yaw;
    
    agv_imu::imu_data msg;
    
    try
    {
        //打开串口
        sp.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }

    //判断串口是否打开成功
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return -1;
    }

    ros::Rate loop_rate(100);
    while(ros::ok())
    {
        //获取缓冲区内的字节数
        size_t n = sp.available();
        if(n!=0)
        {
            uint8_t buffer[1024];
            //读出数据
            n = sp.read(buffer, n);

            for(int i=0; i<n; i++)
            {
                if(buffer[i]==0X55)            //IMU地址
                {
                        if(buffer[i+1]==0x51) //加速度帧
                        {
                            signed short int temp;
                            temp=buffer[i+3];
                            temp=temp<<8;
                            temp=(temp|buffer[i+2]);
                            ax=(temp/32768.0)*16*9.8051;
                            temp=buffer[i+5];         //加速度八位
                            temp=temp<<8;
                            temp=(temp|buffer[i+4]);  //加速度高八位
                            ay=(temp/32768.0)*16*9.8051;
                            temp=buffer[i+7];         //加速度低八位
                            temp=temp<<8;
                            temp=(temp|buffer[i+6]);  //加速度高八位
                            az=(temp/32768.0)*16*9.8051;
                            std::cout<<"ax:"<<ax<<std::endl;
                            std::cout<<"ay:"<<ay<<std::endl;
                            std::cout<<"az:"<<az<<std::endl;
                            i+=11;
                        }
                        if(buffer[i+1]==0x52) //角速度帧
                        {
                            signed short int temp;
                            temp=buffer[i+3];
                            temp=temp<<8;
                            temp=(temp|buffer[i+2]);
                            wx=(temp/32768.0)*2000;
                            temp=buffer[i+5];         //角速度低八位
                            temp=temp<<8;
                            temp=(temp|buffer[i+4]);  //角速度高八位
                            wy=(temp/32768.0)*2000;
                            temp=buffer[i+7];         //角速度低八位
                            temp=temp<<8;
                            temp=(temp|buffer[i+6]);  //角速度高八位
                            wz=(temp/32768.0)*2000;
                            std::cout<<"wx:"<<wx<<std::endl;
                            std::cout<<"wy:"<<wy<<std::endl;
                            std::cout<<"wz:"<<wz<<std::endl;
                            i+=11;
                        }
                     if(buffer[i+1]==0x53) //角度帧
                    {
                        unsigned short int temp;
                        temp=buffer[i+3];         //角度低八位
                        temp=temp<<8;
                        temp=(temp|buffer[i+2]);  //角度高八位
                        angle_roll=(temp/32768.0)*180;
                        temp=buffer[i+5];         //角度低八位
                        temp=temp<<8;
                        temp=(temp|buffer[i+4]);  //角度高八位
                        angle_pitch=(temp/32768.0)*180;
                        temp=buffer[i+7];         //角度低八位
                        temp=temp<<8;
                        temp=(temp|buffer[i+6]);  //角度高八位
                        angle_yaw=(temp/32768.0)*180;
                        std::cout<<"angle_roll:"<<angle_roll<<std::endl;
                        std::cout<<"angle_pitch:"<<angle_pitch<<std::endl;
                        std::cout<<"angle_yaw:"<<angle_yaw<<std::endl;
                        i+=11;
                    }
                     msg.angx=angle_roll;
                     msg.angy=angle_pitch;
                     msg.angz=angle_yaw;
                     msg.ax=ax;
                     msg.ay=ay;
                     msg.az=az;
                     msg.wx=wx;
                     msg.wy=wy;
                     msg.wz=wz;
                     pub_imu_data.publish(msg);
                }
            }
            std::cout << std::endl;
            //sp.write(buffer, n);
        }
        loop_rate.sleep();
    }

    //关闭串口
    sp.close();

    return 0;
}

自定义消息话题：imu_data.msg

float64 ax
float64 ay
float64 az
float64 wx
float64 wy
float64 wz
float64 angx
float64 angy
float64 angz

效果