Saves color and depth images in TUM RGB-D format. Tested on ROS melodic.

save_tum_rgbd.py

#!/usr/bin/python
import cv2
import cv_bridge
import datetime
import message_filters
import numpy as np
import os
import rospy
import sys
import sensor_msgs


class SaveTumRGBD(object):
    def __init__(self, output_dir, depth_scale, rgb_full_topic_name, depth_full_topic_name, approximate_sync):
        self._output_dir = output_dir
        self._depth_scale = depth_scale
        self._rgb_sub = message_filters.Subscriber(rgb_full_topic_name, sensor_msgs.msg.Image)
        self._depth_sub = message_filters.Subscriber(depth_full_topic_name, sensor_msgs.msg.Image)
        sync = self._create_sync(approximate_sync)
        sync.registerCallback(self._images_callback)

    def _create_sync(self, approximate):
        if approximate:
            return message_filters.ApproximateTimeSynchronizer((self._rgb_sub, self._depth_sub), queue_size=100, slop=0.1)
        else:
            return message_filters.TimeSynchronizer((self._rgb_sub, self._depth_sub), queue_size=100)

    def _images_callback(self, rgb_msg, depth_msg):
        bridge = cv_bridge.CvBridge()
        sec = rgb_msg.header.stamp.to_sec()
        rgb_img = bridge.imgmsg_to_cv2(rgb_msg)
        rgb_filename = os.path.join("rgb", "{}.png".format(sec))
        rgb_path = os.path.join(self._output_dir, rgb_filename)
        cv2.imwrite(rgb_path, rgb_img)
        with open(os.path.join(self._output_dir, "rgb.txt"), "a") as f:
            f.write("{} {}\n".format(sec, rgb_filename))
        
        depth_img = (bridge.imgmsg_to_cv2(depth_msg) * self._depth_scale).astype(np.uint16)
        depth_filename = os.path.join("depth", "{}.png".format(sec))
        depth_path = os.path.join(self._output_dir, depth_filename)
        cv2.imwrite(depth_path, depth_img)
        with open(os.path.join(self._output_dir, "depth.txt"), "a") as f:
            f.write("{} {}\n".format(sec, depth_filename))


def main():
    rospy.init_node('save_tum_rgbd')

    camera = rospy.get_param('~camera', "/camera")
    rgb_topic_name = rospy.get_param('~rgb', "rgb/image_raw")
    depth_topic_name = rospy.get_param('~depth', "depth/image_raw")
    rgb_full_topic_name = "{}/{}".format(camera, rgb_topic_name)
    depth_full_topic_name = "{}/{}".format(camera, depth_topic_name)
    print(rgb_full_topic_name, depth_full_topic_name)

    output_dir = rospy.get_param('~output_dir', "tum_rgbd_{}".format(datetime.datetime.now().strftime("%Y%m%d-%H%M%S")))
    depth_scale = rospy.get_param('~depth_scale', 5000)
    approximate_sync = rospy.get_param('~approximate_sync', False)

    os.makedirs(os.path.join(output_dir, "rgb"))
    os.makedirs(os.path.join(output_dir, "depth"))
    extractor = SaveTumRGBD(output_dir, depth_scale, rgb_full_topic_name, depth_full_topic_name, approximate_sync)
    rospy.spin()


if __name__ == '__main__':
    main()