Matlab分析rosbag記錄的數據

在進行機器人數據可視化與分析的過程中，難免要用到Matlab，雖然ROS提供了rqt_plot功能，但是從可視化的角度，當前還是用Matlab畫圖更清晰一些。所以在此記錄一下用Matlab顯示rosbag記錄的機器人軌跡數據的過程。

1. ROS記錄Topic數據

rosbag的具體使用方法，參考WIKI說明。在機器人運動啓動之前，首先啓動記錄功能，以儘可能完整的記錄機器人的運動數據。

1.1 建立保存路徑

首先在home目錄下，建立了一個保存.bag文件的文件夾。之後在當前位置下開始記錄軌跡數據。

mkdir ~/bagfile
cd bagfile

1.2 記錄/JointState Topic數據

利用以下命令記錄/JointState的數據，記錄文件的名字直接用默認的以時間命名的名稱。

rosbag record /JointState

1.3 啓動機器人運動

在此過程中，rosbag會不斷的將對應話題的數據記錄到.bag文件中，運動結束後，在啓動rosbag的命令窗口中Ctrl+C結束記錄過程。

2. Matlab對.bag文件的處理

2.1 文件導入

.bag文件放在如下圖所示的目錄結構中，即存放在當前.m文件同級的Data文件夾下。

利用以下代碼可以直接讀取ROS定義的消息類型。

bagdata = rosbag('./Data/trajectory.bag');

加載之後，在Matlab中選擇加載後的數據類型，可以看到如下的內容。

2.2 Topic選擇

在加載完成數據之後，可以選擇上圖中的AvaliableTopics查看記錄有哪些Topic，如下圖所示：

如果記錄有多個Topic，則會顯示多個。之後利用函數選擇需要處理的Topic名稱。

bagtopic = select(bagdata,'Topic','/joint_states');

2.3 數據讀取

利用readMessages函數從加載的數據中，讀取select函數選擇的數據。

topicdata = readMessages(bagtopic);

則需要的數據就讀取到了topicdata中，如下圖所示：

進入數據的下一層，則是：

在這之後就可以進行後續的數據處理和可視化了。

3. 完整程序

以下是Matlab代碼，函數的使用可以help一下：

%% 用於分析rosbag文件中的數據

%% 清理
clear;
clc;

%% bag文件讀取
bagdata = rosbag('./Data/trajectory.bag');

%% bag文件數據選擇,選擇Topic
bagtopic = select(bagdata,'Topic','/joint_states');

%% Topic數據讀取
topicdata = readMessages(bagtopic);

% %% bag文件選擇數據類型
% bagdatatype = select(bagdata,'MessageType','sensor_msgs/JointState');
% %% 按類型數據讀取
% typedata = readMessages(bagdatatype);

%% 數據重塑，讀取數據個數
[dataSize,~] = size(topicdata);

%% 重新處理一下關節名稱
jointName = topicdata{1,1}.Name;
for i=1:18
   jointName{i,1} = strrep(jointName{i,1},'_','\_');
end

%% 數據整理
jointData = [];
for i=1:dataSize
    tempData = topicdata{i,1}.Position;
    jointData = [jointData tempData];
end

%% 畫出A機械臂的運動軌跡
figure;
for j=8:14
    plot([1:dataSize],jointData(j,:),'LineWidth',1.5);
    hold on;
end
xlabel('Seq');
ylabel('Rad');
title('A arm joint space trajectory');
grid on;
legend(jointName(8:14));

%% 畫出B機械臂的運動軌跡
figure;
for j=1:7
    plot([1:dataSize],jointData(j,:),'LineWidth',1.5);
    hold on;
end
xlabel('Seq');
ylabel('Rad');
title('B arm joint space trajectory');
grid on;
legend(jointName(1:7));

運動截圖就不放了。