2024年运维最新【ROS2】获取imu数据并可视化保姆级教程(C++)_imu可视化(2)

    {
        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)((short)stcGyro[1]<<8 | stcGyro[0])) / 32768.00 \* 2000 / 180 \* M_PI;//弧度制
        gyro.y = ((short)((short)stcGyro[3]<<8 | stcGyro[2])) / 32768.00 \* 2000 / 180 \* M_PI;
        gyro.z = ((short)((short)stcGyro[5]<<8 | stcGyro[4])) / 32768.00 \* 2000 / 180 \* M_PI;
    }   
    else if(data[index + 1] == 0x53)
    {
        stcAngle = &data[index + 2];
        //memcpy(&stcAngle, &pData\_head[index + 2], 8);
        angle.r = ((short)((short)stcAngle[1]<<8 | stcGyro[0])) / 32768.00 \* M_PI;
        angle.p = ((short)((short)stcAngle[3]<<8 | stcGyro[2])) / 32768.00 \* M_PI;
        angle.y = ((short)((short)stcAngle[5]<<8 | stcGyro[4])) / 32768.00 \* M_PI; 
    }

  /\*case 0x54: //磁力计

memcpy(&stcMag, &pData_head[2], 8);
mag.x = stcMag[0];
mag.y = stcMag[1];
mag.z = stcMag[2];*/
//这里我一开始用switch case的写法
/*case 0x59: //四元数
memcpy(&stcQuat, &pData_head[2], 8);
quat.w = stcQuat[0] / 32768.00;
quat.x = stcQuat[1] / 32768.00;
quat.y = stcQuat[2] / 32768.00;
quat.z = stcQuat[3] / 32768.00;*/

        //这个型号imu传感器6轴，暂时不启用这些读取
        //default:printf("over\n");
    
    printf("over\n");
    usLength = usLength - 11;
    index += 11;

}

}

};

上面printf的内容是我调试用的，不需要可以注释掉。一开始我使用传地址的方式，传入函数的参数一个强制转换为(char\*)类型的数组名，内参是一个char指针变量，但发布节点运行到函数内解析数据的代码就挂掉了，无任何提示，所以我这里换了一种写法，后续无bug，但也没想白之前的写法有什么问题，有想法的bro可以交流以下。


ok，跳过题外话，我们接下来编写发布节点。


## 5.编写publisher\_imu.cpp


publisher\_imu.cpp的代码如下：

#include
#include <math.h>
#include “serial/serial.h” //前面安装的ROS2内置串口包
#include <memory.h>
#include “std_msgs/msg/string.hpp”
#include “sensor_msgs/msg/magnetic_field.hpp”
//#include “geometry_msgs/msg/detail/vector3__struct.hpp”
#include “sensor_msgs/msg/imu.hpp”
#include “tf2/LinearMath/