在Matlab中读取并分析rosbag数据

03-16-2021 Tutorial Matlab, ROS Comments Words: 2.5k(words) Reading time: 11(mins)

我最近在用Sawyer机器人做实验，用rosbag记录了机器人状态数据，后续要进行可视化和分析，虽然ROS内置rqt_plot有绘图功能，但是rqt_plot的功能比较简单，数据量大的时候查看起来也比较麻烦，还是导入进Matlab分析更好一些。

Matlab可以直接读取rosbag数据，对于ROS自带的topic信息，比如 /joint_states 和 /cmd_vel，用Matlab读取很方便。但是对于使用了自定义的message类型的topic，需要重新编译存放msg文件的ROS package，这一步骤在Matlab中使用rosgenmsg命令完成，借助Matlab的java接口，Matlab就能识别自定义的message了。

我的Matlab版本是R2021a，需要注意的是在R2020b版本中Matlab对ROS功能的支持做了很大改动，rosgenmsg 直接包含在了toolbox中（不需要额外的support package)，网上有的教程写需要通过roboticsAddons命令安装插件，应该用的是R2020b之前的Matlab版本。以后可以直接看这份官方文档：ROS Custom Message Support，具体操作跟着文档走即可，这里主要是记录一下需要注意的地方。

更新Cmake版本

Cmake版本要确保在3.15.5或以上，在Matlab官方文档ROS System Requirements也可以查到。

检查你目前的Cmake版本：

1	cmake --version

如果版本低于3.15.5则需要更新，最常见的想法可能是直接在Linux终端用sudo apt-get install cmake安装，但是安装完发现是低于要求的版本，比如我用的Ubuntu 16.04，只能得到3.5.1，用sudo apt-get install cmake安装不了最新版本的原因是操作系统本身的repositories没有更新，16.04是Ubuntu的长期支持（LTS）版本，要在5年内保持稳定，一般只会进行关键或者安全方面的更新，不会经常主动更新最新版本的packages，通常每6个月才会更新一次。

推荐安装方法如下：

卸载Ubuntu提供的默认版本：

1	sudo apt remove --purge --auto-remove cmake

或者

1	sudo apt purge --auto-remove cmake

去CMake官网下载界面，查看Cmake版本号，确定要安装的版本（修改version和build变量到需要的版本号，下例：3.19.1）：
1
2
version=3.19
build=1

先新建一个文件夹temp来存放cmake源码包，然后获取cmake源码包：

1
2
3

mkdir ~/temp
cd ~/temp
wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz

解压源码包:
1
tar -xzvf cmake-$version.$build.tar.gz
进入解压后的cmake目录:
1
cd cmake-$version.$build/
安装cmake：
1
2
3
./bootstrap
make -j$(nproc)
sudo make install
Note:
- 因为我的是多核CPU，这里使用了make -j$(nproc)命令进行并行编译，不过直接用make出问题的概率小一点，nproc资料可以看这里
- make install命令需要root privilege
最后，打开一个新终端，检查Cmake版本：
1
cmake --version
Note:
- cmake --version要在新终端才有用，因为用上面的方法Cmake会默认安装在/usr/local/bin/，而如果用开始提到的sudo apt-get install cmake安装，默认安装路径会是/usr/bin/，原因可以看StackExchange上这个回答：/usr/local/bin is for normal user programs not managed by the distribution package manager, e.g. locally compiled packages. You should not install them into /usr/bin because future distribution upgrades may modify or delete them without warning.
cmake --version运行的结果:
1
2
3
cmake version 3.19.1

CMake suite maintained and supported by Kitware (kitware.com/cmake).

确认Cmake路径

更新好Cmake版本后，为了确保Cmake在Matlab中可用，需要保证Cmake在Matlab的环境变量中：

首先在Linux终端中查看Cmake路径，上面已经提过Cmake会默认安装在/usr/local/bin/，可以在终端查看：
1
which cmake
运行结果：
1
/usr/local/bin/cmake
然后打开Mtalab，命令行运行：
1
!which cmake
运行结果应该和前面Linux终端查到的路径一致：
1
/usr/local/bin/cmake

最后也在Matlab查看一下Cmake版本：

1	!cmake --version

运行结果应和前面Linux终端查到版本号一致：

1
2
3

cmake version 3.19.1

CMake suite maintained and supported by Kitware (kitware.com/cmake).

编译自定义message

这里用Sawyer提供的ROS packageintera_core_msgs举例，自定义的message文件就在msg里面。我先建了一个文件夹custom_msgs，然后在里面再建个文件夹sawyer_custom_msg_matlab包含这个package，以后有其他package也可以放到这个文件夹里面。intera_core_msgspackage我是直接从Github上复制进来的，里面已经生成且配置好CmakeLists.txt和package.xml文件了，如果你想自己create ROS package也可以，注意别忘了修改这两个文件。后面的matlab_msg_gen_ros1文件夹就是用Matlab编译生成的，下面会演示。

