Read and analyze rosbag data in Matlab

Tutorial , Comments

I was experimenting with the Sawyer robot and using rosbag to record the robot status data, which need to be visualized and analyzed later. Usually the built-in rqt_plot in ROS can be used to have a overview of the data. However, rqt_plot is relatively simple and not handy when the data size is large. So it is better to import data into Matlab for analysis.

Matlab can directly read rosbag data. For topics common used in ROS, such as /joint_states and /cmd_vel, it is very convenient to use Matlab to read. However, topics that use a custom message type, you will need to recompile the ROS package that stores the msg file. This step can be completed using rosgenmsg command in Matlab, with the help of Matlab’s java interface, Matlab will be able to recognize the custom message.

My Matlab version is R2021a when I was writing this blog. Note that in the R2020b version, Matlab’s support for ROS has been greatly changed. rosgenmsg is directly included in the toolbox (no additional support package is required). You may found some online tutorials use the roboticsAddons command to install rosgenmsg plugin, maybe the Matlab version they used is before R2020b. You may read this official document directly in the future: ROS Custom Message Support, just follow the documentation step by step, purpose of this blog is mainly to record some tricky problems that need attention when the first time you are dealing with it.

Update Cmake version

Make sure your Cmake version is 3.15.5 or later, this requirement can be found in official Matlab document ROS System Requirements.

Check your current Cmake version:

1
cmake --version

If the version is older than 3.15.5, it needs to be updated. The most common way is directly with sudo apt-get install cmake in the Linux terminal, but after installation, you may found the version is still older than the required version, such as Ubuntu 16.04 I was using, I can only get 3.5.1. The reason why I can not install the latest version with sudo apt-get install cmake is that the repositories of the operating system itself have not been updated. 16.04 is the long-term support (LTS) version of Ubuntu, and it must be stable within 5 years. Generally, only critical or security updates are performed, and the latest versions of packages are not updated frequently, usually every 6 months.

Recommended installation method:

  • Uninstall the default version provided by Ubuntu:
    1
    sudo apt remove --purge --auto-remove cmake
    Or
    1
    sudo apt purge --auto-remove cmake
  • Go to CMake official website download interface, check the Cmake version number, and determine the version to be installed (modify the version and build variables to the required version number, following example: 3.19.1):
    1
    2
    version=3.19
    build=1
  • Create a temp folder to store the cmake source package, and download the cmake source package:
    1
    2
    3
    mkdir ~/temp
    cd ~/temp
    wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz
  • Unzip the source package:
    1
    tar -xzvf cmake-$version.$build.tar.gz
  • Enter the unzipped cmake directory:
    1
    cd cmake-$version.$build/

  • Install cmake：

    1
    2
    3
    ./bootstrap
    make -j$(nproc)
    sudo make install

    Note:

    • Because I have a multi-core CPU, the make -j$(nproc) command is used here for parallel compilation, just using make is also ok, the nproc information can be found here
    • make install command requires root privilege

  • Finally, open a new terminal and check the Cmake version:

    1
    cmake --version

    Note:

    • cmake --version is only valid in a new terminal, because for the method used above, Cmake will be installed in /usr/local/bin/ by default, and if you use the sudo apt-get install cmake mentioned at the beginning, the default installation path will be /usr/bin/, the reason can be seen This answer on 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.

    Result after running cmake --version:

    1
    2
    3
    cmake version 3.19.1

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

Confirm Cmake path

After updating the Cmake version, in order to ensure Cmake is available in Matlab, you need to check Cmake is on the environment variables of Matlab:

  • First, check the Cmake path in the Linux terminal. As mentioned above, Cmake will be installed by default in /usr/local/bin/, which can be check in the terminal:
    1
    which cmake
    Result：
    1
    /usr/local/bin/cmake
  • Then open Mtalab and run the command:
    1
    !which cmake
    The result should be consistent with the path found in the Linux terminal earlier:
    1
    /usr/local/bin/cmake
  • Finally, check the Cmake version in Matlab:
    1
    !cmake --version
    The running result should be consistent with the version number found in the Linux terminal earlier:
    1
    2
    3
    cmake version 3.19.1

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

Compile the custom message

