baoshi-measure-cargo/cargo_height_measure.py

import time

import cv2

from depth_common import (
    Settings,
    TemporalFilter,
    compute_roi_bounds,
    extract_depth_data,
    find_nearest_point,
    init_depth_pipeline,
    nearest_distance_in_roi,
)

# 键盘退出键
ESC_KEY = 27
# 打印间隔（秒）
PRINT_INTERVAL = 1  # seconds
# 从环境变量加载测量配置
SETTINGS = Settings.from_env()


def main():
    # 初始化时间滤波器，减少抖动
    temporal_filter = TemporalFilter(alpha=0.5)
    try:
        # 启动深度相机管线
        pipeline, depth_intrinsics, depth_profile = init_depth_pipeline()
        print("depth profile: ", depth_profile)
    except Exception as e:
        print(e)
        return
    last_print_time = time.time()
    while True:
        try:
            # 获取一帧深度数据
            frames = pipeline.wait_for_frames(100)
            if frames is None:
                continue
            depth_frame = frames.get_depth_frame()
            depth_data = extract_depth_data(depth_frame, SETTINGS, temporal_filter)
            if depth_data is None:
                continue
            # 计算中心 ROI 区域
            bounds = compute_roi_bounds(depth_data, depth_intrinsics, SETTINGS)
            if bounds is None:
                continue
            x_start, x_end, y_start, y_end, center_distance = bounds
            roi = depth_data[y_start:y_end, x_start:x_end]
            # 计算 ROI 内最近距离
            nearest_distance = nearest_distance_in_roi(roi, SETTINGS) or 0

            # 找出 ROI 内最近点用于可视化
            nearest_point = find_nearest_point(
                roi,
                x_start,
                y_start,
                SETTINGS,
                nearest_distance,
            )

            current_time = time.time()
            if current_time - last_print_time >= PRINT_INTERVAL:
                # 定期输出最近距离
                print(
                    "nearest distance in "
                    f"{SETTINGS.roi_width_cm}cm x {SETTINGS.roi_height_cm}cm area: ",
                    nearest_distance,
                )
                last_print_time = current_time

            # 生成彩色深度图并叠加标注
            depth_image = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
            depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET)

            cv2.rectangle(
                depth_image,
                (x_start, y_start),
                (x_end - 1, y_end - 1),
                (0, 255, 0),
                2,
            )
            if nearest_point is not None:
                cv2.circle(depth_image, nearest_point, 4, (0, 0, 0), -1)
                cv2.circle(depth_image, nearest_point, 6, (0, 255, 255), 2)
            # 文字标注当前测量值
            label = f"nearest: {nearest_distance} mm"
            cv2.putText(
                depth_image,
                label,
                (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.8,
                (255, 255, 255),
                2,
                cv2.LINE_AA,
            )
            center_label = f"center: {int(center_distance)} mm"
            cv2.putText(
                depth_image,
                center_label,
                (10, 60),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.8,
                (255, 255, 255),
                2,
                cv2.LINE_AA,
            )

            cv2.imshow("Depth Viewer", depth_image)
            key = cv2.waitKey(1)
            if key == ord('q') or key == ESC_KEY:
                break
        except KeyboardInterrupt:
            break
    # 清理窗口与相机资源
    cv2.destroyAllWindows()
    pipeline.stop()


if __name__ == "__main__":
    main()