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 <topic_name> 记录单个主题消息

ros2 bag record -o <file_name> <topic_name_1> <topic_name_2>...<topic_name_n> 记录多个主题消息

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

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

1.  
   $ mkdir bag
2.  
   $ cd bag
3.  
   $ ros2 topic list
4.  
   $ 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 <file_dir_name>

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

1.3 ros2 bag play

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

命令格式：ros2 bag play <file_dir_name>

例如：ros2 bag play subset

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

2、ros2 bag record 编程实现

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

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

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

1.  
   $ cd ~/ros2_ws/src
2.  
   $ 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，实现图像发布功能：

1.  
   #!/usr/bin/env python3
2.  
    
3.  
   import rclpy
4.  
   from rclpy.node import Node
5.  
   import gym
6.  
   from pyglet.window import key
7.  
   from gym_duckietown.envs import DuckietownEnv
8.  
   import cv2
9.  
   from sensor_msgs.msg import Image
10.  
    from cv_bridge import CvBridge
11.  
     
12.  
    class ImagePub(Node):
13.  
     
14.  
    def __init__(self, name):
15.  
    super().__init__(name)
16.  
    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)
17.  
    self.env.reset()
18.  
    self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
19.  
    self.timer = self.create_timer(0.05, self.timer_callback)
20.  
    self.bridge = CvBridge()
21.  
     
22.  
    def timer_callback(self):
23.  
    action = self.env.action_space.sample()
24.  
    obs, reward, done, info = self.env.step(action)
25.  
    self.pub_img.publish(self.bridge.cv2_to_imgmsg(obs, 'rgb8'))
26.  
    if done:
27.  
    self.env.reset()
28.  
     
29.  
    def main(args=None):
30.  
    rclpy.init(args=args)
31.  
    node = ImagePub(name="image_pub")
32.  
    rclpy.spin(node=node)
33.  
    rclpy.shutdown()

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

1.  
   #!/usr/bin/env python3
2.  
    
3.  
   import rclpy
4.  
   from rclpy.node import Node
5.  
   import cv2
6.  
   import numpy as np
7.  
   from sensor_msgs.msg import Image
8.  
   from cv_bridge import CvBridge
9.  
    
10.  
    class ImageSub(Node):
11.  
     
12.  
    def __init__(self,name):
13.  
    super().__init__(name)
14.  
    self.sub_img = self.create_subscription(Image, "duckiebot_node/image", self.cb_image, 10)
15.  
    self.bridge = CvBridge()
16.  
     
17.  
    def cb_image(self,imgmsg):
18.  
    image = self.bridge.imgmsg_to_cv2(imgmsg, 'bgr8')
19.  
    cv2.imshow("image", image)
20.  
    cv2.waitKey(1)
21.  
     
22.  
     
23.  
    def main(args=None):
24.  
    rclpy.init(args=args)
25.  
    node = ImageSub(name="image_sub")
26.  
    rclpy.spin(node=node)
27.  
    rclpy.shutdown()

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

1.  
   entry_points={
2.  
   'console_scripts': [
3.  
   'image_pub = ros2_bag.image_pub:main',
4.  
   'image_sub = ros2_bag.image_sub:main',
5.  
   ],
6.  
   },

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

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

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

        修改image_sub节点：

1.  
   #!/usr/bin/env python3
2.  
    
3.  
   import rclpy
4.  
   from rclpy.node import Node
5.  
   import cv2
6.  
   import numpy as np
7.  
   from sensor_msgs.msg import Image
8.  
   from cv_bridge import CvBridge
9.  
    
10.  
    #引入rosbag2_py包，用于处理bag文件所需的函数和结构
11.  
    from rclpy.serialization import serialize_message
12.  
    import rosbag2_py
13.  
     
14.  
    class ImageSub(Node):
15.  
     
16.  
    def __init__(self,name):
17.  
    super().__init__(name)
18.  
    self.sub_img = self.create_subscription(Image, "duckiebot_node/image", self.cb_image, 10)
19.  
    self.bridge = CvBridge()
20.  
    #首先创建将用于写入包的writer对象，类型是SequentialWriter，它将按照收到的顺序将消息写入包中
21.  
    self.writer = rosbag2_py.SequentialWriter()
22.  
     
23.  
    #指定要创建的包的URI和格式(sqlite3)，其他选项保持默认值。
24.  
    storage_options = rosbag2_py._storage.StorageOptions(
25.  
    uri='my_bag',
26.  
    storage_id='sqlite3')
27.  
    #使用默认的转换选项，该选项将不执行转换，并以接收消息时使用的序列化格式存储消息。
28.  
    converter_options = rosbag2_py._storage.ConverterOptions('', '')
29.  
    #用writer对象以设定的配置打开包
30.  
    self.writer.open(storage_options, converter_options)
31.  
    #设置我们希望存储的主题，通过创建一个TopicMetadata对象并将其注册到编写器来完成。
