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 imsavedef 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 EOFErrork = 255 / (x_max - x_min)b = 255 - k * x_maxret = (im_depth * k + b).astype(np.uint8)return retdef 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_colordef 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 * scalereturn img def color_callback(color_msg):global color_imagecolor_image = bridge.imgmsg_to_cv2(color_msg, desired_encoding="bgr8")def depth_callback(depth_msg):global depth_imagedepth_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 = 0try: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 += 1cv2.imshow("live", np.hstack((color_image_copy_resized, depth_image_color)))# 原本是save# 按 'q' 键或按下 Esc 键退出程序if key & 0xFF == ord('q') or key == 27:cv2.destroyAllWindows()breakexcept rospy.ROSInterruptException:passif __name__ == '__main__':color_image = Nonedepth_image = Nonebridge = CvBridge()run()

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