ROS learning - recording and playback data

Keywords: ROS Autonomous vehicles

This blog will introduce how to use ROS system to record data into a. bag file, and then play back the data to produce similar effects in the running system.

Record data (create a bag file)

Record topic data from a running ROS system and store it in a bag file.

First, open a Terminal window and run roscore.

roscore

Open the second Terminal window and run the turnlesim in the turnlesim package_ Node:

rosrun turtlesim turtlesim_node

Open the third Terminal window and run the turtle in the turtle SIM package_ teleop_ The key node can receive keyboard input:

rosrun turtlesim turtle_teleop_key

Return the following values:

Reading from keyboard
---------------------------
Use arrow keys to move the turtle. 'q' to quit.

In this way, you can control the movement of the little turtle through the direction keys on the keyboard. (note that it must be entered in the turtle_teleop_key window, not in the little turtle display window).

Record all published topic s

Open a new Terminal window and enter the following command to view all current topic s.

rostopic list -v

Return to the topic in:

Published topics:
 * /turtle1/color_sensor [turtlesim/Color] 1 publisher
 * /turtle1/cmd_vel [geometry_msgs/Twist] 1 publisher
 * /rosout [rosgraph_msgs/Log] 2 publishers
 * /rosout_agg [rosgraph_msgs/Log] 1 publisher
 * /turtle1/pose [turtlesim/Pose] 1 publisher

Subscribed topics:
 * /turtle1/cmd_vel [geometry_msgs/Twist] 1 subscriber
 * /rosout [rosgraph_msgs/Log] 1 subscriber

The published topic list in the above list is the only message type that may be recorded in the log data file/ turtle1/cmd_vel topic is by teleop_ Published by turtle, / turtle 1 / color_ Sensor and / turtle 1 / pose are published by turtle sim.

We will now record the published data, create a new folder bagfiles, and then switch to the new folder to record the data to the bag file.

mkdir ~/bagfiles
cd ~/bagfiles
rosbag record -a

In the above command, we create a temporary folder, and then use rosbag record to record data to the bag file in this folder. "- a" represents recording all published topic s.
Next, in turtle_ teleop_ In the key window, use the direction key to control the movement of the little turtle. Then exit with Ctrl + C, switch to the folder just created, and check whether there is a bag file named by date and time:

$ ls
	2021-11-17-15-29-40.bag

This bag file records all topic s published by any node in the time after running rosbag record.

Test and playback data in bag files

In the previous section, we recorded a. Bag file using the rosbag record command. Next, we will use the rosbag info and rosbag play commands to view the bag file information and playback data respectively.

Using the rosbag info command

First, we use rosbag info to see what information is recorded in this bag file. Execute this command in the folder where the bag file is located. The usage is as follows:

rosbag info <your bagfile>

For our bag file, the command changes to the following form:

rosbag info 2021-11-17-15-29-40.bag 

Return result:

path:        2021-11-17-15-29-40.bag
version:     2.0
duration:    1:00s (60s)
start:       Nov 17 2021 15:29:40.61 (1637134180.61)
end:         Nov 17 2021 15:30:40.62 (1637134240.62)
size:        521.8 KB
messages:    7486
compression: none [1/1 chunks]
types:       geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
             rosgraph_msgs/Log   [acffd30cd6b6de30f120938c17c593fb]
             turtlesim/Color     [353891e354491c51aabe32df673fb446]
             turtlesim/Pose      [863b248d5016ca62ea2e895ae5265cf9]
topics:      /rosout                    4 msgs    : rosgraph_msgs/Log   (2 connections)
             /turtle1/cmd_vel          16 msgs    : geometry_msgs/Twist
             /turtle1/color_sensor   3733 msgs    : turtlesim/Color    
             /turtle1/pose           3733 msgs    : turtlesim/Pose

This tells us the name and type of topic and the number of message s in the bag file for each topic. We can see that four of the five topics in the rostopic list are recorded. This is because we used the "- a" command.

Using the rosbag play command