Note

因为只要用到msg，我复制intera_core_msgspackage时没有复制srv，需要的时候再加进去。

目前不支持ROS actions，在自定义message生成期间将被忽略。

接下来就可以在Matlab里面用rosgenmsg命令编译了：

声明自定义message所在ROS package的路径：

1	sawyer_folder = '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'

运行结果：

1
2
3

sawyer_folder =

    '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'

Call rosgenmsg:

1	rosgenmsg(sawyer_folder)

运行结果：

Identifying message files in folder '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'..Done.
Validating message files in folder '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'..Done.
[1/1] Generating MATLAB interfaces for custom message packages... Done.
Running catkin build in folder '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab/matlab_msg_gen_ros1/glnxa64'.
Build in progress. This may take several minutes...
Build succeeded.build log
 
To use the custom messages, follow these steps:
 
1. Add the custom message folder to the MATLAB path by executing:
 
addpath('/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab/matlab_msg_gen_ros1/glnxa64/install/m')
savepath
 
2. Refresh all message class definitions, which requires clearing the workspace, by executing:
 
clear classes
rehash toolboxcache
 
3. Verify that you can use the custom messages. 
   Enter "rosmsg list" and ensure that the output contains the generated
   custom message types.

然后按照上面的提示说明加入环境变量，清理一下工作空间就可以了，逐条运行：
1
2
3
4
addpath('/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab/matlab_msg_gen_ros1/glnxa64/install/m')
savepath
clear classes
rehash toolboxcache
Note
执行savepath时如果提示无法修改Matlab路径文件的话，有两种解决方法：
- 第一种：在当前Matlab命令窗口修改权限：
  查看pathdef路径:
  1
  which pathdef
  运行结果：
  1
  /usr/local/MATLAB/R2021a/toolbox/local/pathdef.m
  修改成所有user都有读和写的权限：
  1
  sudo chmod 666 /usr/local/MATLAB/R2018a/toolbox/local/pathdef.m
- 第二种：在Linux终端用sudo matlab重新打开Matlab

最后用查看自定义的message types是否被成功生成：

1	rosmsg list

可以发现intera_core_msgspackage下msg文件夹里面的所有自定义message都已被成功生成：

intera_core_msgs/AnalogIOState                                 
intera_core_msgs/AnalogIOStates                                
intera_core_msgs/AnalogOutputCommand                           
intera_core_msgs/CameraControl                                 
intera_core_msgs/CameraSettings                                
intera_core_msgs/CollisionAvoidanceState                       
intera_core_msgs/CollisionDetectionState                       
intera_core_msgs/DigitalIOState                                
intera_core_msgs/DigitalIOStates                               
intera_core_msgs/DigitalOutputCommand                          
intera_core_msgs/EndpointNamesArray                            
intera_core_msgs/EndpointState                                 
intera_core_msgs/EndpointStates                                
intera_core_msgs/HeadPanCommand                                
intera_core_msgs/HeadState                                     
intera_core_msgs/HomingCommand                                 
intera_core_msgs/HomingState                                   
intera_core_msgs/IOComponentCommand                            
intera_core_msgs/IOComponentConfiguration                      
intera_core_msgs/IOComponentStatus                             
intera_core_msgs/IODataStatus                                  
intera_core_msgs/IODeviceConfiguration                         
intera_core_msgs/IODeviceStatus                                
intera_core_msgs/IONodeConfiguration                           
intera_core_msgs/IONodeStatus                                  
intera_core_msgs/IOStatus                                      
intera_core_msgs/InteractionControlCommand                     
intera_core_msgs/InteractionControlState                       
intera_core_msgs/JointCommand                                  
intera_core_msgs/JointLimits                                   
intera_core_msgs/NavigatorState                                
intera_core_msgs/NavigatorStates                               
intera_core_msgs/RobotAssemblyState                            
intera_core_msgs/SEAJointState                                 
intera_core_msgs/URDFConfiguration

rosbag数据处理

接下来以intera_core_msgs/EndpointState这个自定义message为例，用Matlab处理rosbag记录的机器人数据。

bag文件读取:
1
bag = rosbag('/home/siqin/catkin_ws/sawyer_end_point_state.bag');
运行后点击Workspace的bag变量，得到如下图所示信息：

选择Topic
点击上图中的AvaliableTopics查看记录有哪些Topic，我这里只记录了一个/robot/limb/right/endpoint_state，如下图所示：

查看topic之后利用select函数选择需要处理的Topic名称:
1
state_select = select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/robot/limb/right/endpoint_state');
从上图还可以注意到MessageType列中的intera_core_msgs/EndpointState正是我们之前自定义的message。

