ROS中Realsense深度相机话题与消息解析

GitHub - IntelRealSense/realsense-ros: Intel® RealSense™ ROS Wrapper for Depth Camera

话题与消息类型

Realsense相机除了可以提供RGB图像，还可以提供深度图，深度图信息的话题为realsense/depth_camera/depth/image_raw，消息类型为sensor_msgs/Image。
sensor_msgs/Image Message

# This message contains an uncompressed image
# (0, 0) is at top-left corner of image
#

Header header        # Header timestamp should be acquisition time of image
                     # Header frame_id should be optical frame of camera
                     # origin of frame should be optical center of camera
                     # +x should point to the right in the image
                     # +y should point down in the image
                     # +z should point into to plane of the image
                     # If the frame_id here and the frame_id of the CameraInfo
                     # message associated with the image conflict
                     # the behavior is undefined

uint32 height         # image height, that is, number of rows
uint32 width          # image width, that is, number of columns

# The legal values for encoding are in file src/image_encodings.cpp
# If you want to standardize a new string format, join
# ros-users@lists.sourceforge.net and send an email proposing a new encoding.

string encoding       # Encoding of pixels -- channel meaning, ordering, size
                      # taken from the list of strings in include/sensor_msgs/image_encodings.h

uint8 is_bigendian    # is this data bigendian?
uint32 step           # Full row length in bytes
uint8[] data          # actual matrix data, size is (step * rows)

消息内容解析与转换

可用rosbag录制depth/image_raw，而后转为txt，摘取部分信息如下：

header: 
  seq: 60363
  stamp: 
    secs: 4021
    nsecs: 264000000
  frame_id: "depth_camera_base"
height: 480
width: 640
encoding: "32FC1"
is_bigendian: 0
step: 2560
data: [0, 0, 192, 127, 0, 0, 192, 127, ... # 480*640*4=1228800

可以看出，深度信息数据的编码形式为32FC1（每个像素值都会用一个32位的浮点数来表示），在data中，每四个字节代表一个像素中的深度信息（例如[0,0,192,127]即0x0000C07F代表NaN)，因此data的长度为heightwidth4。
通过CvBridge，可将ROS的图像消息与OpenCV图像相互转换：
cv_bridge/Tutorials - ROS Wiki
在python中实现如下：

from cv_bridge import CvBridge
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(image_message, desired_encoding='passthrough')

from cv_bridge import CvBridge
bridge = CvBridge()
image_message = bridge.cv2_to_imgmsg(cv_image, encoding="passthrough")

在本项目中的demo如下：

#!/usr/bin/env python

import rospy
import argparse
import struct
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import matplotlib.pyplot as plt

class DepthSubscriber:
    def __init__(self, row, col, output_file):
        rospy.init_node('depth_subscriber', anonymous=True)
        self.bridge = CvBridge()
        self.depth_sub = rospy.Subscriber('/typhoon_h480_0/realsense/depth_camera/depth/image_raw', Image, self.depth_callback)
        self.row = row
        self.col = col
        self.output_file = output_file

    def depth_callback(self, depth_msg):
        try:
            with open(self.output_file, 'w') as file:
                file.write(str(depth_msg))
            print("Length of depth_msg data: ", len(depth_msg.data)) # 1228800
            # 将深度图像转换为OpenCV格式
            depth_image = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding="passthrough")

            '''
            # 以下是另一种方法，与bridge.imgmsg_to_cv2函数等价，展开描述如何解析深度图像
            # 将每4个字节的二进制数据解析为float
            depth_values_ori = []
            for i in range(0, len(depth_msg.data), 4):
                depth_value_bytes = bytes(depth_msg.data[i:i+4])
                depth_value_float = struct.unpack('f', depth_value_bytes)[0]
                depth_values_ori.append(depth_value_float)
            # print("解析后的深度值:", depth_values_ori)
            
            # 将depth_values_ori按照depth_msg的行列数转换为深度图像
            depth_values = np.array(depth_values_ori)
            depth_image = depth_values.reshape(depth_msg.height, depth_msg.width)
            # 存储转换后的深度图像为一个新的txt文件
            with open('depth_image.txt', 'w') as file:
                for i in range(480):
                    for j in range(640):
                        file.write(str(depth_image[i][j]) + ' ')
                    file.write('\n')
            # 画出深度图像
            plt.imshow(depth_image, cmap='gray')
            plt.savefig('depth_image.png')
            '''

        except CvBridgeError as e:
            rospy.logerr(e)
            return

        # 获取特定像素的深度值
        depth_value = depth_image[self.row, self.col]

        
        rospy.loginfo("Depth value at pixel ({}, {}): {}".format(self.row, self.col, depth_value))


if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Depth Subscriber with row and col parameters for Intel Realsense')
    parser.add_argument('--row', type=int, default=240, help='Row value for pixel (default: 240)')
    parser.add_argument('--col', type=int, default=320, help='Column value for pixel (default: 320)')
    parser.add_argument('--output_file', type=str, default='depth_msg.txt', help='Output file for depth value (default: depth_value.txt)')
    args = parser.parse_args()

    try:
        depth_subscriber = DepthSubscriber(args.row, args.col, args.output_file)
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

解析出的图像深度值（单位为米）：

Realsense D435i的最小深度距离(Min-Z)为0.105m，最大范围约10m。

画出的深度图（这里有点小问题，图上越近颜色越深了，但实际上应该是越近越浅）：