#!/usr/bin/env python3 """D405 采集台:实时预览 + 网页 UI 控制(d405_ui.html)。 http://localhost:8405 可调:分辨率/帧率(重启流)、预览画质、深度显示范围、预览布局、 自动/手动曝光与增益、定时录制、是否保存深度。 录制输出 captures/<时间戳>/: color.mp4 彩色视频(按当前分辨率/帧率) depth/%06d.png 16bit 深度 PNG(0.1mm,已对齐彩色;可关) intrinsics.json 内参 + depth_scale + 逐帧双流硬件时间戳/帧号 + 丢帧统计 + 录制参数 用法: .venv-d405/bin/python d405_capture_server.py [--port 8405] """ import argparse, time, threading, datetime, pathlib, json from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer import numpy as np import cv2 import pyrealsense2 as rs HERE = pathlib.Path(__file__).resolve().parent CAPTURE_ROOT = HERE / "captures" UI_FILE = HERE / "d405_ui.html" latest_jpeg = None frame_seq = 0 frame_lock = threading.Condition() state_lock = threading.RLock() cfg = { # 可从 UI 修改 "width": 848, "height": 480, "fps": 30, # 改动需重启流 "jpeg_quality": 80, "depth_max": 1.0, "layout": "side", "auto_exposure": True, "exposure": 8000, "gain": 64, } st = { # 只读状态 "stream_ok": False, "device": "-", "error": "", "fps_actual": 0.0, "depth_fill": 0.0, "dist_cm": 0.0, "on": False, "frames": 0, "elapsed": 0.0, "duration": 0, "drops": 0, "backjumps": 0, "last_saved": None, } rec = {"dir": None, "writer": None, "t0": 0.0, "save_depth": True, # 逐帧时间记录:双流硬件时间戳 + 帧号 + 主机单调钟到达时刻 "color_ts": [], "depth_ts": [], "color_fn": [], "depth_fn": [], "arrival_ns": [], "prev_cts": None, "prev_cfn": None, "ts_domain": ""} restart_flag = threading.Event() sensor_dirty = threading.Event() intrinsics = {} def start_recording(duration, save_depth, out_dir=""): with state_lock: if st["on"]: return "已在录制中" if not st["stream_ok"]: return "视频流未就绪" root = CAPTURE_ROOT if out_dir: root = pathlib.Path(out_dir).expanduser() if not root.is_absolute(): root = CAPTURE_ROOT.parent / root # 相对路径挂在 hardware/ 下 try: root.mkdir(parents=True, exist_ok=True) probe = root / ".w" probe.touch(); probe.unlink() except OSError as e: return f"保存位置不可写:{root}({e})" d = root / f"{datetime.datetime.now():%Y%m%d_%H%M%S}" if save_depth: (d / "depth").mkdir(parents=True, exist_ok=True) else: d.mkdir(parents=True, exist_ok=True) rec.update(dir=d, t0=time.time(), save_depth=save_depth, color_ts=[], depth_ts=[], color_fn=[], depth_fn=[], arrival_ns=[], prev_cts=None, prev_cfn=None, writer=cv2.VideoWriter(str(d / "color.mp4"), cv2.VideoWriter_fourcc(*"mp4v"), cfg["fps"], (cfg["width"], cfg["height"]))) st.update(on=True, frames=0, elapsed=0.0, duration=duration, drops=0, backjumps=0) print(f"[record] start -> {d} (duration={duration}s, depth={save_depth})", flush=True) return None def stop_recording(): with state_lock: if not st["on"]: return st["on"] = False rec["writer"].release() d, n = rec["dir"], st["frames"] meta = dict(intrinsics) meta.update(frames=n, width=cfg["width"], height=cfg["height"], fps_nominal=cfg["fps"], fps_actual=round(n / max(time.time() - rec["t0"], 1e-6), 2), save_depth=rec["save_depth"], # 时间来源说明:global_time 已禁用,时间戳来自相机硬件时钟 # (单调,不受主机 NTP/对时算法影响,可用于速度/加速度计算) time_source="device hardware clock, global_time disabled", timestamp_domain=rec["ts_domain"], timestamps_ms=rec["color_ts"], # 兼容旧字段名=彩色流 depth_timestamps_ms=rec["depth_ts"], color_frame_numbers=rec["color_fn"], depth_frame_numbers=rec["depth_fn"], host_arrival_monotonic_ns=rec["arrival_ns"], dropped_frames=st["drops"], timestamp_backjumps=st["backjumps"], sync_note="每行数据来自同一 frameset;D405 彩色取自左成像器," "与深度同一次曝光,硬件级同步") (d / "intrinsics.json").write_text(json.dumps(meta, indent=1)) st["last_saved"] = f"{d}({n} 帧)" print(f"[record] saved {d} ({n} frames)", flush=True) def apply_sensor_options(profile): sensor = profile.get_device().first_depth_sensor() try: sensor.set_option(rs.option.enable_auto_exposure, 1 if cfg["auto_exposure"] else 0) if not cfg["auto_exposure"]: sensor.set_option(rs.option.exposure, float(cfg["exposure"])) sensor.set_option(rs.option.gain, float(cfg["gain"])) except Exception as e: st["error"] = f"曝光设置失败: {e}" def capture_loop(): global latest_jpeg, frame_seq, intrinsics while True: pipe = rs.pipeline() c = rs.config() with state_lock: w, h, fps = cfg["width"], cfg["height"], cfg["fps"] c.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps) c.enable_stream(rs.stream.depth, w, h, rs.format.z16, fps) try: profile = pipe.start(c) except Exception as e: with state_lock: st["stream_ok"] = False st["error"] = f"{w}x{h}@{fps} 启动失败:{e}(2 秒后自动重试)" print(f"[stream] start failed: {e}, retry in 2s", flush=True) restart_flag.wait(timeout=2) # 自动重试;UI 改参数可立即触发 restart_flag.clear() continue align = rs.align(rs.stream.color) scale = profile.get_device().first_depth_sensor().get_depth_scale() dev = profile.get_device() # 禁用 global_time:librealsense 的软件对时算法会造成时间戳回跳 # (典型 ~150ms),禁用后走相机硬件时钟,严格单调 for s in dev.query_sensors(): if s.supports(rs.option.global_time_enabled): s.set_option(rs.option.global_time_enabled, 0) apply_sensor_options(profile) frames = align.process(pipe.wait_for_frames()) intr = frames.get_color_frame().profile.as_video_stream_profile().intrinsics with state_lock: intrinsics = {"fx": intr.fx, "fy": intr.fy, "cx": intr.ppx, "cy": intr.ppy, "depth_scale_m": scale, "aligned_to": "color"} st.update(stream_ok=True, error="", device=f"{dev.get_info(rs.camera_info.name)} " f"#{dev.get_info(rs.camera_info.serial_number)}") n_win, t_win = 0, time.time() misses = 0 try: while not restart_flag.is_set(): if sensor_dirty.is_set(): sensor_dirty.clear() apply_sensor_options(profile) try: raw = pipe.wait_for_frames(5000) misses = 0 except RuntimeError as e: misses += 1 print(f"[stream] frame timeout ({misses}): {e}", flush=True) if misses >= 2: # 连续超时 → 整个流重启 break continue frames = align.process(raw) cf, df = frames.get_color_frame(), frames.get_depth_frame() color = np.asanyarray(cf.get_data()) depth_raw = np.asanyarray(df.get_data()) with state_lock: if st["on"]: rec["writer"].write(color) if rec["save_depth"]: cv2.imwrite(str(rec["dir"] / "depth" / f"{st['frames']:06d}.png"), depth_raw) cts, dts = cf.get_timestamp(), df.get_timestamp() cfn, dfn = cf.get_frame_number(), df.get_frame_number() if not rec["ts_domain"]: rec["ts_domain"] = str(cf.get_frame_timestamp_domain()) if rec["prev_cts"] is not None and cts <= rec["prev_cts"]: st["backjumps"] += 1 if rec["prev_cfn"] is not None and cfn > rec["prev_cfn"] + 1: st["drops"] += int(cfn - rec["prev_cfn"] - 1) rec["prev_cts"], rec["prev_cfn"] = cts, cfn rec["color_ts"].append(cts) rec["depth_ts"].append(dts) rec["color_fn"].append(int(cfn)) rec["depth_fn"].append(int(dfn)) rec["arrival_ns"].append(time.monotonic_ns()) st["frames"] += 1 st["elapsed"] = time.time() - rec["t0"] if st["duration"] > 0 and st["elapsed"] >= st["duration"]: stop_recording() recording, nrec, el = st["on"], st["frames"], st["elapsed"] q, dmax, layout = (cfg["jpeg_quality"], cfg["depth_max"], cfg["layout"]) # 统计 n_win += 1 now = time.time() if now - t_win >= 1.0: hh, ww = depth_raw.shape roi = depth_raw[hh // 3:2 * hh // 3, ww // 3:2 * ww // 3] nz = roi[roi > 0] with state_lock: st["fps_actual"] = n_win / (now - t_win) st["depth_fill"] = 100.0 * np.count_nonzero(depth_raw) \ / depth_raw.size st["dist_cm"] = float(np.median(nz)) * scale * 100 \ if nz.size else 0.0 n_win, t_win = 0, now # 预览合成 panels = [] if layout in ("side", "color"): panels.append(color) if layout in ("side", "depth"): dm = np.clip(depth_raw.astype(np.float32) * scale, 0, dmax) vis = cv2.applyColorMap((dm / dmax * 255).astype(np.uint8), cv2.COLORMAP_TURBO) vis[depth_raw == 0] = 0 panels.append(vis) combo = np.hstack(panels) if len(panels) > 1 else panels[0] label = f"{datetime.datetime.now():%H:%M:%S}" if recording: label += f" REC {nrec} f {el:.0f}s" cv2.circle(combo, (12, 14), 6, (0, 0, 255), -1) cv2.putText(combo, label, (24, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 255) if recording else (255, 255, 255), 1) ok, jpg = cv2.imencode(".jpg", combo, [cv2.IMWRITE_JPEG_QUALITY, q]) if ok: with frame_lock: latest_jpeg = jpg.tobytes() frame_seq += 1 frame_lock.notify_all() finally: stop_recording() with state_lock: st["stream_ok"] = False pipe.stop() restart_flag.clear() class Handler(BaseHTTPRequestHandler): def log_message(self, *a): pass def _json(self, obj, code=200): body = json.dumps(obj).encode() self.send_response(code) self.send_header("Content-Type", "application/json") self.end_headers() self.wfile.write(body) def _body(self): n = int(self.headers.get("Content-Length") or 0) return json.loads(self.rfile.read(n) or b"{}") def do_POST(self): if self.path == "/api/start": b = self._body() err = start_recording(int(b.get("duration", 0)), bool(b.get("save_depth", True)), str(b.get("out_dir", "")).strip()) self._json({"ok": err is None, "error": err}) elif self.path == "/api/stop": stop_recording() self._json({"ok": True}) elif self.path == "/api/config": b = self._body() need_restart = False with state_lock: for k in ("width", "height", "fps"): if k in b and int(b[k]) != cfg[k]: if st["on"]: self._json({"error": "录制中不能改视频流参数"}) return cfg[k] = int(b[k]) need_restart = True for k in ("jpeg_quality", "exposure", "gain"): if k in b: cfg[k] = int(b[k]) if "depth_max" in b: cfg["depth_max"] = float(b["depth_max"]) if "layout" in b: cfg["layout"] = str(b["layout"]) if "auto_exposure" in b: cfg["auto_exposure"] = bool(b["auto_exposure"]) st["error"] = "" if need_restart: restart_flag.set() deadline = time.time() + 8 while time.time() < deadline: time.sleep(0.2) with state_lock: if st["stream_ok"]: break if st["error"]: self._json({"error": st["error"]}) return with state_lock: self._json({"ok": st["stream_ok"], "error": st["error"] or None}) else: sensor_dirty.set() self._json({"ok": True}) else: self.send_error(404) def do_GET(self): if self.path == "/": self.send_response(200) self.send_header("Content-Type", "text/html; charset=utf-8") self.end_headers() self.wfile.write(UI_FILE.read_bytes()) elif self.path == "/api/status": with state_lock: out = dict(st) out.update(width=cfg["width"], height=cfg["height"], fps=cfg["fps"]) self._json(out) elif self.path.startswith("/frame.jpg"): # 等到「新」一帧再返回(最多 1.5s),前端拉帧循环靠它对齐相机节奏 with frame_lock: seq0 = frame_seq frame_lock.wait_for(lambda: frame_seq != seq0, timeout=1.5) buf = latest_jpeg if buf is None: self.send_error(503, "no frame yet") return self.send_response(200) self.send_header("Content-Type", "image/jpeg") self.send_header("Cache-Control", "no-store") self.send_header("Content-Length", str(len(buf))) self.end_headers() try: self.wfile.write(buf) except (BrokenPipeError, ConnectionResetError): pass elif self.path.startswith("/stream"): # 兼容保留的 MJPEG 端点(UI 已改用 /frame.jpg 拉帧自愈) self.send_response(200) self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=f") self.end_headers() try: while True: with frame_lock: frame_lock.wait(timeout=2) buf = latest_jpeg if buf is None: continue self.wfile.write(b"--f\r\nContent-Type: image/jpeg\r\n\r\n" + buf + b"\r\n") except (BrokenPipeError, ConnectionResetError): pass else: self.send_error(404) if __name__ == "__main__": ap = argparse.ArgumentParser() ap.add_argument("--port", type=int, default=8405) args = ap.parse_args() threading.Thread(target=capture_loop, daemon=True).start() print(f"[live] http://localhost:{args.port}", flush=True) ThreadingHTTPServer(("127.0.0.1", args.port), Handler).serve_forever()