RealSense深度相机在Ubuntu18.04的ros环境下，保存同一时刻下深度图像和彩色图像

背景：Ubuntu18.04  ROS Melodic 已安装配置好RealSense相关程序，链接D435i相机后，得到如下Rostopic：

/camera/color/image_raw        # 彩色图像信息
/camera/depth/image_rect_raw    # 深度图像信息

于是写一个python程序：

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import cv2
import time
import os
from imageio import imsave

def depth2Gray(im_depth):
    """
    将深度图转至三通道8位灰度图
    (h, w, 3)
    """
    # 16位转8位
    x_max = np.max(im_depth)
    x_min = np.min(im_depth)
    if x_max == x_min:
        print('图像渲染出错 ...')
        raise EOFError
    
    k = 255 / (x_max - x_min)
    b = 255 - k * x_max

    ret = (im_depth * k + b).astype(np.uint8)
    return ret

def depth2RGB(im_depth):
    """
    将深度图转至三通道8位彩色图
    先将值为0的点去除，然后转换为彩图，然后将值为0的点设为红色
    (h, w, 3)
    im_depth: 单位 mm或m
    """
    im_depth = depth2Gray(im_depth)
    im_color = cv2.applyColorMap(im_depth, cv2.COLORMAP_JET)
    return im_color

def inpaint(img, missing_value=0):
    """
    Inpaint missing values in depth image.
    :param missing_value: Value to fill in teh depth image.
    """
    img = cv2.copyMakeBorder(img, 1, 1, 1, 1, cv2.BORDER_DEFAULT)
    mask = (img == missing_value).astype(np.uint8)

    scale = np.abs(img).max()
    img = img.astype(np.float32) / scale  # Has to be float32, 64 not supported.
    img = cv2.inpaint(img, mask, 1, cv2.INPAINT_NS)

    # Back to original size and value range.
    img = img[1:-1, 1:-1]
    img = img * scale

    return img 

def color_callback(color_msg):
    global color_image
    color_image = bridge.imgmsg_to_cv2(color_msg, desired_encoding="bgr8")

def depth_callback(depth_msg):
    global depth_image
    depth_image = bridge.imgmsg_to_cv2(depth_msg, desired_encoding="passthrough")

def run():
    rospy.init_node('realsense_capture_node')
    bridge = CvBridge()
    color_sub = rospy.Subscriber('/camera/color/image_raw', Image, color_callback)
    depth_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, depth_callback)

    # 创建保存图像的文件夹
    save_path = os.path.join(os.getcwd(), "data", time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime()))
    os.makedirs(save_path)

    # 创建实时图像显示窗口
    cv2.namedWindow("live", cv2.WINDOW_AUTOSIZE)
    # cv2.namedWindow("save", cv2.WINDOW_AUTOSIZE)
    saved_count = 0

    try:
        while not rospy.is_shutdown():
            # 在回调函数中接收图像数据，这样我们就可以在主循环中使用它们
            if color_image is not None and depth_image is not None:
                color_image_copy = color_image.copy()
                depth_image_copy = depth_image.copy()

                # 可视化深度图像
                depth_image_copy = inpaint(depth_image_copy)
                depth_image_color = depth2RGB(depth_image_copy)

                # 调整彩色图像的大小以匹配深度图像的分辨率
                color_image_copy_resized = cv2.resize(color_image_copy, (depth_image_color.shape[1], depth_image_color.shape[0]))
                cv2.imshow("live", np.hstack((color_image_copy_resized, depth_image_color)))
                key = cv2.waitKey(30)

                # 按 's' 键保存图像
                if key & 0xFF == ord('s'):
                    # 调整彩色图像的大小以匹配深度图像的分辨率
                    color_image_copy_resized = cv2.resize(color_image_copy, (depth_image_copy.shape[1], depth_image_copy.shape[0]))
                    
                    cv2.imwrite(os.path.join(save_path, "{:04d}r.png".format(saved_count)), color_image_copy_resized)
                    imsave(os.path.join(save_path, "{:04d}d.tiff".format(saved_count)), depth_image_copy)
                    saved_count += 1
                    cv2.imshow("live", np.hstack((color_image_copy_resized, depth_image_color)))# 原本是save

                # 按 'q' 键或按下 Esc 键退出程序
                if key & 0xFF == ord('q') or key == 27:
                    cv2.destroyAllWindows()
                    break
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    color_image = None
    depth_image = None
    bridge = CvBridge()
    run()

运行程序后，会出现一个窗口，里面有两个画面，按下s键就会保存一套照片，按下q键即退出