ROS2 Python节点同步ros bag录制启停VideoWriter实现方法

我有一个Python实现的ROS2节点，功能是订阅图像话题，通过cv2.VideoWriter把接收到的图像流写入视频文件。现在需要实现执行ros bag record命令时立刻触发节点启动视频写入，保证生成的视频和其他日志数据时间完全同步。

• 调用ros bag record命令时，自动启动节点的视频写入流程
• 触发Ctrl-C中断或者ros bag record进程终止时，自动停止视频写入，正确释放资源

import rclpy
from rclpy import qos
from rclpy.node import Node
from sensor_msgs.msg import Image
import rosbag2_py
from datetime import datetime
import os
import cv_bridge
import cv2

class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.subscriber = self.create_subscription(
            Image, "/zedm/zed_node/left/image_rect_color", self._zed_image_cb, 10
        )
        self.bridge = cv_bridge.CvBridge()
        
        self.vid_writer = None

    def init_writer(self, msg):
        video_format = 'mp4'
        size = (msg.width, msg.height)
        fourcc = cv2.VideoWriter_fourcc(*"H264")
        now = datetime.now()
        tstamp = now.strftime('%Y%m%d_%H_%M_%S')
        self.vid_writer = cv2.VideoWriter(
            filename=f'zed_image_left_{tstamp}.{video_format}',
            apiPreference=cv2.CAP_FFMPEG,
            fourcc=fourcc,
            fps=60,
            frameSize=size,
        )

    def _zed_image_cb(self, msg):
        self.get_logger().info("got image")
        np_img = self.bridge.imgmsg_to_cv2(msg)  # 300, 640, 4
        if not self.vid_writer:
            self.init_writer(msg)
        self.vid_writer.write(np_img[:, :, :3])

    def destroy_node(self):
        super().destroy_node()
        self.get_logger().info("releasing video")
        # finish the video writing.
        if self.vid_writer:
            self.vid_writer.release()


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

实现逻辑 #

ROS2的rosbag2录制进程启动、停止时都会向/rosout话题发送状态日志，只需要在节点中订阅该话题，匹配到录制开始的日志就初始化视频写入器开始存帧，匹配到录制结束日志、或者节点收到中断信号时就停止写入释放资源即可，不需要修改ros bag record的原有调用逻辑。

修改后可直接运行的代码 #

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image
from rcl_interfaces.msg import Log
from datetime import datetime
import cv_bridge
import cv2
import os

class SyncedVideoRecorder(Node):

    def __init__(self):
        super().__init__('synced_video_recorder')
        self.bridge = cv_bridge.CvBridge()
        self.vid_writer = None
        self.is_recording = False

        # 订阅图像话题，QOS和传感器数据配置对齐避免丢帧
        self.img_sub = self.create_subscription(
            Image, 
            "/zedm/zed_node/left/image_rect_color", 
            self._zed_image_cb, 
            qos_profile=QoSProfile(
                reliability=ReliabilityPolicy.BEST_EFFORT,
                history=HistoryPolicy.KEEP_LAST,
                depth=30
            )
        )

        # 订阅rosout监听rosbag录制状态
        self.rosout_sub = self.create_subscription(
            Log,
            "/rosout",
            self._rosout_cb,
            10
        )

    def _rosout_cb(self, msg):
        # 检测到rosbag开始录制，触发写入启动
        if msg.name == "rosbag2_recorder" and "Recording started" in msg.msg:
            self.get_logger().info("检测到rosbag录制启动，同步开始视频写入")
            self.is_recording = True
        # 检测到rosbag录制结束，触发资源释放
        if msg.name == "rosbag2_recorder" and "Recording stopped" in msg.msg:
            self.get_logger().info("检测到rosbag录制停止，结束视频写入")
            self.stop_writer()

    def init_writer(self, msg):
        video_format = 'mp4'
        size = (msg.width, msg.height)
        fourcc = cv2.VideoWriter_fourcc(*"avc1")
        now = datetime.now()
        tstamp = now.strftime('%Y%m%d_%H_%M_%S')
        output_path = os.path.join(os.getcwd(), f'zed_image_left_{tstamp}.{video_format}')
        self.vid_writer = cv2.VideoWriter(
            filename=output_path,
            apiPreference=cv2.CAP_FFMPEG,
            fourcc=fourcc,
            fps=60,
            frameSize=size,
        )
        if not self.vid_writer.isOpened():
            self.get_logger().error("视频写入器初始化失败，请检查编码配置")
            self.is_recording = False

    def stop_writer(self):
        self.is_recording = False
        if self.vid_writer:
            self.vid_writer.release()
            self.vid_writer = None
            self.get_logger().info("视频文件已保存，写入资源已释放")

    def _zed_image_cb(self, msg):
        if not self.is_recording:
            return
        # 直接指定输出编码为bgr8，避免手动切通道导致的颜色异常
        np_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
        if not self.vid_writer:
            self.init_writer(msg)
        if self.vid_writer and self.vid_writer.isOpened():
            self.vid_writer.write(np_img)

    def destroy_node(self):
        self.stop_writer()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    recorder_node = SyncedVideoRecorder()
    try:
        rclpy.spin(recorder_node)
    except KeyboardInterrupt:
        pass
    finally:
        recorder_node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

使用注意 #

• 先启动该视频录制节点，再执行ros bag record命令即可自动同步启动写入，不需要额外传参
• 不管是Ctrl-C终止rosbag进程、rosbag录制正常结束、还是手动终止视频节点，都会自动触发资源释放，不会生成损坏的视频文件
• 视频文件默认保存在执行命令的当前目录，时间戳和同批次rosbag文件完全对齐

内容的提问来源于stack exchange，提问作者ejang