Closing a VideoWriter in a ROS2 node

Viewed 45

I have the following ROS2 node implemented in Python which subscribes to an image topic and writes a video from the stream using cv2.VideoWriter. I'd like to trigger this node to start writing as soon as a ros bag record command has been called, so that I can synchronize the video to other logs.

How do I do the following:

  1. start the node upon ros bag record being called.
  2. stop the video writing upon Ctrl-C or ros bag record process being terminated?
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)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
0 Answers
Related