The next step is to play back the bag file to reproduce the above recorded contents in the running ROS system. First, terminate the teleop program run in the previous section, that is, in the turtle_ teleop_ Use Ctrl + C in the terminal of key to end the command.
Then, use the following command to clear the existing tracks:

rosservice call /clear

Then use the rosbag play command to play back the data:

rosbag play 2021-11-17-15-29-40.bag

It can be seen that the little turtle plays back according to the driving track in the previous recording process:

[ INFO] [1637137724.569364284]: Opening 2021-11-17-15-29-40.bag

Waiting 0.2 seconds after advertising topics... done.

Hit space to toggle paused, or 's' to step.
 [RUNNING]  Bag Time: 1637134180.611523   Duration: 0.000000 / 60.010978         [RUNNING]  Bag Time: 1637134180.612333   Duration: 0.000810 / 60.010978         [RUNNING]  Bag Time: 1637134180.712481   Duration: 0.100958 / 60.010978 

 ...
 ...
[RUNNING]  Bag Time: 1637134240.557778   Duration: 59.946255 / 60.010978    
[RUNNING]  Bag Time: 1637134240.573918   Duration: 59.962395 / 60.010978        
[RUNNING]  Bag Time: 1637134240.573929   Duration: 59.962406 / 60.010978        
[RUNNING]  Bag Time: 1637134240.590156   Duration: 59.978633 / 60.010978               

Done.


If rosbag play publishes a message immediately after the broadcast, subscribers may not receive the previous published messages. We can use the - d option to specify the waiting time.

Finally, the topic / turnle1 / CMD will be published_ Vel, and the little turtle should start following similar rules as we used to through teleop_key mode.
We can also do some other operations, such as using the "- s" parameter of rosbag play to make the bag play not from the starting position, but after some time. In addition, there is a "- r" parameter that allows us to change the frequency of playing bag files. For example, if we want to set 2x speed playback, the command is as follows:

rosbag play -r 2 2021-11-17-15-29-40.bag

It should be noted that this will cause the trajectory we see to be slightly different.

Subset of record data

When we use the above command in a replicated system, we may publish hundreds of topics, some of which are similar to the image stream, and there will be a lot of data. In this way, it is not practical to directly record all topics to the bag file in the hard disk. Therefore, the rosbag record command supports recording specific topics into a bag file, allowing users to record only the topics they are interested in.
For example, we just need to record / turnle1 / CMD_ Vel and / turtle 1 / pose are two topic s:

rosbag record -o subset /turtle1/cmd_vel /turtle1/pose

The above command "- o subuset" is to name the. bag file subset, followed by / turnle1 / CMD_ Vel and / turnle1 / pose are the two topic names to be recorded.
After execution, wait a moment and end with Ctrl + C.

Next, use rosbag info to view the recorded bag file information:

rosbag info subset_2021-11-17-16-59-22.bag

Return result:

path:        subset_2021-11-17-16-59-22.bag
version:     2.0
duration:    17.7s
start:       Nov 17 2021 16:59:22.31 (1637139562.31)
end:         Nov 17 2021 16:59:39.99 (1637139579.99)
size:        92.5 KB
messages:    1123
compression: none [1/1 chunks]
types:       geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
             turtlesim/Pose      [863b248d5016ca62ea2e895ae5265cf9]
topics:      /turtle1/cmd_vel     17 msgs    : geometry_msgs/Twist
             /turtle1/pose      1106 msgs    : turtlesim/Pose

As you can see, the bag file contains only the / turtle 1 / CMD we recorded_ Vel and / turtle 1 / pose are two topic s.

We may have noticed that the turtle's path may not be accurately mapped to the original keyboard input to track the same path. (the rough shape should be the same) this is because the path tracked by turnlesim is very sensitive to time changes in the system, and rosbag has limited ability to accurately copy the system behavior in terms of the time when rosbag record records and processes messages and the time when rosbag play generates and processes messages. For nodes such as turnlesim, small time changes in processing command messages will subtly change the behavior. We should not expect to completely imitate the driving trajectory.

Posted by buzz on Wed, 17 Nov 2021 17:22:49 -0800