ROS2高效学习第九章 -- ros2 bag之 python 实现离线读取包数据 其二

1 前言和资料

上一篇文章ROS2高效学习第九章 – ros2 bag之编程实现包录制 其一，我们使用 cpp 编程实现了 ros2 bag 录制，支持录制任意消息类型的 topic。针对录制下来的 bag，我们可以使用 ros2 bag info ***db3 查看统计信息。但如果想离线查看 bag 里的具体 topic 数据，ros2 就没有提供相应的命令。
在更早一点的文章ROS高效入门第九章 – 掌握rosbag工具，编写python包录制、读取、截断和回放样例中，我们使用 python 编程实现了离线查看 ros1 bag 数据功能。本文我们将复用ROS2高效学习第九章 – ros2 bag之编程实现包录制 其一中的 bag_operator 软件包，使用 ros2 的接口，实现 ros2 bag 的离线查看。
本文参考资料如下：
（1）ROS高效入门第九章 – 掌握rosbag工具，编写python包录制、读取、截断和回放样例 第2.2节
（2）Reading-From-A-Bag-File-CPP
（3）Recording-A-Bag-From-Your-Own-Node-Py
本系列博客汇总：ROS2 高效学习系列。

2 正文

2.1 bag_echo 功能介绍

（1）bag_echo 支持 -b 指定要查看的 ros2 bag；支持 -t 指定要查看的 topic，可以一次指定多个 topic，也可以不指定，默认查看所有 topic 数据。

2.2 bag_operator 之 bag_echo

（1）在 bag_operator 中创建 bag_echo.py

cd ~/colcon_ws/src/bag_operator 
mkdir scripts
touch scripts/bag_echo.py

（2）编写 bag_echo.py：由于 ros2 bag 使用了sqlite3 存储，因此接口调用要比 ros1 bag 复杂一些。

#! /usr/bin/env python3
# -*- coding: utf-8 -*-

import os
import argparse
import logging
import rosbag2_py
from rclpy.serialization import deserialize_message
from rosidl_runtime_py.utilities import get_message

logging.basicConfig(level=logging.DEBUG)

class BagEcho(object):
    def __init__(self, bag, topics):
        self._bag = bag
        self._topics = topics
        self._reader = rosbag2_py.SequentialReader()

    def echo_bag(self):
        # 以sqlite3格式打开ros2 bag
        storage_options = rosbag2_py._storage.StorageOptions(uri=self._bag, storage_id='sqlite3')
        converter_options = rosbag2_py._storage.ConverterOptions('', '')
        self._reader.open(storage_options, converter_options)
        # 设置要读取的topic作为过滤条件，如果没有指定topic，则读取所有topic
        if self._topics:
            storage_filter = rosbag2_py._storage.StorageFilter(topics=self._topics)
            self._reader.set_filter(storage_filter)
        # 获取所有话题及其类型，并使用map存储
        topic_name2type = {topic_metadata.name: topic_metadata.type for topic_metadata in self._reader.get_all_topics_and_types()}

        # 循环读取消息，并进行个数统计
        msg_cnts = {}        
        while self._reader.has_next():
            (topic_name, data, t) = self._reader.read_next()

            # 统计每个话题的消息数
            if topic_name not in msg_cnts:  
                msg_cnts[topic_name] = 1
            else:
                msg_cnts[topic_name] += 1            

            # 反序列化消息
            message_type = get_message(topic_name2type[topic_name])
            msg = deserialize_message(data, message_type)

            # 打印消息内容
            print("----------[%s : %s]---------" % (topic_name, topic_name2type[topic_name]))
            print(msg)            

        # 打印摘要信息
        logging.info("----------[rosbag summary]---------")
        for topic_name in msg_cnts:  
            logging.info("topic %s msg cnt is %d" %(topic_name, msg_cnts[topic_name]))

def main():
  parser = argparse.ArgumentParser(description="echo ros2 bag topic")
  parser.add_argument("-b", "--bag", type=str, required=True, help="specify rosbag")
  parser.add_argument("-t", "--topics", nargs="+", type=str, help="specify topic")
  # extra_args可以让-t指定多个topic，并使用argparse.SUPPRESS隐藏extra_args参数
  parser.add_argument('extra_args', nargs='*', help=argparse.SUPPRESS)
  args=parser.parse_args()

  if not os.path.isfile(args.bag):
      logging.error("%s is not found!" %args.bag)
      return

  echo = BagEcho(args.bag, args.topics)
  echo.echo_bag()

if __name__ == "__main__":
    main()

（3）添加 CMakeLists.txt

install(PROGRAMS
  scripts/bag_echo.py
  DESTINATION lib/${PROJECT_NAME})

（4）编译并运行

~/colcon_ws
colcon build --packages-select bag_operator
source install/local_setup.bash
# 指定多个 topic
./install/bag_operator/lib/bag_operator/bag_echo.py -b 2024-03-14-18-09-20-bag/2024-03-14-18-09-20-bag_0.db3 -t /turtle1/cmd_vel /turtle1/pose
# 不指定 topic，查看所有数据
./install/bag_operator/lib/bag_operator/bag_echo.py -b 2024-03-14-18-09-20-bag/2024-03-14-18-09-20-bag_0.db3

3 总结

本文代码托管在本人的github上：bag_operator