ROS2学习笔记（十一）-- ROS2 bag数据记录与回放

        简介：ROS2提供了ros2 bag命令，可以记录指定主题的数据到文件中，也可以将记录下的内容再发布出来，相当于是数据的回放，除了通过命令行的方式实现数据记录以外，也可以通过编程实现主题数据记录以及而合成的主题数据记录

---

目录

1、ros2 bag命令行工具

1.1 ros2 bag record

1.2 ros2 bag info

1.3 ros2 bag play

2、ros2 bag record 编程实现

2.1 先实现基础的发布订阅功能

2.2 从节点中记录主题数据包

2.3 从节点中记录合成数据包

2.4 从可执行文件记录合成数据

---

1、ros2 bag命令行工具

1.1 ros2 bag record

命令功能：记录指定主题消息，消息数据包可通过play命令回放

命令格式：

ros2 bag record 记录单个主题消息

ros2 bag record -o ... 记录多个主题消息

ros2 bag record -a 记录系统内所有主题消息

启动所有节点，新开终端，创建新目录，查看主题列表，记录/ros2_robot/duckiebot_node/image主题消息

$ mkdir bag
$ cd bag
$ ros2 topic list
$ ros2 bag record /ros2_robot/duckiebot_node/image

        可以看到当前目录下创建了一个以rosbag2+时间戳的目录，目录下有一个metadata.yaml文件，内容是数据记录的相关信息介绍，另外一个db3格式文件，就是我们记录下来的主题数据。

$ ros2 bag record -o subset /ros2_robot/duckiebot_node/image /ros2_robot/control_node/action

$ ros2 bag record -a

1.2 ros2 bag info

命令功能：查看主题数据记录文件信息

命令格式：ros2 bag info

我们可以用之前的3各记录命令生成的数据文件来测试：

1.3 ros2 bag play

命令功能：回放记录下来的主题数据，可通过ros2 topic echo查看回放数据，也可以通过rqt工具查看

命令格式：ros2 bag play

例如：ros2 bag play subset

注：如果记录的主题数据有自定义消息，则需要配置环境变量。

2、ros2 bag record 编程实现

        上文介绍了ros2 bag的命令行操作，而主题数据记录也可以通过编程实现。为了不与之前的功能冲突，我们新建一个功能包来实现编程记录主题数据包。

2.1 先实现基础的发布订阅功能

        新建功能包ros2_bag，并创建两个节点image_pub和image_sub：

$ cd ~/ros2_ws/src
$ ros2 pkg create ros2_bag --build-type ament_python --dependencies rclpy rosbag2_py sensor_msgs

        在ros2_bag目录下新建image_pub.py和image_sub.py

        编辑image_pub.py，实现图像发布功能：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import gym
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
 
class ImagePub(Node):
 
    def __init__(self, name):
        super().__init__(name)
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        self.env.reset()
        self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
        self.timer = self.create_timer(0.05, self.timer_callback)
        self.bridge = CvBridge()
    
    def timer_callback(self):
        action = self.env.action_space.sample()
        obs, reward, done, info = self.env.step(action)
        self.pub_img.publish(self.bridge.cv2_to_imgmsg(obs, 'rgb8'))
        if done:
            self.env.reset()
 
def main(args=None):
    rclpy.init(args=args)
    node = ImagePub(name="image_pub")
    rclpy.spin(node=node)
    rclpy.shutdown()

        编辑image_sub.py，实现图像订阅显示功能：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image 
from cv_bridge import CvBridge

class ImageSub(Node):
    
    def __init__(self,name):
        super().__init__(name)
        self.sub_img = self.create_subscription(Image, "duckiebot_node/image", self.cb_image, 10)
        self.bridge = CvBridge()
    
    def cb_image(self,imgmsg):
        image = self.bridge.imgmsg_to_cv2(imgmsg, 'bgr8')
        cv2.imshow("image", image)
        cv2.waitKey(1)       
    
    
def main(args=None):
    rclpy.init(args=args)
    node = ImageSub(name="image_sub")
    rclpy.spin(node=node)
    rclpy.shutdown()

        修改setup.py，添加启动入口点配置：

entry_points={
        'console_scripts': [
            'image_pub = ros2_bag.image_pub:main',
            'image_sub = ros2_bag.image_sub:main',
        ],
    },

        返回工作空间，编译、配置并启动两个节点：

$ cd ~/ros2_ws
$ colcon build --packages-select ros2_bag
$ source install/setup.bash
$ ros2 run ros2_bag image_pub
$ ros2 run ros2_bag image_sub

2.2 从节点中记录主题数据包

        修改image_sub节点：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image 
from cv_bridge import CvBridge

#引入rosbag2_py包，用于处理bag文件所需的函数和结构
from rclpy.serialization import serialize_message
import rosbag2_py