利用readMessages函数读取select函数选择的数据

1	stateMsgs = readMessages(state_select);

运行后点击Workspace的stateMsgs变量，得到如下图所示信息：

可以看到stateMsgs是一个1408x1的array，包含了记录的1408条message数据，点击第一条看看：

上图显示的是定义intera_core_msgs/EndpointState的数据结构，我们可以先在Linux终端看一下具体内容：

1	rosmsg show intera_core_msgs/EndpointState

运行结果：

可以看到intera_core_msgs/EndpointState使用了std_msgs和geometry_msgs两种ROS自带的message来定义，主要包含了机器人TCP的以下信息：

pose（位姿）：

positon 位置： x y z坐标

orinentation 姿态：用四元数表示

twist（旋量，6维向量）：

linear：沿轴的线速度，3维向量

angular：绕轴的角速度 3维向量

wrench（旋量，6维向量）：

force：力，3维向量

torque：力矩，3维向量

Note： 四元数和旋量理论后面有空单独写几篇讨论

以pose中的position为例：点击上图stateMsgs中的1x1 Pose，得到如下图所示信息：

可以看到Pose包含了positon和orinentation信息，点开positon：

positon包含了机器人TCP的x y z坐标，与在终端查看的结果一致，点开X的信息：

Note：
图中每个tab的名称代表的是当前数据的读取方式，比如stateMsg{1,1}.Pose.Position.X表示的是X的读取方式。

接下来可视化数据，建立一个三维数组，将所有position数据读入数组：

TCP_position=zeros(1408,3);
for i=1:1408
    TCP_position(i,1)=stateMsgs{i,1}.Pose.Position.X;
    TCP_position(i,2)=stateMsgs{i,1}.Pose.Position.Y;
    TCP_position(i,3)=stateMsgs{i,1}.Pose.Position.Z;
end

绘制轨迹图：

figure;
for i=1:3
    plot((1:1408),TCP_position(:,i),'LineWidth',1.5);
    hold on;
end
xlabel('seq');
title('Position of End Point');
grid on;
legend('Position.X','Position.Y','Position.Z');
  
figure;
comet3(position(:,1),position(:,2),position(:,3),0.5);

完整Matlab代码：

clear all;
close all;
clc;

bag = rosbag('/home/siqin/catkin_ws/sawyer_end_point_state.bag');

state_select = select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/robot/limb/right/endpoint_state');% {'intera_core_msgs/EndpointState'}
stateMsgs = readMessages(state_select);

% rosmsg show intera_core_msgs/EndpointState
% std_msgs/Header header
%   uint32 seq
%   time stamp
%   string frame_id
% geometry_msgs/Pose pose
%   geometry_msgs/Point position
%     float64 x
%     float64 y
%     float64 z
%   geometry_msgs/Quaternion orientation
%     float64 x
%     float64 y
%     float64 z
%     float64 w
% geometry_msgs/Twist twist
%   geometry_msgs/Vector3 linear
%     float64 x
%     float64 y
%     float64 z
%   geometry_msgs/Vector3 angular
%     float64 x
%     float64 y
%     float64 z
% geometry_msgs/Wrench wrench
%   geometry_msgs/Vector3 force
%     float64 x
%     float64 y
%     float64 z
%   geometry_msgs/Vector3 torque
%     float64 x
%     float64 y
%     float64 z
% bool valid

TCP_position=zeros(1408,3);
for i=1:1408
    TCP_position(i,1)=stateMsgs{i,1}.Pose.Position.X;
    TCP_position(i,2)=stateMsgs{i,1}.Pose.Position.Y;
    TCP_position(i,3)=stateMsgs{i,1}.Pose.Position.Z;
end

figure;
for i=1:3
    plot((1:1408),TCP_position(:,i),'LineWidth',1.5);
    hold on;
end
xlabel('seq');
title('Position of End Point');
grid on;
legend('Position.X','Position.Y','Position.Z');

figure;
comet3(TCP_position(:,1),TCP_position(:,2),TCP_position(:,3),0.5);

References

[1] https://www.mathworks.com/help/ros/ug/ros-custom-message-support.html
[2] https://askubuntu.com/questions/355565/how-do-i-install-the-latest-version-of-cmake-from-the-command-line
[3] https://www.mathworks.com/matlabcentral/answers/623103-matlab-2020b-rosgenmsg-can-t-find-cmake
[4] https://blog.csdn.net/yaked/article/details/97682872
[5] https://blog.csdn.net/weixin_40712763/article/details/78909608
[6] https://blog.csdn.net/u012424737/article/details/106766307

Siqin Dong

Student

在Matlab中读取并分析rosbag数据

更新Cmake版本

确认Cmake路径

编译自定义message

rosbag数据处理

References

Siqin DongStudent