32.  
    #指定此对象指定主题名称、主题数据类型和使用的序列化格式。
33.  
    topic_info = rosbag2_py._storage.TopicMetadata(
34.  
    name='duckiebot_node/image',
35.  
    type='sensor_msgs/msg/Image',
36.  
    serialization_format='cdr')
37.  
    self.writer.create_topic(topic_info)
38.  
     
39.  
    def cb_image(self,imgmsg):
40.  
    image = self.bridge.imgmsg_to_cv2(imgmsg, 'bgr8')
41.  
    cv2.imshow("image", image)
42.  
    cv2.waitKey(1)
43.  
    #调用serialize_message()序列化消息并将结果传递给writer写入数据包中
44.  
    self.writer.write('duckiebot_node/image', serialize_message(imgmsg), self.get_clock().now().nanoseconds)
45.  
     
46.  
     
47.  
    def main(args=None):
48.  
    rclpy.init(args=args)
49.  
    node = ImageSub(name="image_sub")
50.  
    rclpy.spin(node=node)
51.  
    rclpy.shutdown()

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

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

1.  
   $ sudo apt install ros-rolling-rosbag2
2.  
   $ 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节点来实现：

1.  
   #!/usr/bin/env python3
2.  
    
3.  
   import rclpy
4.  
   from rclpy.node import Node
5.  
   import gym
6.  
   from pyglet.window import key
7.  
   from gym_duckietown.envs import DuckietownEnv
8.  
   import cv2
9.  
   from sensor_msgs.msg import Image
10.  
    from cv_bridge import CvBridge
11.  
     
12.  
    import rosbag2_py
13.  
    from rclpy.serialization import serialize_message
14.  
     
15.  
    class ImagePub(Node):
16.  
     
17.  
    def __init__(self, name):
18.  
    super().__init__(name)
19.  
    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)
20.  
    self.env.reset()
21.  
    self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
22.  
    self.timer = self.create_timer(0.05, self.timer_callback)
23.  
    self.bridge = CvBridge()
24.  
     
25.  
    self.writer = rosbag2_py.SequentialWriter()
26.  
    storage_options = rosbag2_py._storage.StorageOptions(
27.  
    uri='my_bag1',
28.  
    storage_id='sqlite3')
29.  
    converter_options = rosbag2_py._storage.ConverterOptions('', '')
30.  
    self.writer.open(storage_options, converter_options)
31.  
    topic_info = rosbag2_py._storage.TopicMetadata(
32.  
    name='duckiebot_node/image',
33.  
    type='sensor_msgs/msg/Image',
34.  
    serialization_format='cdr')
35.  
    self.writer.create_topic(topic_info)
36.  
     
37.  
    def timer_callback(self):
38.  
    action = self.env.action_space.sample()
39.  
    obs, reward, done, info = self.env.step(action)
40.  
    imgmsg = self.bridge.cv2_to_imgmsg(obs, 'rgb8')
41.  
    self.pub_img.publish(imgmsg)
42.  
    self.writer.write('duckiebot_node/image', serialize_message(imgmsg), self.get_clock().now().nanoseconds)
43.  
    if done:
44.  
    self.env.reset()
45.  
     
46.  
    def main(args=None):
47.  
    rclpy.init(args=args)
48.  
    node = ImagePub(name="image_pub")
49.  
    rclpy.spin(node=node)
50.  
    rclpy.shutdown()

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

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

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

1.  
   #!/usr/bin/env python3
2.  
    
3.  
   from rclpy.clock import Clock
4.  
   from rclpy.duration import Duration
5.  
   from rclpy.serialization import serialize_message
6.  
   from duckietown_interface.msg import Twist2D
7.  
    
8.  
   import rosbag2_py
9.  
    
10.  
    def main(args=None):
11.  
    writer = rosbag2_py.SequentialWriter()
12.  
     
13.  
    storage_options = rosbag2_py._storage.StorageOptions(
14.  
    uri='my_bag2',
15.  
    storage_id='sqlite3')
16.  
    converter_options = rosbag2_py._storage.ConverterOptions('', '')
17.  
    writer.open(storage_options, converter_options)
18.  
     
19.  
    topic_info = rosbag2_py._storage.TopicMetadata(
20.  
    name='action',
21.  
    type='duckietown_interface/msg/Twist2D',
22.  
    serialization_format='cdr')
23.  
    writer.create_topic(topic_info)
24.  
     
25.  
    time_stamp = Clock().now()
26.  
    for ii in range(0, 100):
27.  
    data = Twist2D
28.  
    data.v = 0.3
29.  
    data.omage = 0.2
30.  
    writer.write(
31.  
    'action',
32.  
    serialize_message(data),
33.  
    time_stamp.nanoseconds)
34.  
    time_stamp = Duration(seconds=1)
35.  
     
36.  
    if __name__ == '__main__':
37.  
    main()

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

'simple_action = ros2_bag.action:main',

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

这篇好文章是转载于：学新通技术网