通过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