通過matlab提取rosbag數據

最近在做AGV導航實驗，導航小車是基於ROS實現的，判斷小車導航是否運行穩定，可以直接通過rviz直觀看出來。但是如何通過數據定性地分析數據，這就需要提取其定位數據，然後通過方差或擬合等數值分析的方法。那首先需要做的就是如何提取數據，matlab是個好工具，用matlab可以提取rosbag中的相關的數據。

代碼如下：

% 定義rosbag所在位置
filePath = fullfile('E:\WLL\Desktop\', 'rosbag', '2019_06_29.bag');

%提取bag中消息格式信息
bag=rosbag(filePath)

%選擇對應的消息，這裏就只選擇了nav_msgs/Odometry
Odometry = select(bag,'MessageType','nav_msgs/Odometry');

%提取信息，因爲經過實際測試，可能由於Odometry消息包含較多信息，導致readMessages函數運行較慢，因此這裏用了兩個for循環，目的是讓position保持較少數據，防止電腦卡死。共取40000個三維座標數據，j爲幾就是幾萬。將數據保存到A.txt文件夾。
position=zeros(10000,3);
 fp=fopen('A.txt','w');
for j=1:4
    data = readMessages(Odometry,((j-1)*10000+1):j*10000);
    for i=1:10000
         position(i,1)=data{i,1}.Pose.Pose.Position.X;
         fprintf(fp,'%f ',position(i,1));
         position(i,2)=data{i,1}.Pose.Pose.Position.Y;
         fprintf(fp,'%f ',position(i,2));
         position(i,3)=data{i,1}.Pose.Pose.Position.Z;
         fprintf(fp,'%f\n',position(i,3));
    end
end
 fclose(fp);

%繪圖程序，可以與上述程序分開寫
dat=load('A.txt')
plot(dat(:,1),dat(:,3),'b.','markersize',5);
hold on