class ImageSub(Node):
    
    def __init__(self,name):
        super().__init__(name)
        self.sub_img = self.create_subscription(Image, "duckiebot_node/image", self.cb_image, 10)
        self.bridge = CvBridge()
        #首先创建将用于写入包的writer对象，类型是SequentialWriter，它将按照收到的顺序将消息写入包中
        self.writer = rosbag2_py.SequentialWriter()
        
        #指定要创建的包的URI和格式(sqlite3)，其他选项保持默认值。
        storage_options = rosbag2_py._storage.StorageOptions(
            uri='my_bag',
            storage_id='sqlite3')
        #使用默认的转换选项，该选项将不执行转换，并以接收消息时使用的序列化格式存储消息。
        converter_options = rosbag2_py._storage.ConverterOptions('', '')
        #用writer对象以设定的配置打开包
        self.writer.open(storage_options, converter_options)
        #设置我们希望存储的主题，通过创建一个TopicMetadata对象并将其注册到编写器来完成。
        #指定此对象指定主题名称、主题数据类型和使用的序列化格式。
        topic_info = rosbag2_py._storage.TopicMetadata(
            name='duckiebot_node/image',
            type='sensor_msgs/msg/Image',
            serialization_format='cdr')
        self.writer.create_topic(topic_info)
    
    def cb_image(self,imgmsg):
        image = self.bridge.imgmsg_to_cv2(imgmsg, 'bgr8')
        cv2.imshow("image", image)
        cv2.waitKey(1)     
        #调用serialize_message()序列化消息并将结果传递给writer写入数据包中
        self.writer.write('duckiebot_node/image', serialize_message(imgmsg), self.get_clock().now().nanoseconds)  
    
    
def main(args=None):
    rclpy.init(args=args)
    node = ImageSub(name="image_sub")
    rclpy.spin(node=node)
    rclpy.shutdown()

        先启动image_pub节点，再启动image_sub节点

        image_sub节点需要rosbag2_py函数库，如果没有安装，提示No module named 'rosbag2_py'，需要先进行安装：

$ sudo apt install ros-rolling-rosbag2
$ source /opt/ros/rolling/setup.bash

        然后再运行image_sub节点，运行一段时间后停止节点，在当前目录下就有了my_bag目录

        可以看到目录下的文件格式，与通过命令行方式生成的主题数据包相同，ros2 bag info查看信息，也可以通过ros2 bag play回放主题数据。

        注1：rosbag2_py函数库默认可能没有安装，安装后需要执行source /opt/ros/rolling/setup.bash 才能正常使用，每次开启新终端都需要执行，也可以直接写入~/.bashrc中，下面两个例程会有同样的问题。

        注2：编码时写死了生成的数据包名称，程序不会自动覆盖，重复运行时会报文件已存在错误，所以每次重复执行，都需要删除之前生成的数据包文件，下面两个例程会有同样的问题。

2.3 从节点中记录合成数据包

        除了从主题订阅进行数据记录以外，也可以在数据发布之前直接存储，我们修改image_pub节点来实现：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import gym
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

import rosbag2_py
from rclpy.serialization import serialize_message
 
class ImagePub(Node):
 
    def __init__(self, name):
        super().__init__(name)
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        self.env.reset()
        self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
        self.timer = self.create_timer(0.05, self.timer_callback)
        self.bridge = CvBridge()
        
        self.writer = rosbag2_py.SequentialWriter()
        storage_options = rosbag2_py._storage.StorageOptions(
            uri='my_bag1',
            storage_id='sqlite3')
        converter_options = rosbag2_py._storage.ConverterOptions('', '')
        self.writer.open(storage_options, converter_options)
        topic_info = rosbag2_py._storage.TopicMetadata(
            name='duckiebot_node/image',
            type='sensor_msgs/msg/Image',
            serialization_format='cdr')
        self.writer.create_topic(topic_info)
    
    def timer_callback(self):
        action = self.env.action_space.sample()
        obs, reward, done, info = self.env.step(action)
    	imgmsg = self.bridge.cv2_to_imgmsg(obs, 'rgb8')
        self.pub_img.publish(imgmsg)
        self.writer.write('duckiebot_node/image', serialize_message(imgmsg), self.get_clock().now().nanoseconds)
        if done:
            self.env.reset()
 
def main(args=None):
    rclpy.init(args=args)
    node = ImagePub(name="image_pub")
    rclpy.spin(node=node)
    rclpy.shutdown()

        与从主题订阅存储类似，只是在发布之前就把数据进行存储，重新编译并运行image_pub一段时间后停止节点，在当前目录下生成my_bag1目录，可以使用ros2 bag play回放：

2.4 从可执行文件记录合成数据

        在ros2_bag源码目录下新建action.py，编辑内容如下：

#!/usr/bin/env python3

from rclpy.clock import Clock
from rclpy.duration import Duration
from rclpy.serialization import serialize_message
from duckietown_interface.msg import Twist2D

import rosbag2_py

def main(args=None):
    writer = rosbag2_py.SequentialWriter()

    storage_options = rosbag2_py._storage.StorageOptions(
        uri='my_bag2',
        storage_id='sqlite3')
    converter_options = rosbag2_py._storage.ConverterOptions('', '')
    writer.open(storage_options, converter_options)

    topic_info = rosbag2_py._storage.TopicMetadata(
        name='action',
        type='duckietown_interface/msg/Twist2D',
        serialization_format='cdr')
    writer.create_topic(topic_info)

    time_stamp = Clock().now()
    for ii in range(0, 100):
        data = Twist2D
        data.v = 0.3
        data.omage = 0.2
        writer.write(
            'action',
            serialize_message(data),
            time_stamp.nanoseconds)
        time_stamp += Duration(seconds=1)

if __name__ == '__main__':
    main()

        在setup.py中添加入口配置：

'simple_action = ros2_bag.action:main',

        返回工作空间编译配置并运行simple_action：