【ROS2】获取imu数据并可视化保姆级教程(C++)_imu可视化，2024年最新Linux运维面试题2024中高级

串口号后续我们需要使用，一般来说是返回/dev/ttyUSB0

2.安装ros2-serial

这个是串口字符数据转发到标准ROS2通信的库，安装步骤如下：

git clone https://github.com/RoverRobotics-forks/serial-ros2.git
cd serial-ros2
mkdir build
cd build
cmake ..
make
sudo make install

3.创建工作空间

先创建文件架，编译一个空的工作空间：

mkdir -p ~/imu_test_ws/src
cd ~/imu_test_ws
colcon build

再创建一个基于c++功能包，用于解析和读取imu数据：

ros2 pkg create --build-type ament_cmake imu_get

在imu_get功能包下的src文件夹里面创建下面三个文件后续在里面编写代码：

sudo gedit transform.hpp
sudo gedit publisher_imu.cpp
sudo gedit sub_imu.cpp

transform.hpp文件用于解析从imu受到的原始数据，publisher_imu.cpp用于发布imu数据的相关话题，sub_imu.cpp用于订阅话题。

接下来，我们就要编写三个文件里面的代码了。

4.编写transform.hpp

在coding之前，我们需要看一下imu的协议说明，大部分imu的协议都是大同小异的，只要能看明白一个型号的imu协议，其他型号也大差不差。我的imu协议说明如下：

这里以加速度为例，角度度和角度同理。一个完整的数据帧有11个字节，我们首先要读到协议头，以确保我们获取到一段完整的数据帧，然后识别数据类型，最后在根据协议给出的说明获取需要的数据，温度数据我这里没有用到，所以只对加速度数据的六个字节进行位运算。transform.hpp的代码如下：

#include <string>
#include <ctype.h>
#include <float.h>
#include <math.h>
class transform\_imu
{
private:
    char \*stcTime;
    char \*stcAcc;
    char \*stcGyro;
    char \*stcAngle;

public:
    struct Acc
    {
        double x;
        double y;
        double z;
    }acc{0,0,0};
    struct Gyro
    {
        double x;
        double y;
        double z;
    }gyro{0,0,0};
    struct Angle
    {
        double r;
        double p;
        double y;
    }angle{0,0,0};
    
/\*public:
 transform\_imu(double,double,double);
 //构造函数,也可尝试用初始化列表方式\*/

    void FetchData(auto &data, int usLength)
{   
    int index = 0;
    //char \*pData\_head = data;
    //printf("count%x\n",\*pData\_head); 
    printf("%x\n",data[index]);
    printf("count%d\n",usLength); 
    while (usLength >= 11)//一个完整数据帧11字节
    {
        printf("%x\n",data[index + 1]);        
        if (data[index] != 0x55)//0x55是协议头
        {
            index++;//指针(/索引)后移，继续找协议头
            usLength--;
            continue;
        }
        //for(int i = 0;i < 1000;i++){printf("once\n");}
        if(data[index + 1] == 0x50) //time
        {
            stcTime = &data[index + 2];
            //memcpy(&stcTime, &data[index + 2], 8);
        }    
        else if(data[index + 1] == 0x51) //加速度
        {
            stcAcc = &data[index + 2];
            //memcpy(&stcAcc, &pData\_head[index + 2], 8);
            acc.x = ((short)((short)stcAcc[1]<<8 | stcAcc[0])) / 32768.00 \* 16 \* 9.8;
            acc.y = ((short)((short)stcAcc[3]<<8 | stcAcc[2])) / 32768.00 \* 16 \* 9.8;
            acc.z = ((short)((short)stcAcc[5]<<8 | stcAcc[4])) / 32768.00 \* 16 \* 9.8;
        }
        else if(data[index + 1] == 0x52)
        {
            stcGyro = &data[index + 2];
            //memcpy(&stcGyro, &pData\_head[index + 2], 8);
            gyro.x = ((short)(