diff --git a/shared/src/orisys_app_shared/acquisition/arrows.py b/shared/src/orisys_app_shared/acquisition/arrows.py index 693545b..29617aa 100644 --- a/shared/src/orisys_app_shared/acquisition/arrows.py +++ b/shared/src/orisys_app_shared/acquisition/arrows.py @@ -19,6 +19,9 @@ FLOW_RAMP_FALLBACK = "#12315D,#6E4AD0,#FA02F3,#FF8A6B,#D8FF00" FLOW_IDLE_HEX = "#8A93A6" """Brand-neutral tint for the grid dots below the flow threshold.""" +#: BGR grey used for depth samples that carry no value. +NO_DEPTH_BGR = (0x8A, 0x8A, 0x8A) + _LUT_STEPS = 256 _RAMP_MIN_STOPS = 2 _RAMP_MAX_STOPS = 8 @@ -138,8 +141,20 @@ def draw_arrow_display( def depth_map_to_bgr( depth: Optional[np.ndarray], colormap: Optional[np.ndarray] = None, + *, + value_range: Optional[tuple[float, float]] = None, + invalid_mask: Optional[np.ndarray] = None, ) -> Optional[np.ndarray]: - """Convert a float depth/divergence map to a colormap BGR image.""" + """Convert a float depth/divergence map to a colormap BGR image. + + ``value_range`` pins the colour scale. Per-frame re-normalisation (the old + behaviour) makes the colours drift from frame to frame, so two frames can + look identical while representing different depths — pass the same range + across a session when frames are meant to be comparable. + + ``invalid_mask`` marks samples that carry no depth. They are painted with a + neutral grey so "no data" can never be mistaken for "zero depth". + """ if depth is None: return None arr = np.asarray(depth, dtype=np.float64) @@ -148,12 +163,28 @@ def depth_map_to_bgr( finite = np.isfinite(arr) if not np.any(finite): return None - lo = float(np.nanmin(arr[finite])) - hi = float(np.nanmax(arr[finite])) - if hi > lo: - normalized = ((arr - lo) / (hi - lo) * 255.0).astype(np.uint8) + if value_range is not None and value_range[1] > value_range[0]: + lo, hi = float(value_range[0]), float(value_range[1]) else: - normalized = np.zeros(arr.shape, dtype=np.uint8) - return cv2.applyColorMap( + lo = float(np.nanmin(arr[finite])) + hi = float(np.nanmax(arr[finite])) + if hi > lo: + scaled = (arr - lo) / (hi - lo) + else: + scaled = np.zeros(arr.shape, dtype=np.float64) + scaled = np.clip(scaled, 0.0, 1.0) + normalized = (scaled * 255.0).astype(np.uint8) + image = cv2.applyColorMap( normalized, cv2.COLORMAP_JET if colormap is None else colormap ) + missing = ~finite + if invalid_mask is not None: + mask = np.asarray(invalid_mask, dtype=bool) + if mask.shape[:2] == image.shape[:2]: + missing = missing | ~mask + if np.any(missing): + #: Neutral grey — deliberately outside the JET ramp so it reads as + #: "undefined" rather than as a depth value. + image[missing] = NO_DEPTH_BGR + return image + diff --git a/shared/src/orisys_app_shared/acquisition/finger_worker.py b/shared/src/orisys_app_shared/acquisition/finger_worker.py index ce09d50..bbbd29c 100644 --- a/shared/src/orisys_app_shared/acquisition/finger_worker.py +++ b/shared/src/orisys_app_shared/acquisition/finger_worker.py @@ -13,6 +13,7 @@ from orisys_app_shared.acquisition.constants import ( FORCE_SAMPLE_RATE_HZ, LOWPASS_CUTOFF_HZ, ) +from orisys_app_shared.acquisition.sinks import RecordSinkMixin from orisys_app_shared.acquisition.sdk_worker import ( CAMERA_DISCONNECT_TIMEOUT_S, HANDOVER_TIMEOUT_S, @@ -53,7 +54,7 @@ def resolve_finger_export_dir(config_ref: str | Path) -> Path | None: return candidate.resolve() -class FingerAcquisitionWorker: +class FingerAcquisitionWorker(RecordSinkMixin): """Background loop: FingerRuntime grab/unroll + deformation + force readout.""" def __init__( @@ -62,6 +63,7 @@ class FingerAcquisitionWorker: on_error: Optional[Callable[[str], None]] = None, on_status: Optional[Callable[[str], None]] = None, ): + self._init_sinks() self._on_error = on_error self._on_status = on_status self._thread: Optional[threading.Thread] = None @@ -276,6 +278,9 @@ class FingerAcquisitionWorker: saw_frame = False last_frame_mono = 0.0 none_streak_start: Optional[float] = None + # Per-session monotonic frame index: the join key shared by the video, + # the CSV and the calibration records. + frame_id = 0 while not self._stop.is_set() and session == self._session_id and runtime is not None: try: @@ -309,12 +314,13 @@ class FingerAcquisitionWorker: none_streak_start = None tactile = runtime.tactile + compute_error = "" try: self._consume_pending_calibration(tactile) tactile.compute_deformation(decompose=True, check_motion=False) tactile.compute_contact() - except Exception: - pass + except Exception as exc: # noqa: BLE001 - reported on the packet + compute_error = f"compute: {type(exc).__name__}: {exc}" t1 = time.perf_counter() frame_times.append(t1) cutoff = t1 - 1.0 @@ -324,6 +330,7 @@ class FingerAcquisitionWorker: tactile, config_ref=mode.config_ref, ) + invalid_reason = forces.get("invalid_reason") or compute_error packet = SdkPacket( raw_frame=_ensure_bgr(source_frame), processed_frame=_ensure_bgr(unrolled), @@ -339,6 +346,10 @@ class FingerAcquisitionWorker: force_unit=forces["force_unit"], force_calibrated=forces["force_calibrated"], session_id=session, + frame_id=frame_id, + valid=bool(forces.get("valid", True)) and not compute_error, + invalid_reason=invalid_reason, + raw_available=bool(forces.get("raw_available", True)), flow=forces["flow"], depth=forces.get("depth"), valid_mask=runtime.valid_mask, @@ -346,6 +357,9 @@ class FingerAcquisitionWorker: with self._lock: self._latest = packet self._phase = "running" + # Record channel: every packet, regardless of UI refresh rate. + self._emit(packet) + frame_id += 1 saw_frame = True last_frame_mono = t1 except Exception as exc: diff --git a/shared/src/orisys_app_shared/acquisition/sdk_worker.py b/shared/src/orisys_app_shared/acquisition/sdk_worker.py index beb502c..e8074b3 100644 --- a/shared/src/orisys_app_shared/acquisition/sdk_worker.py +++ b/shared/src/orisys_app_shared/acquisition/sdk_worker.py @@ -3,6 +3,7 @@ from __future__ import annotations import json +import logging import threading import time from dataclasses import dataclass @@ -12,10 +13,13 @@ from typing import Callable, Optional import cv2 import numpy as np +logger = logging.getLogger(__name__) + from orisys_app_shared.acquisition.constants import ( FORCE_SAMPLE_RATE_HZ, LOWPASS_CUTOFF_HZ, ) +from orisys_app_shared.acquisition.sinks import RecordSinkMixin VIDEO_EOF_TIMEOUT_S = 0.5 CAMERA_DISCONNECT_TIMEOUT_S = 1.5 @@ -53,6 +57,16 @@ class SdkPacket: force_unit: str = "raw" force_calibrated: bool = False session_id: int = 0 + #: Monotonic per-session frame index; the join key for video, CSV and + #: calibration records. + frame_id: int = 0 + #: False when the force readout failed. Invalid packets carry NaN forces and + #: must never be averaged into calibration data or written as zeros. + valid: bool = True + invalid_reason: str = "" + #: False when the sensor exposes no RAW channels; the raw fields are then + #: NaN rather than a copy of the calibrated values. + raw_available: bool = True flow: Optional[np.ndarray] = None depth: Optional[np.ndarray] = None valid_mask: Optional[np.ndarray] = None @@ -117,11 +131,22 @@ def _sensor_force_calibrated(sensor, *, config_ref: str | Path | None = None) -> def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None): + """Read one force sample. + + A failed readout is reported as invalid with NaN values — never as zeros, + which would be indistinguishable from a genuine zero reading and would + silently poison every average built on top of it. + """ info = sensor.info - fn = fx = fy = 0.0 - fn_raw = fx_raw = fy_raw = 0.0 + nan = float("nan") + fn = fx = fy = nan + fn_raw = fx_raw = fy_raw = nan flow = None depth = None + valid = True + error = "" + #: Whether the sensor exposes RAW channels at all. When it does not, the raw + #: fields stay NaN instead of being filled with the calibrated values. read_raw = all( hasattr(info, name) for name in ("FNORMAL_RAW", "FSHEARX_RAW", "FSHEARY_RAW") @@ -173,22 +198,22 @@ def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None): fn, fx, fy, flow, depth = sensor.read_info( info.FNORMAL, info.FSHEARX, info.FSHEARY, info.VRAW, info.DEPTH ) - fn_raw, fx_raw, fy_raw = fn, fx, fy elif has_flow: fn, fx, fy, flow = sensor.read_info( info.FNORMAL, info.FSHEARX, info.FSHEARY, info.VRAW ) - fn_raw, fx_raw, fy_raw = fn, fx, fy elif has_depth: fn, fx, fy, depth = sensor.read_info( info.FNORMAL, info.FSHEARX, info.FSHEARY, info.DEPTH ) - fn_raw, fx_raw, fy_raw = fn, fx, fy else: fn, fx, fy = sensor.read_info(info.FNORMAL, info.FSHEARX, info.FSHEARY) - fn_raw, fx_raw, fy_raw = fn, fx, fy - except Exception: - pass + except Exception as exc: # noqa: BLE001 - reported, not swallowed + valid = False + error = f"{type(exc).__name__}: {exc}" if str(exc) else type(exc).__name__ + fn = fx = fy = nan + fn_raw = fx_raw = fy_raw = nan + flow = None if depth is not None: arr = np.asarray(depth) if arr.size == 0: @@ -205,6 +230,9 @@ def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None): "force_unit": "N" if calibrated else "raw", "flow": flow, "depth": depth, + "valid": valid, + "invalid_reason": error, + "raw_available": read_raw, } @@ -212,7 +240,7 @@ def _is_file_source(video_source: str | int) -> bool: return isinstance(video_source, str) -class SdkAcquisitionWorker: +class SdkAcquisitionWorker(RecordSinkMixin): """Background loop: Sensor.get_img + deformation + force readout.""" def __init__( @@ -221,6 +249,7 @@ class SdkAcquisitionWorker: on_error: Optional[Callable[[str], None]] = None, on_status: Optional[Callable[[str], None]] = None, ): + self._init_sinks() self._on_error = on_error self._on_status = on_status self._thread: Optional[threading.Thread] = None @@ -424,6 +453,9 @@ class SdkAcquisitionWorker: saw_frame = False last_frame_mono = 0.0 none_streak_start: Optional[float] = None + # Per-session monotonic frame index: the join key shared by the video, + # the CSV and the calibration records. + frame_id = 0 while not self._stop.is_set() and session == self._session_id: try: @@ -455,18 +487,20 @@ class SdkAcquisitionWorker: continue none_streak_start = None + compute_error = "" try: self._consume_pending_calibration(sensor) sensor.compute_deformation(decompose=True, check_motion=False) sensor.compute_contact() - except Exception: - pass + except Exception as exc: # noqa: BLE001 - reported on the packet + compute_error = f"compute: {type(exc).__name__}: {exc}" t1 = time.perf_counter() frame_times.append(t1) cutoff = t1 - 1.0 frame_times = [t for t in frame_times if t >= cutoff] fps = float(len(frame_times)) if frame_times else 0.0 forces = read_forces_from_sensor(sensor, config_ref=mode.config_ref) + invalid_reason = forces.get("invalid_reason") or compute_error packet = SdkPacket( raw_frame=_ensure_bgr(raw), processed_frame=_ensure_bgr(processed), @@ -482,12 +516,19 @@ class SdkAcquisitionWorker: force_unit=forces["force_unit"], force_calibrated=forces["force_calibrated"], session_id=session, + frame_id=frame_id, + valid=bool(forces.get("valid", True)) and not compute_error, + invalid_reason=invalid_reason, + raw_available=bool(forces.get("raw_available", True)), flow=forces["flow"], depth=forces.get("depth"), ) with self._lock: self._latest = packet self._phase = "running" + # Record channel: every packet, regardless of UI refresh rate. + self._emit(packet) + frame_id += 1 saw_frame = True last_frame_mono = t1 except Exception as exc: diff --git a/shared/src/orisys_app_shared/acquisition/sinks.py b/shared/src/orisys_app_shared/acquisition/sinks.py new file mode 100644 index 0000000..6fd7d24 --- /dev/null +++ b/shared/src/orisys_app_shared/acquisition/sinks.py @@ -0,0 +1,69 @@ +"""Record-channel fan-out shared by the acquisition workers. + +The workers used to expose exactly one consumer of their data — a single +``latest()`` slot polled by a Qt timer. That made recording depend on the UI +refresh rate, and it meant that hiding a panel could stop a recording. + +Every worker now also publishes each :class:`~orisys_app_shared.acquisition.sdk_worker.SdkPacket` +to a list of sinks, from the acquisition thread. Sinks are simple callables and +must be cheap and Qt-free; a sink that raises is logged and dropped for that +packet rather than being allowed to kill acquisition. +""" + +from __future__ import annotations + +import logging +import threading +from typing import Any, Callable, Iterable + +logger = logging.getLogger(__name__) + + +class RecordSinkMixin: + """Mixin adding ``add_sink`` / ``remove_sink`` / ``_emit`` to a worker.""" + + def _init_sinks(self) -> None: + self._sinks: list[Callable[[Any], None]] = [] + self._sinks_lock = threading.Lock() + self._emit_failures = 0 + + def add_sink(self, sink: Callable[[Any], None]) -> None: + """Register a record-channel consumer of every packet.""" + with self._sinks_lock: + if sink not in self._sinks: + self._sinks.append(sink) + + def remove_sink(self, sink: Callable[[Any], None]) -> None: + with self._sinks_lock: + if sink in self._sinks: + self._sinks.remove(sink) + + def sinks(self) -> tuple[Callable[[Any], None], ...]: + with self._sinks_lock: + return tuple(self._sinks) + + @property + def emit_failures(self) -> int: + """Number of sink calls that raised — surfaced so it is never silent.""" + return self._emit_failures + + def _emit(self, packet: Any) -> None: + """Hand ``packet`` to every sink, from the acquisition thread.""" + for sink in self.sinks(): + try: + sink(packet) + except Exception: # noqa: BLE001 - one bad consumer must not stop capture + self._emit_failures += 1 + logger.exception("Record sink %r failed", sink) + + +def emit_all(sinks: Iterable[Callable[[Any], None]], packet: Any) -> int: + """Utility for tests: emit to a plain list of sinks, returning failures.""" + failures = 0 + for sink in sinks: + try: + sink(packet) + except Exception: # noqa: BLE001 + failures += 1 + logger.exception("Record sink %r failed", sink) + return failures