录包与回放
我们在调试机器人程序的时候，经常会碰到一些异常的情况，然后系统就跑飞了。由于机器人系统通常都比较复杂，涉及到的传感器和执行器的数据很多，所以复现故障通常都会比较麻烦。 此时就产生了一个需求: 我们能不能把复现故障的过程录下来，然后不断的回放数据，直到找出问题的原因。此外为了交流方便，我们也会专门录制一些数据包用于验证算法， 比如著名的 Cartographer 就提供了一个德意志博物馆的数据包。
rosbag 提供了一些工具，可以高效的录制和回放数据。它是 ROS 的基础工具之一，和 roscpp, rospy 等收录在一个git仓库中。本文中，我们先参照官方教程介绍一下常用的录包和回放功能， 再简单浏览一下 rosbag 的命令行工具，最后了解一下它的 API， 看看如何在代码里读取数据包的内容。
1. 录包和回放教程
在初次安装ROS系统的时候，我们运行了一个官方的小程序 turtlesim，模拟了一只乌龟。
如下面的左图所示，我们分别在三个终端中运行了roscore, turtlesim_node和turtle_teleop_key，可以看到类似中间的窗口。
在运行turtle_teleop_key的终端中，按下方向键，就可以控制小乌龟前进、后退、左转、右转了。在窗口中会留下乌龟划过的痕迹。
（注意!!! 控制乌龟运动是在左图最下面那个turtle_teleop_key的终端中，不是在中间那个画着乌龟的窗口）
在开始录包之前，我们先简单看一下目前的系统状态。如下面右图所示，通过指令rostopic list我们看到当前系统中一共有6个消息主题。
其中，"/turtle1/cmd_vel"，是由turtle_teleop_key发布turtlesim_node订阅的消息主题，是它让乌龟在地面上爬。
通过rqt_graph我们可以打开右图最上面的那个GUI界面，看到节点和消息的订阅发布关系。
"/turtle1/pose"则是turtlesim_node用来反馈乌龟在窗口中位姿的消息主题。其它四个消息主题无关紧要，不表。
关于这些消息主题更详细的内容，我们可以通过rostopic info来查看他们。
 |
 |
 |
