import os
import time
import pyrealsense2 as rs
import numpy as np
import cv2

TARGET_SERIAL = "243122075614"
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:
    raise RuntimeError("未检测到任何 RealSense 相机，请检查连接")

selected_device = None
for dev in devices:
    if dev.get_info(rs.camera_info.serial_number) == TARGET_SERIAL:
        selected_device = dev
        break
if selected_device is None:
    raise RuntimeError(f"未找到序列号为 {TARGET_SERIAL} 的 RealSense 相机")

pipeline = rs.pipeline()
config = rs.config()
config.enable_device(TARGET_SERIAL)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pipeline.start(config)

align = rs.align(rs.stream.color)

# =============================
# 3. 数据采集流程
# =============================

# 捕获对齐后的彩色图像
for i in range(10):
    try:
        frames = pipeline.wait_for_frames(timeout_ms=5000)
        if frames:
            break
    except RuntimeError:
        print("⚠️ 读取帧超时，重试中...")
        time.sleep(1)
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()

if not color_frame:
    print("未获取彩色图像，跳过")
    exit(-1)

color_image = np.asanyarray(color_frame.get_data())
img_path = f"color_image.png"
cv2.imwrite(img_path, color_image)
print(f"保存图像 {img_path}")

# 停止RealSense
pipeline.stop()
print("\n=== 数据采集完成 ===")