Matlab解析ROS数据包中的惯导数据和相机数据并保存为带时间戳的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