Matlab解析ROS数据包中的惯导(IMU)数据和相机(Camera)数据并保存为带时间戳的txt和jpg文件

介绍

有时候我们需要使用matlab处理ROS录制的数据包中的数据，将ROS数据包解析并保存文件则是第一步。
目前的工作需要将数据包中的IMU数据和相机数据保存为带时间戳的格式，方便利用matlab进行处理。
使用时只需要将bag包放在脚本所在的目录，并修改脚本文件中bag包路径名，以及确定bag包中含有imu和相机数据，然后直接运行，将会在当前目录生成对应的文件。
目录如下图所示：

加速度保存格式：

matlab代码如下：

clear all
clc
bag = rosbag('mynt_eyes_data.bag'); %加载bag包

%% 从包中解析imu和camera话题
imu_raw = select(bag,'Topic','/mynteye/imu/data_raw');
image_raw = select(bag,'Topic','/mynteye/left/image_raw');
camera_info = select(bag,'Topic','/mynteye/left/camera_info');

image_msg  = select(image_raw,'MessageType','sensor_msgs/Image');
imu_msg  = select(imu_raw,'MessageType','sensor_msgs/Imu');
camera_msg = select(camera_info,'MessageType','sensor_msgs/CameraInfo');

images = readMessages(image_msg);
imus  = readMessages(imu_msg);
camera  = readMessages(camera_msg);
%% 创建保存数据的文件夹和文件
mkdir('data/pic')

basePath = sprintf('./data/');
picPath = sprintf('./data/pic/');
acc_fid = fopen([basePath 'accelerometer.txt'],'w');
vel_fid = fopen([basePath 'gyroscope.txt'], 'w');
%% 挨个IMU将数据写入打开的文件中
for i = 1:imu_msg.NumMessages
    imu = imus(i);
    imu_timestamp = vpa(imu{
    
    1}.Header.Stamp.seconds*1000);
    acc_x = imu{
    
    1}.LinearAcceleration.X;
    acc_y = imu{
    
    1}.LinearAcceleration.Y;
    acc_z = imu{
    
    1}.LinearAcceleration.Z; 
    
    vel_x = imu{
    
    1}.AngularVelocity.X;
    vel_y = imu{
    
    1}.AngularVelocity.Y;
    vel_z = imu{
    
    1}.AngularVelocity.Z;
    
    fprintf(acc_fid, '%d %f %f %f\n', imu_timestamp, acc_x,acc_y,acc_z); % 格式：【时间戳 x轴加速度 y轴加速度 z轴加速度】
    fprintf(vel_fid, '%d %f %f %f\n', imu_timestamp, vel_x,vel_y,vel_z); % 格式：【时间戳 x轴角速度 y轴角速度 z轴角速度】
end
fclose(acc_fid);
fclose(vel_fid);
%% 将包中的图像逐帧保存为图片
for i = 1:image_msg.NumMessages
    image = images(i);
    image_timestamp = vpa(image{
    
    1}.Header.Stamp.seconds*1000);
    image_out = readImage(image{
    
    1});
    name = sprintf("./data/pic/%d.jpg",image_timestamp); %以时间戳命名
    imwrite(image_out, name); 
end