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));

运动截图就不放了。