Files
d405-capture/d405_capture_server.py
liyang a038da124a 时间戳治理 + 工作距离/健康度指示 + 低分辨率提醒
- 禁用 librealsense global_time_enabled,时间戳改走相机硬件时钟(hardware_clock),
  消除软件对时造成的 ~150ms 回跳(旧录制 424×240@60 第 575 帧实测 −149ms)
- intrinsics.json 逐帧记录彩色/深度双流硬件时间戳、帧号、主机到达时刻,
  以及本段丢帧数与时间戳回跳数;README 补字段表与坑
- /api/status 增加中心距离 dist_cm(中央 1/3 区域深度中位数)与 drops/backjumps
- UI 状态栏:中心距离按 D405 最佳 7–50cm 变色提示;录制健康度实时显示;
  分辨率低于 848×480 时提示指尖/指缝细节风险;首次连接同步服务端流参数
- 实测 424×240@60:600 帧与 3595 帧两段均零丢帧零回跳,间隔恒定 16.697ms,
  彩色-深度时间戳差 <0.33ms

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
2026-09-15 17:10:15 +08:00

387 lines
17 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#!/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()