如何将RealSense两点测距Python脚本转换为ROS脚本？

核心问题排查 #

你之前转换后结果不准，大概率是以下原因：

• 订阅了未对齐的深度话题（比如/camera/depth/image_raw），导致深度帧和彩色帧的像素坐标系不匹配
• 未同步深度图像与相机内参的时间戳，使用了不同帧的数据
• 深度值单位未转换（ROS发布的深度图像单位是毫米，原脚本用的是米）

正确的ROS转换脚本 #

以下是基于realsense-ros包的rospy实现，严格对齐原脚本的逻辑：

import rospy
import math
import pyrealsense2 as rs
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
import message_filters

class ROSDistanceMeasure:
    def __init__(self):
        rospy.init_node('distance_measure_node', anonymous=True)
        self.bridge = CvBridge()
        self.color_intrin = None

        # 订阅对齐后的深度图像（已匹配彩色帧的分辨率和坐标系）和彩色相机内参
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        info_sub = message_filters.Subscriber('/camera/color/camera_info', CameraInfo)
        
        # 同步消息（时间差允许0.1秒）
        self.ts = message_filters.TimeSynchronizer([depth_sub, info_sub], 10)
        self.ts.registerCallback(self.callback)

        # 定义测量的像素点（和原脚本一致）
        self.x1 = 480
        self.y1 = 550
        self.x2 = 810
        self.y2 = self.y1

        rospy.spin()

    def callback(self, depth_msg, info_msg):
        # 初始化相机内参（仅执行一次）
        if self.color_intrin is None:
            self.color_intrin = rs.intrinsics()
            self.color_intrin.width = info_msg.width
            self.color_intrin.height = info_msg.height
            self.color_intrin.ppx = info_msg.K[2]
            self.color_intrin.ppy = info_msg.K[5]
            self.color_intrin.fx = info_msg.K[0]
            self.color_intrin.fy = info_msg.K[4]
            # 根据相机模型设置畸变参数
            if info_msg.distortion_model == "plumb_bob":
                self.color_intrin.model = rs.distortion.brown_conrady
            else:
                self.color_intrin.model = rs.distortion.none
            self.color_intrin.coeffs = list(info_msg.D)

        # 将ROS深度图像转为OpenCV格式（16UC1，单位毫米）
        depth_img = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding="16UC1")

        # 获取两点的深度值（转换为米）
        udist = depth_img[self.y1, self.x1] / 1000.0
        vdist = depth_img[self.y2, self.x2] / 1000.0

        # 跳过无效深度值（0表示无数据）
        if udist == 0 or vdist == 0:
            rospy.logwarn("Invalid depth value detected")
            return

        # 反投影为3D点
        point1 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x1, self.y1], udist)
        point2 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x2, self.y2], vdist)

        # 计算3D距离
        dist = math.sqrt(
            math.pow(point1[0] - point2[0], 2) +
            math.pow(point1[1] - point2[1], 2) +
            math.pow(point1[2] - point2[2], 2)
        )

        rospy.loginfo("Measured distance: {:.3f} meters".format(dist))

if __name__ == '__main__':
    try:
        ROSDistanceMeasure()
    except rospy.ROSInterruptException:
        pass

关键注意事项 #

• 话题确认：确保你的RealSense ROS节点发布了/camera/aligned_depth_to_color/image_raw话题，可通过rostopic list验证。如果没有，需要在启动RealSense节点时启用对齐功能（比如在launch文件中添加align_depth:=true）。
• 内参转换：从CameraInfo消息中提取的内参需要严格映射到rs.intrinsics结构，这是保证反投影准确的核心。
• 深度单位：ROS发布的深度图像是16位无符号整数，单位为毫米，必须除以1000转换为米，和原脚本的get_distance返回值单位一致。
• 消息同步：使用message_filters同步深度图像和相机内参，避免因时间差导致的帧不匹配问题。

内容的提问来源于stack exchange，提问作者Vuro H