Here I use the ROS package intera_core_msgs provided by Sawyer as an example. The custom message file is in msg folder. I first create a folder custom_msgs, and create a folder sawyer_custom_msg_matlab inside it to contain the package, other packages also can be placed in this folder in the future. I copied the intera_core_msgs package directly from Github, which has already generated and configured CmakeLists.txt and package.xml file, if you wish to create a ROS package yourself, don’t forget to modify these two files. The following matlab_msg_gen_ros1 folder is generated by compiling with Matlab, which will be demonstrated below.


Note

  • Only msg is used here, I did not copy srv when coping the intera_core_msgs package, I can add it when it is needed.
  • ROS actions are not supported currently, it will be ignored during custom message generation.

Next, you can use the rosgenmsg command to compile in Matlab:

  • Declare the path of the ROS package where the custom message is located:
    1
    sawyer_folder = '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'
    Result:
    1
    2
    3
    sawyer_folder =

        '/home/siqin/Documents/MATLAB/custom_msgs/sawyer_custom_msg_matlab'
  • Call rosgenmsg:
    1
    rosgenmsg(sawyer_folder)
    Result：
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    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.
  • Then follow the instructions above to add environment variables, clear the workspace, and run the following commands one by one:
    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
    When executing savepath, if you are prompted that the Matlab path file cannot be modified, there are two solutions:

    • Method 1: Modify permissions in the current Matlab command window:
      Check pathdef path:
      1
      which pathdef
      Result：
      1
      /usr/local/MATLAB/R2021a/toolbox/local/pathdef.m
      Modify it so that all users have read and write permissions:
      1
      sudo chmod 666 /usr/local/MATLAB/R2018a/toolbox/local/pathdef.m
    • Method 2: Reopen Matlab with sudo matlab in the Linux terminal

Finally, use to see if the custom message types are successfully generated：

1
rosmsg list

We can see that all custom messages in the msg folder under the intera_core_msgs package have been successfully generated:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
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 data processing

Next, I will take the custom message intera_core_msgs/EndpointState as an example, and use Matlab to process the robot data recorded by rosbag.

  • Read bag file: 
    1
    bag = rosbag('/home/siqin/catkin_ws/sawyer_end_point_state.bag');
    After running, click the bag variable in the Workspace to get the information shown in the following figure:


  • Select Topic
    Click AvailableTopics in the above picture to review what topics are recorded. I only recorded one topic /robot/limb/right/endpoint_state here, as shown in the following figure:

    After reviewing the topic, use the select function to select the topic name to be processed:

    1
    state_select = select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/robot/limb/right/endpoint_state');

    In the figure as shown above, you may also notice that the intera_core_msgs/EndpointState in the MessageType column is the message we customized before.


  • Use the readMessages function to read the data selected by the select function

    1
    stateMsgs = readMessages(state_select);

    After running, click the stateMsgs variable in the Workspace to get the information shown in the following figure:


    You can see that stateMsgs is a 1408x1 array, which contains 1408 message data records, click on the first one:

    The figure above shows the data structure that defines intera_core_msgs/EndpointState. We can first look at the content in the Linux terminal:

    1
    rosmsg show intera_core_msgs/EndpointState

    Result：


    We can see that intera_core_msgs/EndpointState uses two ROS built-in messages std_msgs and geometry_msgs, intera_core_msgs/EndpointState mainly contains the following information of the robot TCP:

    pose:

    • positon: x y z coordinates
    • orinentation: represented by quaternions

    twist (a screw, 6-dimensional vector):

    • linear: linear velocity along an axis, a 3-dimensional vector
    • angular: angular velocity about an axis, a 3-dimensional vector

    wrench (a screw, 6-dimensional vector):

    • force: a 3-dimensional vector
    • torque: a 3-dimensional vector

    Note: Quaternion and Screw theory will be discussed separately later if I have time.

    Take the position in pose as an example: click 1x1 Pose in the stateMsgs shown in the above figure, get the information shown in the figure below:

    You can see that Pose contains positon and orinentation information, click on positon:

    The positon contains the x y z coordinates of the robot TCP, which are consistent with the results viewed in the terminal. Click on the information of X:


    Note：
    The name of each tab in the figure represents the method of accessing the current data. For example, stateMsg{1,1}.Pose.Position.X represents the method of accessing X.

    Next, to visualize the data, we can create a three-dimensional array, and put all the position data into the array:

    1
    2
    3
    4
    5
    6
    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

    Draw a trajectory graph:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    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);

    Complete Matlab code:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    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 DongStudent

Mechanical Engineering