接入 Studio 多点标定的会话置零与状态记录

支持显式无载荷置零、处理线程内切换模型及完整标定包快照。shared、界面检查与兼容测试通过;Studio 全量仍有一项回放停止时序测试失败。
This commit is contained in:
ykcheungab
2026-10-19 09:47:51 +08:00
parent bc5dfc035d
commit 31750f0f84
19 changed files with 514 additions and 32 deletions
@@ -3,6 +3,7 @@
from __future__ import annotations
import logging
import statistics
import threading
import time
from collections import deque
@@ -323,6 +324,12 @@ class FingerAcquisitionWorker(RecordSinkMixin):
self._end_reason: str | None = None
self._phase: str = "idle"
self._pending_calibration: str | None = None
self._pending_device_id: str | None = None
self._pending_zero = False
self._cancel_zero = False
self._zero_samples: list[tuple[float, float, float]] | None = None
self._zero_model = None
self._zero_reference: dict = {}
#: Path and device serial for the **next** ``start()`` (live open kwargs).
self._session_cal_path: str | None = None
self._session_device_id: str | None = None
@@ -425,6 +432,11 @@ class FingerAcquisitionWorker(RecordSinkMixin):
# zeroing request must not fire into the new session's frames.
self._calibration_version = ""
self._pending_reset = False
self._pending_zero = False
self._cancel_zero = False
self._zero_samples = None
self._zero_reference = {}
self._pending_calibration = None
# Nor any filter state: until this session's sensor has been
# configured and asked, there is nothing to report about it.
self._filter_state = {}
@@ -499,6 +511,9 @@ class FingerAcquisitionWorker(RecordSinkMixin):
never computed: keeping the pre-pause reference frame would report the hold
itself as a large deformation.
"""
with self._lock:
if self._zero_samples is not None:
self._cancel_zero = True
pipe = self._pipe
if pipe is None and self._pause.is_set():
with self._lock:
@@ -531,6 +546,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
if self._session_id <= 0:
raise RuntimeError("Finger runtime is not running")
self._pending_reset = True
self._cancel_zero = True
def _consume_pending_reset(
self, runtime, frame_id: int, mono_t: float, *, pipeline=None
@@ -563,10 +579,53 @@ class FingerAcquisitionWorker(RecordSinkMixin):
if self._on_reset_applied is not None:
self._on_reset_applied(int(frame_id), float(mono_t), int(session))
def set_pending_calibration(self, path: str | Path | None) -> None:
def set_pending_calibration(self, path: str | Path | None, *, device_id=None) -> None:
"""Queue a force_calibration.json path for the acquisition thread."""
with self._lock:
self._pending_calibration = str(path) if path is not None else None
self._pending_device_id = device_id
def request_calibration_zero(self) -> None:
"""Operator-confirmed unloaded interval; collect 30 frames on the HHD owner thread."""
with self._lock:
self._pending_zero = True
def _collect_zero(self, runtime, values, frame_id):
sensor = getattr(runtime.tactile, "raw_sensor", runtime.tactile)
model = getattr(sensor, "calibration", None)
contact_found = bool(getattr(sensor, "centroid_found", False))
with self._lock:
requested = self._pending_zero
self._pending_zero = False
resetting = self._pending_reset or self._cancel_zero
self._cancel_zero = False
if resetting or model is not self._zero_model:
self._zero_samples = None
if resetting:
self._zero_reference = {}
return
if requested:
if getattr(model, "schema_version", None) != 3:
raise ValueError("unloaded zero requires a schema 3 model")
sensor.set_calibration_reference({}, reference_id="zero-measurement-pending")
self._zero_samples = []
self._zero_model = model
self._zero_reference = {}
if self._zero_samples is None:
return
if contact_found or not np.all(np.isfinite(values)):
self._zero_samples = None
raise ValueError("unloaded zero cancelled: reset, contact or invalid force; remove load and retry")
self._zero_samples.append(tuple(values))
if len(self._zero_samples) == 30:
raw = {axis: statistics.median(row[index] for row in self._zero_samples)
for index, axis in enumerate(("fn", "fx", "fy")) if axis in model.channels}
reference_id = f"studio-unloaded-{self._session_id}-{frame_id}"
runtime.set_calibration_reference(raw, reference_id=reference_id)
self._zero_reference = {"reference_id": reference_id, "raw": raw, "samples": 30,
"applied_after_frame": frame_id}
self._zero_samples = None
self._status("Unloaded session zero measured; calibration is ready for contact.")
def _consume_pending_calibration(
self, sensor, frame_id: int = 0, mono_t: float = 0.0
@@ -579,13 +638,19 @@ class FingerAcquisitionWorker(RecordSinkMixin):
"""
with self._lock:
path = self._pending_calibration
device_id = self._pending_device_id
self._pending_calibration = None
session = self._session_id
if path is None or sensor is None:
return
if not hasattr(sensor, "set_calibration"):
raise RuntimeError("sensor does not support set_calibration")
sensor.set_calibration(path)
if device_id:
sensor.set_calibration(path, device_id=device_id)
else:
sensor.set_calibration(path)
self._zero_samples = None
self._zero_reference = {}
version = calibration_digest(path)
self._calibration_version = version
if self._on_calibration_applied is not None:
@@ -762,6 +827,21 @@ class FingerAcquisitionWorker(RecordSinkMixin):
with self._lock:
self._phase = "running"
def on_frame(snap):
# SDK calls this on its HHD thread, after copying this frame's outputs.
if any(value == "zero_unavailable" for value in (getattr(snap, "calibration_status", None) or {}).values()):
self._zero_reference = {}
snapshots.put((snap, self._calibration_version, dict(self._zero_reference)))
try:
mono_ns = getattr(snap.source_metadata, "mono_ns", None)
mono = max(0.0, mono_ns / 1e9 - mono_t0) if mono_ns is not None else max(0.0, time.perf_counter() - mono_t0)
self._consume_pending_calibration(runtime, snap.seq + 1, mono)
self._collect_zero(runtime, (snap.normal_force_raw, snap.shear_x_raw, snap.shear_y_raw), snap.seq)
except Exception as exc: # noqa: BLE001 - reject the request, keep the existing model
logger.warning("Calibration request rejected: %s", exc)
if self._on_error is not None:
self._on_error(str(exc))
try:
pipe = StreamPipeline(
raw_sensor,
@@ -770,7 +850,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
# the pane's contact overlay and the 2D arrow both read it.
contact=True,
include=self._stream_include(),
on_frame=snapshots.put,
on_frame=on_frame,
)
except Exception as exc: # noqa: BLE001 - reported, not swallowed
self._set_end_reason("error")
@@ -808,7 +888,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
snap = None
if expect > 0:
try:
snap = snapshots.get(timeout=0.05)
snap, version, reference = snapshots.get(timeout=0.05)
except queue_module.Empty:
snap = None
except Exception as exc: # noqa: BLE001 - a stage failure ends the session
@@ -844,6 +924,8 @@ class FingerAcquisitionWorker(RecordSinkMixin):
break
capture_fps = packet.capture_fps
capture_dropped = packet.capture_dropped
packet.calibration_version = version
packet.calibration_reference = reference
if self._pause.is_set():
# The hold arrived after this frame was published but before
# it was built: see the box worker. A paused worker publishes
@@ -853,18 +935,6 @@ class FingerAcquisitionWorker(RecordSinkMixin):
# A zeroing is queued onto the pipeline's flow thread, which
# owns the tracker, and reported with the frame and time it
# takes effect — the same confirmation rule as the serial loop.
try:
self._consume_pending_calibration(
tactile, frame_id + 1, float(packet.mono_t or 0.0)
)
except Exception as exc: # noqa: BLE001
errors.append(f"calibration rejected: {type(exc).__name__}: {exc}")
packet.valid = False
packet.stale_forces = True
packet.forces_valid = False
packet.invalid_reason = packet.invalid_reason or errors[-1]
if self._on_error is not None and session == self._session_id:
self._on_error(errors[-1])
try:
self._consume_pending_reset(
runtime,
@@ -1121,7 +1191,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
# frame is dated when it happened rather than when it was queued.
frame_mono = max(0.0, mono_ns / 1e9 - self._mono_t0)
try:
self._consume_pending_calibration(tactile, frame_id, frame_mono)
self._consume_pending_calibration(runtime, frame_id, frame_mono)
except Exception as exc: # noqa: BLE001
message = f"calibration rejected: {type(exc).__name__}: {exc}"
if self._on_error is not None and session == self._session_id:
@@ -1168,6 +1238,14 @@ class FingerAcquisitionWorker(RecordSinkMixin):
sensor,
config_ref=mode.config_ref,
)
if any(value == "zero_unavailable" for value in forces.get("calibration_status", {}).values()):
self._zero_reference = {}
reference = dict(self._zero_reference)
try:
self._collect_zero(runtime, (forces["normal_force_raw"], forces["shear_x_raw"], forces["shear_y_raw"]), frame_id)
except ValueError as exc:
if self._on_error is not None:
self._on_error(str(exc))
invalid_reason = forces.get("invalid_reason") or compute_error
raw_frame = _ensure_bgr(source_frame).copy()
# The frame the tactile chain consumed, kept for depth replay.
@@ -1219,6 +1297,8 @@ class FingerAcquisitionWorker(RecordSinkMixin):
force_unit=forces["force_unit"],
force_calibrated=forces["force_calibrated"],
calibrated_axes=tuple(forces.get("calibrated_axes", ())),
calibration_status=forces.get("calibration_status", {}),
calibration_reference=reference,
calibration_version=self._calibration_version,
session_id=session,
frame_id=frame_id,
@@ -1238,7 +1318,8 @@ class FingerAcquisitionWorker(RecordSinkMixin):
flow_frame_id=frame_id if flow_valid else last_flow_frame_id,
depth_valid=depth_valid,
depth_frame_id=frame_id if depth_valid else last_depth_frame_id,
forces_valid=bool(forces.get("valid", True)) and not compute_error,
forces_valid=bool(forces.get("valid", True)) and not compute_error
and all(value == "ok" for value in forces.get("calibration_status", {}).values()),
contact=contact,
)
with self._lock:
@@ -9,7 +9,7 @@ from __future__ import annotations
import json
import logging
from dataclasses import dataclass
from dataclasses import dataclass, field
from pathlib import Path
import cv2
@@ -128,6 +128,8 @@ class SdkPacket:
forces_valid: bool | None = None
#: Contact centre from ``compute_contact``; optional so old call sites stay valid.
contact: ContactObservation | None = None
calibration_status: dict[str, str] = field(default_factory=dict)
calibration_reference: dict = field(default_factory=dict)
def contact_from_arrays(
@@ -205,6 +207,9 @@ def from_snapshot(
asks for it and reads back what the device reports).
"""
err = str(compute_error or "")
snapshot_contact = getattr(snapshot, "contact_found", None)
if snapshot_contact is not None:
contact_found = snapshot_contact
if valid is None:
valid = not err
stale = bool(err)
@@ -230,6 +235,11 @@ def from_snapshot(
flow_valid = flow is not None and not stale
depth_valid = depth is not None and not stale
calibrated = _force_calibrated(sensor)
statuses = dict(getattr(snapshot, "calibration_status", None) or {})
units = getattr(snapshot, "force_units", None)
axes = tuple(axis for axis, unit in zip(("fn", "fx", "fy"), units) if unit == "N") if units else _calibrated_axes(sensor)
if units:
calibrated = bool(axes)
return SdkPacket(
raw_frame=ensure_bgr(raw_frame) if raw_frame is not None else processed,
processed_frame=processed,
@@ -257,7 +267,7 @@ def from_snapshot(
shear_y_raw=float(snapshot.shear_y_raw),
force_unit=str(snapshot.unit),
force_calibrated=calibrated,
calibrated_axes=_calibrated_axes(sensor),
calibrated_axes=axes,
calibration_version=str(calibration_version or ""),
session_id=int(session_id),
frame_id=frame,
@@ -272,8 +282,9 @@ def from_snapshot(
flow_frame_id=frame if flow_valid else None,
depth_valid=depth_valid,
depth_frame_id=frame if depth_valid else None,
forces_valid=bool(valid),
forces_valid=bool(valid) and all(value == "ok" for value in statuses.values()),
contact=contact,
calibration_status=statuses,
)
@@ -311,6 +311,7 @@ def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None):
which would be indistinguishable from a genuine zero reading and would
silently poison every average built on top of it.
"""
sensor = getattr(sensor, "raw_sensor", sensor)
info = sensor.info
nan = float("nan")
fn = fx = fy = nan
@@ -408,6 +409,7 @@ def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None):
"valid": valid,
"invalid_reason": error,
"raw_available": read_raw,
"calibration_status": dict(getattr(sensor, "calibration_status", {}) or {}),
}
@@ -640,12 +642,18 @@ def calibration_digest(path: str | Path) -> str:
# A parsed SDK document has no path of its own; hash its content.
to_dict = getattr(path, "to_dict", None)
model_fingerprint = getattr(path, "fingerprint", None)
if isinstance(model_fingerprint, str):
return model_fingerprint
if callable(to_dict):
return hashlib.sha256(
json.dumps(to_dict(), sort_keys=True, default=str).encode("utf-8")
).hexdigest()
target = Path(str(path))
if target.is_file():
if json.loads(target.read_text(encoding="utf-8-sig")).get("schema_version") == 3:
from orisys.calibration import load
return load(target, strict=True).fingerprint
return hashlib.sha256(target.read_bytes()).hexdigest()
if not isinstance(path, (str, Path)):
return hashlib.sha256(
@@ -658,6 +666,7 @@ def calibration_digest(path: str | Path) -> str:
def loaded_path(sensor) -> str | None:
"""Where the sensor's active calibration came from, if it knows."""
sensor = getattr(sensor, "raw_sensor", sensor)
calibration = getattr(sensor, "calibration", None)
if calibration is None:
return None
@@ -671,6 +680,7 @@ def loaded_calibration_version(sensor) -> str:
The sensor may have loaded a product-bundle file while opening, which is
exactly the set of coefficients the first frames are measured with.
"""
sensor = getattr(sensor, "raw_sensor", sensor)
calibration = getattr(sensor, "calibration", None)
if calibration is None:
return ""
@@ -142,6 +142,7 @@ class BareVideoSetup:
#: Serial the operator confirmed for ``calibration``. Empty when the pass uses the
#: bundle's own file or raw force.
device_id: str | None = None
unloaded_reference_frames: int = 0
@dataclass(frozen=True)
@@ -335,6 +336,9 @@ def _calibration_identity(path: Path | None) -> str:
return ""
try:
digest = hashlib.sha256(Path(path).read_bytes()).hexdigest()[:12]
if json.loads(Path(path).read_text(encoding="utf-8-sig")).get("schema_version") == 3:
from orisys.calibration import load
digest = load(path, strict=True).fingerprint[:12]
except OSError:
return Path(path).name
return f"{Path(path).name}@{digest}"
@@ -531,6 +535,7 @@ def build_workspace(
device_id=device_id,
)
directory = workspace_directory(identity, root=root)
document["synthetic"]["unloaded_reference_frames"] = 0 if setup is None else setup.unloaded_reference_frames
directory.mkdir(parents=True, exist_ok=True)
mode = _link_or_copy(resolved, directory / VIDEO_FILENAME)
document["synthetic"]["video_in_workspace"] = mode
@@ -542,14 +542,18 @@ class ForceSample:
def from_reading(cls, reading: Mapping[str, Any]) -> ForceSample:
"""One :func:`~orisys_app_shared.acquisition.sdk_worker.read_forces_from_sensor`
reading, as this record's own type."""
axes = reading.get("calibrated_axes")
def newton(axis, key):
return float(reading.get(key, _NAN)) if axes is None or axis in axes else _NAN
return cls(
normal=float(reading.get("normal_force", _NAN)),
shear_x=float(reading.get("shear_x", _NAN)),
shear_y=float(reading.get("shear_y", _NAN)),
normal=newton("fn", "normal_force"),
shear_x=newton("fx", "shear_x"),
shear_y=newton("fy", "shear_y"),
normal_raw=float(reading.get("normal_force_raw", _NAN)),
shear_x_raw=float(reading.get("shear_x_raw", _NAN)),
shear_y_raw=float(reading.get("shear_y_raw", _NAN)),
valid=bool(reading.get("valid", True)),
valid=bool(reading.get("valid", True)) and all(
value == "ok" for value in reading.get("calibration_status", {}).values()),
calibrated=bool(reading.get("force_calibrated", False)),
)
@@ -618,9 +618,12 @@ class AnalysisService:
or dict(session.get("calibration") or {}).get("version")
or ""
)
zero_frames = int((session.get("synthetic") or {}).get("unloaded_reference_frames", 0))
if zero_frames not in (0, 30):
raise ValueError("unloaded reference must contain exactly 30 frames")
key = result_key(
recording=identity,
sensor_config=self.chain.identity(),
sensor_config=self.chain.identity() + (";unloaded-reference=30" if zero_frames else ""),
calibration=calibration,
reference_frame=self.reference_frame,
)
@@ -810,6 +813,7 @@ class AnalysisService:
from orisys_app_shared.acquisition.sdk_worker import process_chain
provenance = state.provenance
zero_frames = int((self.reader().session().get("synthetic") or {}).get("unloaded_reference_frames", 0))
for position, number in enumerate(state.segments):
if state.reasons or self._stop_requested():
if self._stop_requested():
@@ -823,6 +827,7 @@ class AnalysisService:
if any(row.segment_frame != index + 1 for index, row in enumerate(rows)):
provenance.warnings.append(f"segment_frame_gap:{number}")
with _OpenChain(self.chain, path, state.working) as chain:
zero_samples = []
offset, measured = self._segment_offset(chain, number, state.offsets)
state.offsets[str(number)] = offset
state.sources.add("measured" if measured else "declared")
@@ -873,6 +878,22 @@ class AnalysisService:
provenance.warnings.append(f"chain:{error}")
state.reasons.append(f"chain_failed:{row.frame_id}")
return
if zero_frames and (self.reference_frame is None or row.frame_id >= self.reference_frame) and len(zero_samples) < zero_frames:
sensor = getattr(chain.sensor, "raw_sensor", chain.sensor)
model = getattr(sensor, "calibration", None)
if getattr(model, "schema_version", None) != 3:
raise ValueError("unloaded reference requires a schema 3 calibration")
keys = (sensor.info.FNORMAL_RAW, sensor.info.FSHEARX_RAW, sensor.info.FSHEARY_RAW)
values = sensor.read_info(*keys)
if getattr(sensor, "centroid_found", False) or not np.all(np.isfinite(values)):
raise ValueError("unloaded reference contains contact or invalid readings")
zero_samples.append(values)
if len(zero_samples) == zero_frames:
raw = {axis: float(np.median([value[index] for value in zero_samples]))
for index, axis in enumerate(("fn", "fx", "fy")) if axis in model.channels}
sensor.set_calibration_reference(raw, reference_id=f"video-unloaded-{row.frame_id}")
provenance.counts["unloaded_reference_frames"] = zero_frames
provenance.counts["unloaded_reference_fn_raw"] = raw.get("fn")
force = self._read_force(chain)
flow = getattr(chain.sensor, "flow", None)
if flow is None or not getattr(flow, "size", 0):
@@ -909,6 +930,8 @@ class AnalysisService:
depth=depth_array,
force=force,
)
if zero_frames and len(zero_samples) != zero_frames:
raise ValueError("video ended before 30 unloaded reference frames were available")
# ------------------------------------------------------------------- internals
def _read_force(self, chain: _OpenChain) -> ForceSample | None:
+1 -1
View File
@@ -88,7 +88,7 @@ def select_display_forces(*, units_pref: str, sample) -> DisplayForces:
values.append(float("nan"))
available.append(False)
if not any(available):
if not any(available) and not getattr(sample, "calibration_status", {}):
# Nothing is calibrated: showing raw is honest as long as it is labelled.
return DisplayForces(
values=raw,
@@ -17,6 +17,7 @@ collide.
from __future__ import annotations
import json
import logging
import shutil
from pathlib import Path
@@ -42,6 +43,23 @@ def store_calibration_snapshot(directory: str | Path, source: str | Path, versio
base = Path(directory) / SNAPSHOT_DIRNAME
base.mkdir(parents=True, exist_ok=True)
origin = Path(source)
data = json.loads(origin.read_text(encoding="utf-8-sig"))
if data.get("schema_version") == 3:
from orisys.calibration import load
document = load(origin, strict=True)
# Keep each model's relative artifact paths in its own immutable directory.
package = base / document.fingerprint
package.mkdir(parents=True, exist_ok=True)
for artifact in data["model"].get("artifacts", []):
relative = Path(artifact["path"])
destination = package / relative
destination.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(origin.parent / relative, destination)
target = package / origin.name
shutil.copy2(origin, target)
load(target, strict=True)
return target.relative_to(Path(directory)).as_posix()
target = base / snapshot_name(version, origin)
if not target.is_file() or target.stat().st_size != origin.stat().st_size:
# Same content-addressed name means the same coefficients; a size check
@@ -56,5 +74,5 @@ def list_calibration_snapshots(directory: str | Path) -> list[str]:
if not base.is_dir():
return []
return sorted(
f"{SNAPSHOT_DIRNAME}/{path.name}" for path in base.iterdir() if path.is_file()
path.relative_to(Path(directory)).as_posix() for path in base.rglob("*") if path.is_file()
)
@@ -46,7 +46,7 @@ from orisys_app_shared.record.video_writer import VideoSegments
logger = logging.getLogger(__name__)
RECORD_FORMAT_VERSION = "1.4"
RECORD_FORMAT_VERSION = "1.5"
# Lifecycle states. ``failed`` and ``stopped`` are terminal; a recorder that has
# not reached a terminal state must not be reused.
@@ -78,6 +78,8 @@ FORCES_HEADER = (
"FX_output_N",
"FY_output_N",
"Fmag_raw",
"calibration_status",
"calibration_reference",
)
FRAMES_HEADER = (
"frame_id",
@@ -1152,6 +1154,8 @@ class _SessionWriter:
format_value(sample.fx_n_processed),
format_value(sample.fy_n_processed),
format_value(raw_magnitude(sample.fn_raw, sample.fx_raw, sample.fy_raw)),
json.dumps(sample.calibration_status, sort_keys=True),
json.dumps(sample.calibration_reference, sort_keys=True),
)
)
@@ -26,7 +26,7 @@ from __future__ import annotations
import math
from collections.abc import Iterable
from dataclasses import dataclass, replace
from dataclasses import dataclass, field, replace
from typing import TYPE_CHECKING
if TYPE_CHECKING: # numpy is imported lazily, by the one function that needs it
@@ -80,6 +80,8 @@ class RecordSample:
time_basis: str = ""
media_time_ms: float | None = None
capture_dropped: int = 0
calibration_status: dict[str, str] = field(default_factory=dict)
calibration_reference: dict = field(default_factory=dict)
# ------------------------------------------------------------- accessors
def raw_triplet(self) -> tuple[float, float, float]:
@@ -183,6 +185,8 @@ def to_record_sample(packet, *, calibration_version: str = "") -> RecordSample:
"time_basis": str(getattr(packet, "time_basis", "") or ""),
"media_time_ms": getattr(packet, "media_time_ms", None),
"capture_dropped": int(getattr(packet, "capture_dropped", 0) or 0),
"calibration_status": dict(getattr(packet, "calibration_status", {}) or {}),
"calibration_reference": dict(getattr(packet, "calibration_reference", {}) or {}),
}
if not valid or stale: