ROS1 Stereo video saver

!README.md

Simple python ros noetic stereo image saver

Synchronous saving of left and right images from a stereo camera with ROS1.

Tested:

• Ubuntu 20.04
• Ros noetic

stereo_saver.py

from typing import List
from click import Tuple
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import message_filters
from dataclasses import dataclass
import numpy as np
from pathlib import Path
import cv2

@dataclass
class StereoImagePair:
    left: np.ndarray
    right: np.ndarray

@dataclass
class StereoSubs:
    left_topic: str
    rigth_topic:str

    def __post_init__(self):
        self.data = None
        self.sub_list: List[message_filters.Subscriber] = []
        self.bridge = CvBridge()

        self.sub_list.append(message_filters.Subscriber(self.left_topic, Image))
        self.sub_list.append(message_filters.Subscriber(self.rigth_topic, Image))

        ts = message_filters.ApproximateTimeSynchronizer(self.sub_list, queue_size=10, slop=0.2, allow_headerless=False)
        ts.registerCallback(self.data_callback)
    
    def data_callback(self, left_image, right_image):
        stereo_l_img = self.bridge.imgmsg_to_cv2(left_image,"bgr8")
        stereo_r_img = self.bridge.imgmsg_to_cv2(right_image,"bgr8")

        self.data = StereoImagePair(stereo_l_img, stereo_r_img)
    
    def get_data(self):
        return self.data
    
    def has_data(self):
        return self.data is not None

def main():

    rospy.init_node("stereo_saver", anonymous=True)

    left_topic = "/zedm/zed_node/left_raw/image_raw_color"
    right_topic = "/zedm/zed_node/right_raw/image_raw_color"
    subs_client = StereoSubs(left_topic, right_topic)


    idx = 0
    while not rospy.is_shutdown():
        uin = input("Press Enter to save image: ")
        if uin == "q":
            break
        if subs_client.has_data():
            stereo_img_pair = subs_client.get_data()
            print(f"Saving image {idx}")
            cv2.imwrite(f"./data/left_{idx}.png", stereo_img_pair.left)
            cv2.imwrite(f"./data/right_{idx}.png", stereo_img_pair.right)
            idx += 1



if __name__ == "__main__":
    main()