WHEELTEC IMU-N100惯导模块之数据解析(二)

最新推荐文章于 2025-03-08 21:33:30 发布

匍匐的狗仔

最新推荐文章于 2025-03-08 21:33:30 发布

阅读量3.8k

点赞数 3

文章标签： ubuntu c++ linux 开发语言算法

本文链接：https://blog.youkuaiyun.com/er_dan_love/article/details/124794614

版权

本文介绍了如何在ROS环境下编写一个订阅者节点，从`imu_raw_data`话题中接收十六进制的IMU数据，进行解析并转化为自定义消息类型`imu_raw_data`发布到`imu_parsed_data`话题。解析过程包括从原始数据中提取角速度、加速度等信息，并通过`memcpy`函数将十六进制数据转换为浮点型数值。同时，文章建议读者尝试自己编写订阅者节点来验证解析结果。

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

前言

这篇文章是在串口通讯接收好数据之后对数据进行解析的过程，主要包括角速度、加速度等。直接开整，肯定有不足之处，大家共同讨论进步！

一、编写测试订阅者

1、编写imu_sub.cpp

这一部分我们主要是来介绍ros中编写订阅者，来订阅上一章ros发布的话题 /raw_imu_data ，并在回调函数中对订阅到的消息作相应的处理。直接上代码(很简单)。

在功能包的 src 下面添加 imu_sub.cpp ，内容如下。

因为逻辑很简单所以代码很少读起来也很容易，做一下简单的介绍。IMUSIZE是说一帧imu数据有64个十六进制数组成。HandleRawData 是回调函数，只要有imu数据就会到这个回调函数中进行处理，具体的处理过程也很简单，就是以十六进制数的格式输出了一下imu数据。

#include<ros/ros.h>
#include<serial/serial.h>
#include<std_msgs/Int8MultiArray.h>
#include<std_msgs/UInt8MultiArray.h>
#include<std_msgs/MultiArrayDimension.h>
#include<sensor_msgs/Imu.h>
#include</home/zjs/imu_data_solution/devel/include/imu_com/imu_raw_data.h>
#define IMUSIZE 64
#define IMUFRAMEHEAD 56
#define IMUFORMAT 57

void HandleRawData(const std_msgs::UInt8MultiArrayConstPtr& raw_data)
{
    uint8_t raw_data_arr[IMUSIZE]={0x00};
    for(int i=0;/*(i<IMUSIZE&&((raw_data->data[IMUFRAMEHEAD]&0xff)==0xfc)&&((raw_data->data[IMUFORMAT]&0xff)==0xf40))*/i<IMUSIZE;i++)
    {
        if(((raw_data->data[IMUFRAMEHEAD]&0xff)==0xfc)&&((raw_data->data[IMUFRAMEHEAD+1]&0xff)==0x40))
        {
            raw_data_arr[i]=raw_data->data[IMUFRAMEHEAD+i]; 
        }
        std::cout<<std::hex<<(raw_data->data[IMUFRAMEHEAD+i]&0xff)<<" ";
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"imu_subscriber");
    ros::NodeHandle nh;
    ros::Subscriber imu_sub=nh.subscribe<std_msgs::UInt8MultiArray>("imu_raw_data",1000,HandleRawData);
    pub_imu_data=nh.advertise<imu_com::imu_raw_data>("imu_parsed_data",1000);
    ros::spin();
    return 0;
}

2、编写CMakeLists.txt文件

add_executable(imu_sub src/imu_sub.cpp)
target_link_libraries(imu_sub ${catkin_LIBRARIES})

3、编写launch文件

在功能包中新建一个名为 launch 的文件夹并创建一个 imu_sub.launch 的文件，文件内容为

<launch>
    <node pkg="imu_test" type="imu_sub" name="imu_sub" output="screen">
    </node>
</launch>

4、运行过程

cd ~/imu_data_test
catkin_make
source devel/setup.sh
roslaunch imu_test imu_sub.launch

5、测试结果

同样以十六进制的方式打印，可以看出来是和我们在上一章介绍串口通讯时所打印的结果是一样的(形式上，数据肯定是不一样的)。

二、编写订阅者

1.添加自己的消息数据类型

在功能包下面添加名为 msg 的文件夹，在该文件夹下添加 filename.msg 文件，这里自己命名，我的是 imu_raw_data.msg 文件，这里面定义了自己的 imu 的数据类型。如下：

float32 Gyroscope_X
float32 Gyroscope_Y
float32 Gyroscope_Z
float32 Accelerometer_X
float32 Accelerometer_Y
float32 Accelerometer_Z
float32 Magnetometer_X
float32 Magnetometer_Y
float32 Magnetometer_Z
float32 IMU_Temperature
float32 Pressure
float32 Pressure_Temperature
int64 Timestamp

