diff --git a/apps/customer/config/compound_eye.png b/apps/customer/config/compound_eye.png index 7cccd46..2879966 100644 Binary files a/apps/customer/config/compound_eye.png and b/apps/customer/config/compound_eye.png differ diff --git a/apps/customer/src/orisys_customer/app_window.py b/apps/customer/src/orisys_customer/app_window.py index 037cd2a..ba698d9 100644 --- a/apps/customer/src/orisys_customer/app_window.py +++ b/apps/customer/src/orisys_customer/app_window.py @@ -128,6 +128,7 @@ class CustomerMainWindow(QMainWindow): self.setStatusBar(QStatusBar()) self.cal_panel.status_message.connect(self.statusBar().showMessage) + self.cal_panel.calibration_exported.connect(self._on_calibration_exported) self._build_menus() self._wire_toolbar() self._wire_sensor_picker() @@ -434,6 +435,13 @@ class CustomerMainWindow(QMainWindow): self._apply_panel_visibility() self._persist_prefs() + def _on_calibration_exported(self, path: str) -> None: + """Apply newly written force_calibration.json to the running Sensor.""" + self._ensure_workers() + for worker in (self._box_worker, self._finger_worker): + if worker is not None and hasattr(worker, "set_pending_calibration"): + worker.set_pending_calibration(path) + def _on_finger_3d_toggled(self, visible: bool) -> None: if self.sensor_picker.sensor_type() != "finger": return diff --git a/apps/customer/src/orisys_customer/ui/cal_panel.py b/apps/customer/src/orisys_customer/ui/cal_panel.py index 454dec4..f99a25e 100644 --- a/apps/customer/src/orisys_customer/ui/cal_panel.py +++ b/apps/customer/src/orisys_customer/ui/cal_panel.py @@ -38,6 +38,7 @@ from orisys_app_shared.paths import calibration_export_dir, sensors_dir class CalPanel(QFrame): status_message = pyqtSignal(str) + calibration_exported = pyqtSignal(str) # path applied to running sensor def __init__(self, parent=None) -> None: super().__init__(parent) @@ -335,4 +336,7 @@ class CalPanel(QFrame): if warnings: msg += "\n" + t("cal.export_warnings", detail="; ".join(warnings)) self.status_message.emit(msg) + # Prefer the bundle file for live apply so the next Sensor() also sees it. + apply_path = local_path if local_path is not None else primary + self.calibration_exported.emit(str(apply_path)) QMessageBox.information(self, t("panels.cal"), msg) diff --git a/apps/customer/src/orisys_customer/ui/force_plots.py b/apps/customer/src/orisys_customer/ui/force_plots.py index b6c62e6..766ed13 100644 --- a/apps/customer/src/orisys_customer/ui/force_plots.py +++ b/apps/customer/src/orisys_customer/ui/force_plots.py @@ -9,6 +9,7 @@ from PyQt5.QtWidgets import QFrame, QHBoxLayout, QLabel, QVBoxLayout from orisys_app_shared.acquisition.constants import FORCE_SAMPLE_RATE_HZ, LOWPASS_CUTOFF_HZ from orisys_app_shared.force import MetricHistory +from orisys_app_shared.force.history import DEFAULT_MAX_FPS from orisys_app_shared.i18n import t from orisys_app_shared.theme import color @@ -40,6 +41,7 @@ class ForcePlotsPanel(QFrame): sample_rate_hz=FORCE_SAMPLE_RATE_HZ, lowpass_cutoff_hz=LOWPASS_CUTOFF_HZ, y_axis_min_span=0.5, + max_fps=DEFAULT_MAX_FPS, ) root = QVBoxLayout(self) diff --git a/shared/src/orisys_app_shared/acquisition/finger_worker.py b/shared/src/orisys_app_shared/acquisition/finger_worker.py index 9537d80..83f1b26 100644 --- a/shared/src/orisys_app_shared/acquisition/finger_worker.py +++ b/shared/src/orisys_app_shared/acquisition/finger_worker.py @@ -73,6 +73,7 @@ class FingerAcquisitionWorker: self._export_dir: Optional[Path] = None self._end_reason: Optional[str] = None self._phase: str = "idle" + self._pending_calibration: Optional[str] = None @property def is_running(self) -> bool: @@ -142,6 +143,20 @@ class FingerAcquisitionWorker: raise RuntimeError("Finger runtime is not running") runtime.tactile.reset() + def set_pending_calibration(self, path: str | Path | 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 + + def _consume_pending_calibration(self, sensor) -> None: + with self._lock: + path = self._pending_calibration + self._pending_calibration = None + if path is None or sensor is None: + return + if hasattr(sensor, "set_calibration"): + sensor.set_calibration(path) + def _set_end_reason(self, reason: str) -> None: with self._lock: if self._end_reason is None: @@ -246,6 +261,7 @@ class FingerAcquisitionWorker: none_streak_start = None tactile = runtime.tactile try: + self._consume_pending_calibration(tactile) tactile.compute_deformation(decompose=True, check_motion=False) tactile.compute_contact() except Exception: diff --git a/shared/src/orisys_app_shared/acquisition/sdk_worker.py b/shared/src/orisys_app_shared/acquisition/sdk_worker.py index 93ec596..069759e 100644 --- a/shared/src/orisys_app_shared/acquisition/sdk_worker.py +++ b/shared/src/orisys_app_shared/acquisition/sdk_worker.py @@ -95,6 +95,8 @@ def _ensure_bgr(img: np.ndarray) -> np.ndarray: def _sensor_force_calibrated(sensor, *, config_ref: str | Path | None = None) -> bool: + if getattr(sensor, "force_calibrated", False): + return True if getattr(sensor, "_force_cal_sensor_name", None) is not None: return True if config_ref is None: @@ -226,6 +228,7 @@ class SdkAcquisitionWorker: self._video_source: Optional[str | int] = None self._end_reason: Optional[str] = None self._phase: str = "idle" + self._pending_calibration: Optional[str] = None @property def is_running(self) -> bool: @@ -294,6 +297,20 @@ class SdkAcquisitionWorker: raise RuntimeError("Sensor is not running") sensor.reset() + def set_pending_calibration(self, path: str | Path | 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 + + def _consume_pending_calibration(self, sensor) -> None: + with self._lock: + path = self._pending_calibration + self._pending_calibration = None + if path is None or sensor is None: + return + if hasattr(sensor, "set_calibration"): + sensor.set_calibration(path) + def _set_end_reason(self, reason: str) -> None: with self._lock: if self._end_reason is None: @@ -384,6 +401,7 @@ class SdkAcquisitionWorker: none_streak_start = None try: + self._consume_pending_calibration(self._sensor) self._sensor.compute_deformation(decompose=True, check_motion=False) self._sensor.compute_contact() except Exception: diff --git a/shared/src/orisys_app_shared/force/calibration_io.py b/shared/src/orisys_app_shared/force/calibration_io.py index 1824ed6..15a616f 100644 --- a/shared/src/orisys_app_shared/force/calibration_io.py +++ b/shared/src/orisys_app_shared/force/calibration_io.py @@ -1,28 +1,28 @@ -"""Build and write schema_version-2 force_calibration.json files.""" +"""Build and write schema_version-2 force_calibration.json files. + +Delegates schema/parse/write to ``orisys.calibration`` so the app and SDK share +one format. ``CalSessionStore`` remains the UI sampling table. +""" from __future__ import annotations -import json from copy import deepcopy from dataclasses import asdict, dataclass, field -from datetime import datetime, timezone +from datetime import datetime from pathlib import Path -from typing import Any, Iterable, Literal +from typing import Any, Literal + +from orisys.calibration import ( + CalSample as SdkCalSample, + build as sdk_build, + check as sdk_check, + pipeline_from_sensor_json, + write as sdk_write, +) ChannelKey = Literal["fn", "fx", "fy"] CHANNEL_KEYS: tuple[ChannelKey, ...] = ("fn", "fx", "fy") -_CHANNEL_TO_JSON = { - "fn": "normal", - "fx": "shear_x", - "fy": "shear_y", -} -_JSON_RAW_KEY = { - "normal": "fnormal", - "shear_x": "fshearx", - "shear_y": "fsheary", -} - WEIGHT_EPS = 1e-6 @@ -33,6 +33,14 @@ class CalSample: n: int std: float = 0.0 + def to_sdk(self) -> SdkCalSample: + return SdkCalSample( + weight_n=float(self.weight_n), + raw_mean=float(self.raw_mean), + n=int(self.n), + std=float(self.std), + ) + @dataclass class CalSessionStore: @@ -78,65 +86,22 @@ class CalSessionStore: out[key] = [asdict(s) for s in self.channel(key)] return out + def to_sdk_samples(self) -> dict[str, list[SdkCalSample]]: + return { + key: [s.to_sdk() for s in self.channel(key)] + for key in CHANNEL_KEYS + } + def meta_from_sensor_json(path: str | Path | None) -> dict[str, Any]: """Pull stitch / tracker fields from a sensor.json for export meta.""" - meta: dict[str, Any] = { - "stitch_resolution": None, - "dis_mode": None, - "hhd_downsample": None, - "config_ref": None, + pipeline = pipeline_from_sensor_json(path) + return { + "stitch_resolution": pipeline.get("stitch_resolution"), + "dis_mode": pipeline.get("dis_mode"), + "hhd_downsample": pipeline.get("hhd_downsample"), + "config_ref": pipeline.get("config_ref"), } - if path is None: - return meta - p = Path(path) - meta["config_ref"] = str(p) - if not p.is_file(): - return meta - try: - data = json.loads(p.read_text(encoding="utf-8")) - except Exception: - return meta - if not isinstance(data, dict): - return meta - camera = data.get("camera") if isinstance(data.get("camera"), dict) else {} - tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {} - stitch = camera.get("resolution_stitch") - if isinstance(stitch, (list, tuple)) and len(stitch) >= 2: - meta["stitch_resolution"] = [int(stitch[0]), int(stitch[1])] - if "dis_mode" in tracker: - try: - meta["dis_mode"] = int(tracker["dis_mode"]) - except (TypeError, ValueError): - meta["dis_mode"] = tracker.get("dis_mode") - if "hhd_downsample" in tracker: - try: - meta["hhd_downsample"] = int(tracker["hhd_downsample"]) - except (TypeError, ValueError): - meta["hhd_downsample"] = tracker.get("hhd_downsample") - return meta - - -def _piecewise_channel( - *, - json_name: str, - samples: Iterable[CalSample], - signed: bool, -) -> dict[str, Any] | None: - rows = sorted(samples, key=lambda s: abs(float(s.raw_mean))) - if len(rows) < 2: - return None - points = [[abs(float(s.raw_mean)), float(s.weight_n)] for s in rows] - block: dict[str, Any] = { - "method": "piecewise_linear", - "raw_key": _JSON_RAW_KEY[json_name], - "extrapolate": True, - "clamp_output_min": 0.0, - "points": points, - } - if signed: - block["signed"] = True - return block def build_force_calibration_v2( @@ -145,64 +110,66 @@ def build_force_calibration_v2( meta: dict[str, Any] | None = None, ) -> tuple[dict[str, Any], list[str]]: """Build schema v2 JSON. Returns (document, skipped_channel_warnings).""" - warnings: list[str] = [] - channels: dict[str, Any] = {} - for key, json_name in _CHANNEL_TO_JSON.items(): - signed = key in ("fx", "fy") - block = _piecewise_channel( - json_name=json_name, - samples=store.channel(key), - signed=signed, + meta = dict(meta or {}) + identity = { + "sensor_type": meta.get("sensor_type", ""), + "sn": meta.get("sn", ""), + } + pipeline = { + "stitch_resolution": meta.get("stitch_resolution"), + "dis_mode": meta.get("dis_mode"), + "hhd_downsample": meta.get("hhd_downsample"), + "config_ref": meta.get("config_ref"), + } + extra = { + k: v + for k, v in meta.items() + if k + not in ( + "sensor_type", + "sn", + "stitch_resolution", + "dis_mode", + "hhd_downsample", + "config_ref", + "samples", ) - if block is None: - if store.channel(key): - warnings.append( - f"{key}: need at least 2 points (have {len(store.channel(key))})" - ) + } + try: + cal = sdk_build( + store.to_sdk_samples(), + identity=identity, + pipeline=pipeline, + origin=str(meta.get("origin") or "user"), + extra_meta=extra, + ) + except Exception as exc: + # Match previous API: raise ValueError with channel warnings when empty. + raise ValueError(str(exc)) from exc + + result = sdk_check(cal) + warnings = list(result.warnings) + for key in CHANNEL_KEYS: + if not cal.has(key): + rows = store.channel(key) + if rows: + msg = f"{key}: need at least 2 points (have {len(rows)})" else: - warnings.append(f"{key}: no samples") - continue - channels[json_name] = block - - base_meta = { - "date": datetime.now(timezone.utc).astimezone().isoformat(timespec="seconds"), - "sensor_type": "", - "sn": "", - "stitch_resolution": None, - "dis_mode": None, - "hhd_downsample": None, - "record_duration_s": None, - "config_ref": None, - "samples": store.to_audit_dict(), - } - if meta: - for k, v in meta.items(): - if k == "samples": - continue - base_meta[k] = v - if "samples" in meta and meta["samples"] is not None: - base_meta["samples"] = meta["samples"] - - doc = { - "schema_version": 2, - "unit": "N", - "channels": channels, - "meta": base_meta, - } - return doc, warnings + msg = f"{key}: no samples" + if msg not in warnings: + warnings.append(msg) + return cal.to_dict(), warnings def write_force_calibration( doc: dict[str, Any], path: str | Path, ) -> Path: - path = Path(path) - path.parent.mkdir(parents=True, exist_ok=True) - path.write_text( - json.dumps(doc, indent=2, ensure_ascii=False) + "\n", - encoding="utf-8", - ) - return path.resolve() + """Write a calibration dict (already built) to disk.""" + from orisys.calibration import parse + + cal = parse(doc) + return sdk_write(cal, path, overwrite=True) def export_force_calibration_v2( diff --git a/shared/src/orisys_app_shared/force/history.py b/shared/src/orisys_app_shared/force/history.py index 36002bf..8bda6cb 100644 --- a/shared/src/orisys_app_shared/force/history.py +++ b/shared/src/orisys_app_shared/force/history.py @@ -1,12 +1,20 @@ -"""Rolling force history with optional display low-pass.""" +"""Rolling force history with optional display low-pass. + +Uses a time-windowed buffer (keep samples in ``[t_end - window_sec, t_end]``) +with a headroom cap so high FPS still covers the visible X range after scroll. +""" from __future__ import annotations +import math from dataclasses import dataclass from typing import Optional import numpy as np +DEFAULT_MAX_FPS = 90.0 +HEADROOM = 1.5 + @dataclass class PlotState: @@ -41,8 +49,8 @@ def lowpass_filter( return y.copy() -def _npoints_for_window(window_sec: float, sample_rate_hz: float) -> int: - return max(2, int(round(float(window_sec) * float(sample_rate_hz)))) +def _max_points_for_window(window_sec: float, max_fps: float) -> int: + return max(2, int(math.ceil(float(window_sec) * float(max_fps) * HEADROOM))) class MetricHistory: @@ -55,35 +63,79 @@ class MetricHistory: sample_rate_hz: float | None = None, lowpass_cutoff_hz: float = 0.0, y_axis_min_span: float = 0.5, + max_fps: float = DEFAULT_MAX_FPS, ): self.channels = int(channels) self.window_sec = float(window_sec) + self.max_fps = max(1.0, float(max_fps)) self.sample_rate_hz = ( - float(sample_rate_hz) - if sample_rate_hz is not None - else (30.0 if self.window_sec <= 0 else 30.0) + float(sample_rate_hz) if sample_rate_hz is not None else 30.0 ) + # ``npoints`` overrides max capacity (tests / callers); else headroom cap. if npoints is None: - self.npoints = _npoints_for_window(self.window_sec, self.sample_rate_hz) + self.max_points = _max_points_for_window(self.window_sec, self.max_fps) else: - self.npoints = int(npoints) + self.max_points = max(2, int(npoints)) + # Back-compat alias used by older callers. + self.npoints = self.max_points self.lowpass_cutoff_hz = float(lowpass_cutoff_hz) self.y_axis_min_span = float(y_axis_min_span) + self._capacity = self.max_points self.reset() def set_window_sec(self, window_sec: float) -> None: self.window_sec = max(0.1, float(window_sec)) - self.npoints = _npoints_for_window(self.window_sec, self.sample_rate_hz) - self.reset() + self.max_points = _max_points_for_window(self.window_sec, self.max_fps) + self.npoints = self.max_points + self._ensure_capacity(self.max_points) + self._prune() + self._y_range_smooth = [None for _ in range(self.channels)] def reset(self) -> None: - self._data = np.full((self.npoints, 1 + self.channels), np.nan, dtype=np.float64) + self._capacity = max(self.max_points, 2) + self._data = np.full( + (self._capacity, 1 + self.channels), np.nan, dtype=np.float64 + ) self.sample_count = 0 self._t0: Optional[float] = None self._y_range_smooth: list[Optional[tuple[float, float]]] = [ None for _ in range(self.channels) ] + def _ensure_capacity(self, needed: int) -> None: + needed = max(needed, self.max_points, 2) + if needed <= self._capacity: + return + new_cap = max(needed, self._capacity * 2) + new_data = np.full( + (new_cap, 1 + self.channels), np.nan, dtype=np.float64 + ) + if self.sample_count > 0: + new_data[: self.sample_count] = self._data[: self.sample_count] + self._data = new_data + self._capacity = new_cap + + def _prune(self) -> None: + """Drop samples older than the time window, then enforce max_points.""" + n = self.sample_count + if n == 0: + return + t_end = float(self._data[n - 1, 0]) + cutoff = t_end - self.window_sec + times = self._data[:n, 0] + # First index with t >= cutoff + keep_from = int(np.searchsorted(times, cutoff, side="left")) + if keep_from > 0: + remaining = n - keep_from + self._data[:remaining] = self._data[keep_from:n] + self.sample_count = remaining + n = remaining + if n > self.max_points: + drop = n - self.max_points + remaining = n - drop + self._data[:remaining] = self._data[drop:n] + self.sample_count = remaining + def push(self, timestamp: float, *values: float) -> None: if len(values) != self.channels: raise ValueError(f"Expected {self.channels} values, got {len(values)}") @@ -91,12 +143,11 @@ class MetricHistory: self._t0 = float(timestamp) t = float(timestamp) - self._t0 row = (t, *map(float, values)) - if self.sample_count < self.npoints: - self._data[self.sample_count, :] = row - self.sample_count += 1 - return - self._data[:-1, :] = self._data[1:, :] - self._data[-1, :] = row + if self.sample_count >= self._capacity: + self._ensure_capacity(self.sample_count + 1) + self._data[self.sample_count, :] = row + self.sample_count += 1 + self._prune() def _smooth_y_range(self, channel: int, ymin: float, ymax: float) -> tuple[float, float]: if self._y_range_smooth[channel] is None: @@ -122,6 +173,24 @@ class MetricHistory: block = self._data[:n] times = block[:, 0].copy() values = block[:, 1:].copy() + t_end = float(times[-1]) + if t_end >= self.window_sec: + x_range = (t_end - self.window_sec, t_end) + # Keep setData lean: only points inside the visible window. + mask = times >= x_range[0] + times = times[mask] + values = values[mask] + # Discrete samples usually start slightly after the left edge; + # prepend a hold point so the curve spans the full X range. + if times.size > 0 and float(times[0]) > x_range[0]: + times = np.concatenate(([x_range[0]], times)) + values = np.vstack([values[0:1], values]) + else: + x_range = (0.0, self.window_sec) + + if times.size == 0: + return None + if self.lowpass_cutoff_hz > 0: for i in range(self.channels): values[:, i] = lowpass_filter( @@ -129,11 +198,6 @@ class MetricHistory: self.lowpass_cutoff_hz, sample_rate_hz=self.sample_rate_hz, ) - t_end = float(times[-1]) - if t_end >= self.window_sec: - x_range = (t_end - self.window_sec, t_end) - else: - x_range = (0.0, self.window_sec) y_ranges = [] for i in range(self.channels): col = values[:, i] diff --git a/shared/tests/test_force_history.py b/shared/tests/test_force_history.py new file mode 100644 index 0000000..1e19c89 --- /dev/null +++ b/shared/tests/test_force_history.py @@ -0,0 +1,63 @@ +"""Tests for time-windowed MetricHistory (force plot buffer).""" + +from __future__ import annotations + +import pytest + +from orisys_app_shared.force.history import MetricHistory + + +def _push_at_rate(hist: MetricHistory, *, fps: float, duration_sec: float, t0: float = 1000.0) -> None: + dt = 1.0 / fps + n = int(round(duration_sec * fps)) + 1 + for i in range(n): + hist.push(t0 + i * dt, 1.0, 0.0, -1.0) + + +def test_fill_phase_x_range_is_fixed_window(): + hist = MetricHistory(window_sec=4.0, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0) + _push_at_rate(hist, fps=30.0, duration_sec=2.0) + state = hist.state() + assert state is not None + assert state.x_range == pytest.approx((0.0, 4.0)) + assert float(state.times[-1]) < 4.0 + + +def test_high_fps_after_scroll_covers_x_range(): + window = 4.0 + hist = MetricHistory(window_sec=window, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0) + _push_at_rate(hist, fps=60.0, duration_sec=window + 1.0) + state = hist.state() + assert state is not None + x0, x1 = state.x_range + assert x1 - x0 == pytest.approx(window) + assert float(state.times[0]) == pytest.approx(x0) + assert float(state.times[-1]) == pytest.approx(x1) + + +def test_nominal_30hz_after_scroll_covers_x_range(): + window = 4.0 + hist = MetricHistory(window_sec=window, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0) + _push_at_rate(hist, fps=30.0, duration_sec=window + 2.0) + state = hist.state() + assert state is not None + x0, x1 = state.x_range + assert x1 - x0 == pytest.approx(window) + assert float(state.times[0]) == pytest.approx(x0) + + +def test_set_window_sec_prunes_older_samples(): + hist = MetricHistory(window_sec=4.0, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0) + _push_at_rate(hist, fps=30.0, duration_sec=5.0) + before = hist.state() + assert before is not None + assert float(before.times[-1]) >= 4.0 + + hist.set_window_sec(1.0) + after = hist.state() + assert after is not None + assert after.x_range[1] - after.x_range[0] == pytest.approx(1.0) + assert float(after.times[0]) == pytest.approx(after.x_range[0]) + # All retained samples must lie within the new window ending at t_end. + t_end = float(after.times[-1]) + assert float(after.times[0]) >= t_end - 1.0 - 1e-9