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>
387 lines
17 KiB
Python
387 lines
17 KiB
Python
#!/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()
|