小伙伴们要根据自己的imu的数据类型来书写自己的 .msg 文件奥，下面这个是我的imu数据类型：

在写好自己的 .msg 文件之后，还需要修改一些参数来让其生成c++代码。打开 package.xml 文件，并添加如下语句：

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

打开CMakeLists.txt文件，添加如下部分的CMake语句：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  serial
  std_msgs
  message_generation #添加的部分
)

在 find_package 中添加 message_generation 。

add_message_files(
  FILES
  imu_raw_data.msg
)

添加自己定义的消息类型文件，注意这些位置在文件中都可以找到！

generate_messages(
  DEPENDENCIES
  std_msgs
)

因为自己的.msg 中的数据类型都在std_msgs中，所以之添加这一个依赖就可以了。

catkin_package(
  CATKIN_DEPENDS roscpp serial std_msgs message_runtime
)

打开catkin_package，并加入 roscpp serial std_msgs message_runtime。

之后回到根目录下进行编译，就可以生成相关的消息类型的文件了，生成imu_raw_data.h文件。

cd ~/imu_data_test
catkin_make
#生成的相关文件存放在了 imu_data_test/devel/include/imu_test 下

2.修改订阅者

1）添加相关函数

第一个函数是把十六进制数转换为 32位float数据类型。第二个是把十六进制数转换为64位int数据类型。(网址介绍的是memcpy()函数的功能以及使用)。

float R4(uint8_t* p)
{
    float r=0.0;
    memcpy(&r,p,4);
    //https://www.runoob.com/cprogramming/c-function-memcpy.html
    return r;
}
int R8(uint8_t *p)
{
    int r=0;
    memcpy(&r,p,8);
    return r;
}

2）数据解算部分

这部分相信大家都可以看懂，调用函数R4()其中传入的参数是该数据的起始位置。举个例子，拿x轴的加速度来说，传入的参数 raw_data_arr 是数组的起始位置，7是非数据区，占7个字节，12是之前的数据所占的字节数，也就是x方向加速度数据的起始位置。这些在手册中都可以查找到。

这里的 imu_data 是我们自己定义的数据类型。在解算完成之后，又将解算好的数据发布出去(虽然有多此一举的感觉，但是我觉得后面用起来会很方便哈哈哈哈)。这也是一个在订阅者中发布话题的小 tip ，定义一个全局变量，然后调用，具体看整体的函数。

 imu_com::imu_raw_data imu_data;    
 if((raw_data_arr[0]&0xff)==0xfc&&(raw_data_arr[1]&0xff)==0x40&&(raw_data_arr[IMUSIZE-1]&0xff)==0xfd)
    {
        imu_data.Gyroscope_X=R4(raw_data_arr+0+7);
        imu_data.Gyroscope_Y=R4(raw_data_arr+4+7);
        imu_data.Gyroscope_Z=R4(raw_data_arr+8+7);
        imu_data.Accelerometer_X=R4(raw_data_arr+12+7);
        imu_data.Accelerometer_Y=R4(raw_data_arr+16+7);
        imu_data.Accelerometer_Z=R4(raw_data_arr+20+7);
        imu_data.Magnetometer_X=R4(raw_data_arr+24+7);
        imu_data.Magnetometer_Y=R4(raw_data_arr+28+7);
        imu_data.Magnetometer_Z=R4(raw_data_arr+32+7);
        imu_data.IMU_Temperature=R4(raw_data_arr+36+7);
        imu_data.Pressure=R4(raw_data_arr+40+7);
        imu_data.Pressure_Temperature=R4(raw_data_arr+44+7);
        imu_data.Timestamp=R4(raw_data_arr+48+7);
        pub_imu_data.publish(imu_data);
    }

3）总体函数

有了上面的讲解，这部分应该比较好读懂，就不具体解释了。

#include<ros/ros.h>
#include<serial/serial.h>
#include<std_msgs/Int8MultiArray.h>
#include<std_msgs/UInt8MultiArray.h>
#include<std_msgs/MultiArrayDimension.h>
#include<sensor_msgs/Imu.h>
#include</home/zjs/imu_data_solution/devel/include/imu_com/imu_raw_data.h>
#define IMUSIZE 64
#define IMUFRAMEHEAD 56
#define IMUFORMAT 57
float R4(uint8_t* p)
{
    float r=0.0;
    memcpy(&r,p,4);
    //https://www.runoob.com/cprogramming/c-function-memcpy.html
    return r;
}
int R8(uint8_t *p)
{
    int r=0;
    memcpy(&r,p,8);
    return r;
}
ros::Publisher pub_imu_data;

void HandleRawData(const std_msgs::UInt8MultiArrayConstPtr& raw_data)
{
    // std::cout<<"raw data's size is "<<raw_data->data.size()<<std::endl;
    // for(int i=0;i<raw_data->data.size();i++)
    // {
    //     std::cout<<std::hex<<(raw_data->data[i]&0xff)<<" ";
    //     if((raw_data->data[i]&0xff==0xfd)&&(raw_data->data[i+1]&0xff==0xfc))
    //     {
    //         std::cout<<std::endl;
    //         std::cout<<std::endl;
    //     }
    // }
    // std::cout<<std::endl;
    uint8_t raw_data_arr[IMUSIZE]={0x00};
    for(int i=0;/*(i<IMUSIZE&&((raw_data->data[IMUFRAMEHEAD]&0xff)==0xfc)&&((raw_data->data[IMUFORMAT]&0xff)==0xf40))*/i<IMUSIZE;i++)
    {
        if(((raw_data->data[IMUFRAMEHEAD]&0xff)==0xfc)&&((raw_data->data[IMUFRAMEHEAD+1]&0xff)==0x40))
        {
            raw_data_arr[i]=raw_data->data[IMUFRAMEHEAD+i]; 
        }
        //std::cout<<std::hex<<(raw_data->data[IMUFRAMEHEAD+i]&0xff)<<" ";
    }
    //std::cout<<std::endl;
    // std::cout<<"this is an arry copyed."<<std::endl;
    // int size=0;
    // for(int i=0;i<IMUSIZE;i++)
    // {
    //     size++;
    //     //std::cout<<std::hex<<(raw_data_arr[i]&0xff)<<" ";
    // }
    // std::cout<<std::endl;
    // std::cout<<"*****************************************************"<<std::endl;

    imu_com::imu_raw_data imu_data;
    // if((raw_data_arr[0]&0xff)==0xfc&&(raw_data_arr[1]&0xff)==0x40&&(raw_data_arr[IMUSIZE-1]&0xff)==0xfd)
    // {
    //     imu_data.Gyroscope_X=R4(raw_data_arr+0+4+7);
    //     imu_data.Gyroscope_Y=R4(raw_data_arr+4+4+7);
    //     imu_data.Gyroscope_Z=R4(raw_data_arr+8+4+7);
    //     imu_data.Accelerometer_X=R4(raw_data_arr+12+4+7);
    //     imu_data.Accelerometer_Y=R4(raw_data_arr+16+4+7);
    //     imu_data.Accelerometer_Z=R4(raw_data_arr+20+4+7);
    //     imu_data.Magnetometer_X=R4(raw_data_arr+24+4+7);
    //     imu_data.Magnetometer_Y=R4(raw_data_arr+28+4+7);
    //     imu_data.Magnetometer_Z=R4(raw_data_arr+32+4+7);
    //     imu_data.IMU_Temperature=R4(raw_data_arr+36+4+7);
    //     imu_data.Pressure=R4(raw_data_arr+40+4+7);
    //     imu_data.Pressure_Temperature=R4(raw_data_arr+44+4+7);
    //     //imu_data.Timestamp=R4(raw_data_arr+48+8+7);
    //     pub_imu_data.publish(imu_data);
    // }
        if((raw_data_arr[0]&0xff)==0xfc&&(raw_data_arr[1]&0xff)==0x40&&(raw_data_arr[IMUSIZE-1]&0xff)==0xfd)
    {
        imu_data.Gyroscope_X=R4(raw_data_arr+0+7);
        imu_data.Gyroscope_Y=R4(raw_data_arr+4+7);
        imu_data.Gyroscope_Z=R4(raw_data_arr+8+7);
        imu_data.Accelerometer_X=R4(raw_data_arr+12+7);
        imu_data.Accelerometer_Y=R4(raw_data_arr+16+7);
        imu_data.Accelerometer_Z=R4(raw_data_arr+20+7);
        imu_data.Magnetometer_X=R4(raw_data_arr+24+7);
        imu_data.Magnetometer_Y=R4(raw_data_arr+28+7);
        imu_data.Magnetometer_Z=R4(raw_data_arr+32+7);
        imu_data.IMU_Temperature=R4(raw_data_arr+36+7);
        imu_data.Pressure=R4(raw_data_arr+40+7);
        imu_data.Pressure_Temperature=R4(raw_data_arr+44+7);
        imu_data.Timestamp=R4(raw_data_arr+48+7);
        pub_imu_data.publish(imu_data);
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"imu_subscriber");
    ros::NodeHandle nh;
    ros::Subscriber imu_sub=nh.subscribe<std_msgs::UInt8MultiArray>("imu_raw_data",1000,HandleRawData);
    pub_imu_data=nh.advertise<imu_com::imu_raw_data>("imu_parsed_data",1000);
    ros::spin();
    return 0;
}