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

總結

以上是生活随笔為你收集整理的利用matlab对rosbag数据,通过matlab提取rosbag数据的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。