现在我们要录制一条标准的曲线，供后续的算法研究用。我们就可以通过如下的指令，借助rosbag record工具来记录下"/turtle1/cmd_vel"和"/turtle1/pose"两条指令。
其中，输入参数"-O"用于指示记录的消息数据保存路径，如下面的第4行所示，保存在了指定路径下的.bag文件中。后续的列表是用来告诉 rosbag 工具需要记录那些主题的消息。
$ rosbag record -O ~/douniwan /turtle1/cmd_vel /turtle1/pose
[ INFO] [1668422112.401323556]: Subscribing to /turtle1/cmd_vel
[ INFO] [1668422112.402912171]: Subscribing to /turtle1/pose
[ INFO] [1668422112.403731585]: Recording to '/home/gyc/douniwan.bag'
我们可以随时通过"Ctrl-C"来终止录包。通过rosbag info可以查看录包的基本信息，如下所示，罗列了包的存储路径、录包时长、消息的起止时间戳、大小、消息类型、
消息主题等各种信息。
$ rosbag info ~/douniwan.bag
path: /home/gyc/douniwan.bag
version: 2.0
duration: 16.9s
start: Nov 14 2022 18:46:43.94 (1668422803.94)
end: Nov 14 2022 18:47:00.87 (1668422820.87)
size: 92.4 KB
messages: 1110
compression: none [1/1 chunks]
types: geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
turtlesim/Pose [863b248d5016ca62ea2e895ae5265cf9]
topics: /turtle1/cmd_vel 51 msgs : geometry_msgs/Twist
/turtle1/pose 1059 msgs : turtlesim/Pose
现在我们把节点turtle_teleop_key关闭，重启turtlesim_node节点。然后，通过rosbag play播放刚才录制的数据包，
就可以看到新的turtlesim_node中的乌龟重复刚才的路线运动。可惜的是，这条路径跟录制时候的路径可能并不完全一致，只是大致相同。这主要受限于系统的时间，
rosbag 记录和播放消息的时候，并不能保证与原来的时间完全一致。这也是仿真世界与物理世界的一种差异。
$ rosbag play ~/douniwan.bag --topics /turtle1/cmd_vel
[ INFO] [1668434977.014689632]: Opening /home/gyc/douniwan.bag
Waiting 0.2 seconds after advertising topics... done.
Hit space to toggle paused, or 's' to step.
[RUNNING] Bag Time: 1668422808.260137 Duration: 0.000000 / 8.421941
现在，我们已经看到如何使用rosbag record录制数据包，并且通过rosbag info和rosbag play查看数据包的基本情况并播放之。
我们的例子中，都显示的指定了录包和播放的消息主题名称，有时为了方便，可以rosbag record -a录制所有的主题，
rosbag play不指定运行参数"--topic"播放所有的消息。
2. rosbag 的命令行工具
通过指令rosbag -h，我们查看 rosbag 的各种命令行工具，下表中列出了常用的一些指令。
官方文档中详细地描述了下述各个工具的功能和运行参数。
此外，我们也可以通过rosbag <subcommands> -h来查看各个工具的详细使用说明。
| subcommands | 说明 |
|---|---|
| check | 检查一个数据包是否可以播放 |
| compress | 压缩数据包，目前只支持BZ2和LZ4两种格式，BZ2压缩率更高但是解压速度慢。 |
| decompress | 解压数据包。 |
| decrypt | 解密数据包。 |
| encrypt | 加密数据包。 |
| filter | 使用python表达式对数据包中的数据过滤生成新的数据包。 |
| fix | 尝试修复数据包，使得它可以在当前系统中播放。 |
| help | 帮助消息。 |
| info | 查看数据包的摘要信息。 |
| play | 播放数据包。 |
| record | 录制数据包。 |
| reindex | 修复损坏数据包或者解析 ROS 早期版本的数据包。 |
本文第1节中的所用到的record、play、info基本覆盖了日常需要的80%的情况。其它比如压缩与解压、加解密等相关的工具我基本没有用过。 其中 filter 还是值得介绍一下。如下所示，filter 后续的输入参数主要有三个，依次描述了将要过滤的数据包名称、过滤之后的数据包名称、python形式的过滤条件表达式。
$ rosbag filter <in-bag> <out-bag> <expression>
下面是rosbag filter的帮助消息。我们主要关注其中的第6到9行输出的信息。我们可以通过内置的python变量 topic、m、t 来分别对主题、消息或者时间进行过滤。
$ rosbag filter --help
Usage: rosbag filter [options] INBAG OUTBAG EXPRESSION
EXPRESSION can be any Python-legal expression.
The following variables are available:
* topic: name of topic
* m: message
* t: time of message (t.secs, t.nsecs)
Filter the contents of the bag.
Options:
-h, --help show this help message and exit
-p PRINT-EXPRESSION, --print=PRINT-EXPRESSION
Python expression to print for verbose debugging. Uses same variables as filter-expression
比如说，我们可以通过topic来筛选特定的主题:
$ rosbag filter douniwan.bag bienao.bag "topic == '/turtle1/cmd_vel'"
bienao.bag 100% 74.0 KB 00:00
$ rosbag info bienao.bag
path: bienao.bag
version: 2.0
duration: 8.4s
start: Nov 14 2022 18:46:48.26 (1668422808.26)
end: Nov 14 2022 18:46:56.68 (1668422816.68)
size: 11.1 KB
messages: 51
compression: none [1/1 chunks]
types: geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
topics: /turtle1/cmd_vel 51 msgs : geometry_msgs/Twist
或者，通过 t 来筛选特定时间的消息:
$ rosbag filter douniwan.bag bienao.bag "t.to_sec() <= 1668422810.26"
bienao.bag 100% 74.0 KB 00:00
$ rosbag info bienao.bag
path: bienao.bag
version: 2.0
duration: 6.3s
start: Nov 14 2022 18:46:43.94 (1668422803.94)
end: Nov 14 2022 18:46:50.25 (1668422810.25)
size: 37.1 KB
messages: 400
compression: none [1/1 chunks]
types: geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
turtlesim/Pose [863b248d5016ca62ea2e895ae5265cf9]
topics: /turtle1/cmd_vel 5 msgs : geometry_msgs/Twist
/turtle1/pose 395 msgs : turtlesim/Pose
通过变量 m 我们甚至还可以直接对消息内容进行过滤。如下面的代码所示，我们可以从主题"/turtle1/pose"的消息中筛选出线速度大于0的消息。
$ rosbag filter douniwan.bag bienao.bag "(topic == '/turtle1/pose') and (m.linear_velocity > 0)"
bienao.bag 100% 74.0 KB 00:00
$ rosbag info bienao.bag
path: bienao.bag
version: 2.0
duration: 8.7s
start: Nov 14 2022 18:46:48.98 (1668422808.98)
end: Nov 14 2022 18:46:57.69 (1668422817.69)
size: 28.9 KB
messages: 317
compression: none [1/1 chunks]
types: turtlesim/Pose [863b248d5016ca62ea2e895ae5265cf9]
topics: /turtle1/pose 317 msgs : turtlesim/Pose
3. rosbag 的 API
虽然大多数情况下，我们都是直接通过rosbag play播放消息的。但有时为了代码方便，我们还是有可能在C++或者python代码中直接操作数据包的。
针对这一需求，官方提供了丰富的 API 供我们使用。下面，我们通过 API 直接读取数据包的内容。
首先在“~/catkin_ws/src”目录下，参照前文教程， 通过指令 catkin_create_pkg 创建一个 learning_rosbag 的包，并在其后的依赖列表中列举了 rosbag。 下面左侧的日志中第9行所指的文件 "readbag.cpp" 是后来自己写的一个读取数据包的C++程序。右侧则是精简之后的CMakeLists.txt。
|
|
在源文件"readbad.cpp"的一开始，我们引入了一些头文件。其中，第一行的<rosbag/bag.h>是 rosbag API 的头文件。
#include <rosbag/bag.h>
#include <iostream>
#include <cassert>
#include <string>
在 main 函数中，我们认定用户一定指定了一个bag文件。 然后创建了一个rosbag::Bag的对象， 并通过其 open 接口以只读的形式打开bag文件。
int main(int argc, char *argv[]) {
assert(2 == argc);
rosbag::Bag bag;
bag.open(argv[1], rosbag::BagMode::Read);
接着，就可以通过对象的各种 get 接口获取bag文件的一些描述信息。
std::cout << "filename: " << bag.getFileName() << std::endl;
std::cout << "version : " << bag.getMajorVersion() << "." << bag.getMinorVersion() << std::endl;
std::cout << "size : " << (bag.getSize() / 1024.0) << " KB" << std::endl;
我们需要构建一个rosbag::View的对象来遍历文件中的消息。 构造view的时候，可以通过rosbag::TopicQuery指定消息主题。 除了消息主题之外，API 还提供了rosbag::TypeQuery来获取指定类型的消息。
std::vector<std::string> topics;
topics.push_back("/turtle1/cmd_vel");
topics.push_back("/turtle1/pose");
rosbag::View view(bag, rosbag::TopicQuery(topics));
我们定义了两个变量用来统计"/turtle1/cmd_vel"和"/turtle1/pose"的消息数量。在for循环中可以通过begin, end接口遍历消息， 得到一个 rosbag::MessageInstance的对象。
int cmd_vel_count = 0;
int pose_count = 0;
// for (rosbag::MessageInstance const m : view) {
for (rosbag::View::iterator it = view.begin(); it != view.end(); it++) {
rosbag::MessageInstance & m = *it;
如下条件语句块所示，通过 rosbag::MessageInstance 对象的接口，我们可以获知消息的主题名称、时间戳、消息类型、类型md5校验和。
if ("/turtle1/cmd_vel" == m.getTopic()) {
cmd_vel_count++;
if (1 == cmd_vel_count) {
geometry_msgs::Twist::ConstPtr msg = m.instantiate<geometry_msgs::Twist>();
std::cout < m.getTopic() < " [" < m.getTime() < "]:" < std::endl;
std::cout < m.getDataType() < " [" < m.getMD5Sum() < "]" < std::endl;
std::cout < *msg < std::endl;
} }
模板接口 instantiate 是将通用的 rosbag::MessageInstance 对象按照指定的消息类型进行转换的接口。如果转换失败就会得到一个空指针 nullptr， 否则就可以获得具体的消息数据。
turtlesim::Pose::ConstPtr msg = m.instantiate();
if (nullptr != msg)
pose_count++;
}
std::cout < "cmd_vel_count:" < cmd_vel_count < std::endl;
std::cout < "pose_count:" < pose_count < std::endl;
return 0;
}
如果一切进行的都很顺利，通过如下的指令运行程序，我们应该可以看到类似如下的输出：
$ cd ~/catkin_ws/
$ catkin_make
$ rosrun learning_rosbag readbag ~/douniwan.bag
filename: /home/gyc/douniwan.bag
version : 2.0
size : 92.4062 KB
/turtle1/cmd_vel [1668422808.260136707]:
geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
linear:
x: 0
y: 0
z: 0
angular:
x: 0
y: 0
z: 2
cmd_vel_count:51
pose_count:1059
4. 总结
本文中，我们参照官方教程介绍了 rosbag record，rosbag play, rosbag info的基本用法。
然后讲述了帮助消息的查询方法，并详细介绍了rosbag filter的功能。最后，了解了一下 rosbag 的 API，并写了一个简单的读取 bag 文件的C++程序。