update to match calibration data handling
This commit is contained in:
Binary file not shown.
|
Before Width: | Height: | Size: 2.0 MiB After Width: | Height: | Size: 2.2 MiB |
@@ -128,6 +128,7 @@ class CustomerMainWindow(QMainWindow):
|
|||||||
|
|
||||||
self.setStatusBar(QStatusBar())
|
self.setStatusBar(QStatusBar())
|
||||||
self.cal_panel.status_message.connect(self.statusBar().showMessage)
|
self.cal_panel.status_message.connect(self.statusBar().showMessage)
|
||||||
|
self.cal_panel.calibration_exported.connect(self._on_calibration_exported)
|
||||||
self._build_menus()
|
self._build_menus()
|
||||||
self._wire_toolbar()
|
self._wire_toolbar()
|
||||||
self._wire_sensor_picker()
|
self._wire_sensor_picker()
|
||||||
@@ -434,6 +435,13 @@ class CustomerMainWindow(QMainWindow):
|
|||||||
self._apply_panel_visibility()
|
self._apply_panel_visibility()
|
||||||
self._persist_prefs()
|
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:
|
def _on_finger_3d_toggled(self, visible: bool) -> None:
|
||||||
if self.sensor_picker.sensor_type() != "finger":
|
if self.sensor_picker.sensor_type() != "finger":
|
||||||
return
|
return
|
||||||
|
|||||||
@@ -38,6 +38,7 @@ from orisys_app_shared.paths import calibration_export_dir, sensors_dir
|
|||||||
|
|
||||||
class CalPanel(QFrame):
|
class CalPanel(QFrame):
|
||||||
status_message = pyqtSignal(str)
|
status_message = pyqtSignal(str)
|
||||||
|
calibration_exported = pyqtSignal(str) # path applied to running sensor
|
||||||
|
|
||||||
def __init__(self, parent=None) -> None:
|
def __init__(self, parent=None) -> None:
|
||||||
super().__init__(parent)
|
super().__init__(parent)
|
||||||
@@ -335,4 +336,7 @@ class CalPanel(QFrame):
|
|||||||
if warnings:
|
if warnings:
|
||||||
msg += "\n" + t("cal.export_warnings", detail="; ".join(warnings))
|
msg += "\n" + t("cal.export_warnings", detail="; ".join(warnings))
|
||||||
self.status_message.emit(msg)
|
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)
|
QMessageBox.information(self, t("panels.cal"), msg)
|
||||||
|
|||||||
@@ -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.acquisition.constants import FORCE_SAMPLE_RATE_HZ, LOWPASS_CUTOFF_HZ
|
||||||
from orisys_app_shared.force import MetricHistory
|
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.i18n import t
|
||||||
from orisys_app_shared.theme import color
|
from orisys_app_shared.theme import color
|
||||||
|
|
||||||
@@ -40,6 +41,7 @@ class ForcePlotsPanel(QFrame):
|
|||||||
sample_rate_hz=FORCE_SAMPLE_RATE_HZ,
|
sample_rate_hz=FORCE_SAMPLE_RATE_HZ,
|
||||||
lowpass_cutoff_hz=LOWPASS_CUTOFF_HZ,
|
lowpass_cutoff_hz=LOWPASS_CUTOFF_HZ,
|
||||||
y_axis_min_span=0.5,
|
y_axis_min_span=0.5,
|
||||||
|
max_fps=DEFAULT_MAX_FPS,
|
||||||
)
|
)
|
||||||
|
|
||||||
root = QVBoxLayout(self)
|
root = QVBoxLayout(self)
|
||||||
|
|||||||
@@ -73,6 +73,7 @@ class FingerAcquisitionWorker:
|
|||||||
self._export_dir: Optional[Path] = None
|
self._export_dir: Optional[Path] = None
|
||||||
self._end_reason: Optional[str] = None
|
self._end_reason: Optional[str] = None
|
||||||
self._phase: str = "idle"
|
self._phase: str = "idle"
|
||||||
|
self._pending_calibration: Optional[str] = None
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_running(self) -> bool:
|
def is_running(self) -> bool:
|
||||||
@@ -142,6 +143,20 @@ class FingerAcquisitionWorker:
|
|||||||
raise RuntimeError("Finger runtime is not running")
|
raise RuntimeError("Finger runtime is not running")
|
||||||
runtime.tactile.reset()
|
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:
|
def _set_end_reason(self, reason: str) -> None:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if self._end_reason is None:
|
if self._end_reason is None:
|
||||||
@@ -246,6 +261,7 @@ class FingerAcquisitionWorker:
|
|||||||
none_streak_start = None
|
none_streak_start = None
|
||||||
tactile = runtime.tactile
|
tactile = runtime.tactile
|
||||||
try:
|
try:
|
||||||
|
self._consume_pending_calibration(tactile)
|
||||||
tactile.compute_deformation(decompose=True, check_motion=False)
|
tactile.compute_deformation(decompose=True, check_motion=False)
|
||||||
tactile.compute_contact()
|
tactile.compute_contact()
|
||||||
except Exception:
|
except Exception:
|
||||||
|
|||||||
@@ -95,6 +95,8 @@ def _ensure_bgr(img: np.ndarray) -> np.ndarray:
|
|||||||
|
|
||||||
|
|
||||||
def _sensor_force_calibrated(sensor, *, config_ref: str | Path | None = None) -> bool:
|
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:
|
if getattr(sensor, "_force_cal_sensor_name", None) is not None:
|
||||||
return True
|
return True
|
||||||
if config_ref is None:
|
if config_ref is None:
|
||||||
@@ -226,6 +228,7 @@ class SdkAcquisitionWorker:
|
|||||||
self._video_source: Optional[str | int] = None
|
self._video_source: Optional[str | int] = None
|
||||||
self._end_reason: Optional[str] = None
|
self._end_reason: Optional[str] = None
|
||||||
self._phase: str = "idle"
|
self._phase: str = "idle"
|
||||||
|
self._pending_calibration: Optional[str] = None
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_running(self) -> bool:
|
def is_running(self) -> bool:
|
||||||
@@ -294,6 +297,20 @@ class SdkAcquisitionWorker:
|
|||||||
raise RuntimeError("Sensor is not running")
|
raise RuntimeError("Sensor is not running")
|
||||||
sensor.reset()
|
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:
|
def _set_end_reason(self, reason: str) -> None:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if self._end_reason is None:
|
if self._end_reason is None:
|
||||||
@@ -384,6 +401,7 @@ class SdkAcquisitionWorker:
|
|||||||
|
|
||||||
none_streak_start = None
|
none_streak_start = None
|
||||||
try:
|
try:
|
||||||
|
self._consume_pending_calibration(self._sensor)
|
||||||
self._sensor.compute_deformation(decompose=True, check_motion=False)
|
self._sensor.compute_deformation(decompose=True, check_motion=False)
|
||||||
self._sensor.compute_contact()
|
self._sensor.compute_contact()
|
||||||
except Exception:
|
except Exception:
|
||||||
|
|||||||
@@ -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
|
from __future__ import annotations
|
||||||
|
|
||||||
import json
|
|
||||||
from copy import deepcopy
|
from copy import deepcopy
|
||||||
from dataclasses import asdict, dataclass, field
|
from dataclasses import asdict, dataclass, field
|
||||||
from datetime import datetime, timezone
|
from datetime import datetime
|
||||||
from pathlib import Path
|
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"]
|
ChannelKey = Literal["fn", "fx", "fy"]
|
||||||
CHANNEL_KEYS: tuple[ChannelKey, ...] = ("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
|
WEIGHT_EPS = 1e-6
|
||||||
|
|
||||||
|
|
||||||
@@ -33,6 +33,14 @@ class CalSample:
|
|||||||
n: int
|
n: int
|
||||||
std: float = 0.0
|
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
|
@dataclass
|
||||||
class CalSessionStore:
|
class CalSessionStore:
|
||||||
@@ -78,65 +86,22 @@ class CalSessionStore:
|
|||||||
out[key] = [asdict(s) for s in self.channel(key)]
|
out[key] = [asdict(s) for s in self.channel(key)]
|
||||||
return out
|
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]:
|
def meta_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
|
||||||
"""Pull stitch / tracker fields from a sensor.json for export meta."""
|
"""Pull stitch / tracker fields from a sensor.json for export meta."""
|
||||||
meta: dict[str, Any] = {
|
pipeline = pipeline_from_sensor_json(path)
|
||||||
"stitch_resolution": None,
|
return {
|
||||||
"dis_mode": None,
|
"stitch_resolution": pipeline.get("stitch_resolution"),
|
||||||
"hhd_downsample": None,
|
"dis_mode": pipeline.get("dis_mode"),
|
||||||
"config_ref": None,
|
"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(
|
def build_force_calibration_v2(
|
||||||
@@ -145,64 +110,66 @@ def build_force_calibration_v2(
|
|||||||
meta: dict[str, Any] | None = None,
|
meta: dict[str, Any] | None = None,
|
||||||
) -> tuple[dict[str, Any], list[str]]:
|
) -> tuple[dict[str, Any], list[str]]:
|
||||||
"""Build schema v2 JSON. Returns (document, skipped_channel_warnings)."""
|
"""Build schema v2 JSON. Returns (document, skipped_channel_warnings)."""
|
||||||
warnings: list[str] = []
|
meta = dict(meta or {})
|
||||||
channels: dict[str, Any] = {}
|
identity = {
|
||||||
for key, json_name in _CHANNEL_TO_JSON.items():
|
"sensor_type": meta.get("sensor_type", ""),
|
||||||
signed = key in ("fx", "fy")
|
"sn": meta.get("sn", ""),
|
||||||
block = _piecewise_channel(
|
}
|
||||||
json_name=json_name,
|
pipeline = {
|
||||||
samples=store.channel(key),
|
"stitch_resolution": meta.get("stitch_resolution"),
|
||||||
signed=signed,
|
"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):
|
try:
|
||||||
warnings.append(
|
cal = sdk_build(
|
||||||
f"{key}: need at least 2 points (have {len(store.channel(key))})"
|
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:
|
else:
|
||||||
warnings.append(f"{key}: no samples")
|
msg = f"{key}: no samples"
|
||||||
continue
|
if msg not in warnings:
|
||||||
channels[json_name] = block
|
warnings.append(msg)
|
||||||
|
return cal.to_dict(), warnings
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
def write_force_calibration(
|
def write_force_calibration(
|
||||||
doc: dict[str, Any],
|
doc: dict[str, Any],
|
||||||
path: str | Path,
|
path: str | Path,
|
||||||
) -> Path:
|
) -> Path:
|
||||||
path = Path(path)
|
"""Write a calibration dict (already built) to disk."""
|
||||||
path.parent.mkdir(parents=True, exist_ok=True)
|
from orisys.calibration import parse
|
||||||
path.write_text(
|
|
||||||
json.dumps(doc, indent=2, ensure_ascii=False) + "\n",
|
cal = parse(doc)
|
||||||
encoding="utf-8",
|
return sdk_write(cal, path, overwrite=True)
|
||||||
)
|
|
||||||
return path.resolve()
|
|
||||||
|
|
||||||
|
|
||||||
def export_force_calibration_v2(
|
def export_force_calibration_v2(
|
||||||
|
|||||||
@@ -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
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from typing import Optional
|
from typing import Optional
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
DEFAULT_MAX_FPS = 90.0
|
||||||
|
HEADROOM = 1.5
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class PlotState:
|
class PlotState:
|
||||||
@@ -41,8 +49,8 @@ def lowpass_filter(
|
|||||||
return y.copy()
|
return y.copy()
|
||||||
|
|
||||||
|
|
||||||
def _npoints_for_window(window_sec: float, sample_rate_hz: float) -> int:
|
def _max_points_for_window(window_sec: float, max_fps: float) -> int:
|
||||||
return max(2, int(round(float(window_sec) * float(sample_rate_hz))))
|
return max(2, int(math.ceil(float(window_sec) * float(max_fps) * HEADROOM)))
|
||||||
|
|
||||||
|
|
||||||
class MetricHistory:
|
class MetricHistory:
|
||||||
@@ -55,35 +63,79 @@ class MetricHistory:
|
|||||||
sample_rate_hz: float | None = None,
|
sample_rate_hz: float | None = None,
|
||||||
lowpass_cutoff_hz: float = 0.0,
|
lowpass_cutoff_hz: float = 0.0,
|
||||||
y_axis_min_span: float = 0.5,
|
y_axis_min_span: float = 0.5,
|
||||||
|
max_fps: float = DEFAULT_MAX_FPS,
|
||||||
):
|
):
|
||||||
self.channels = int(channels)
|
self.channels = int(channels)
|
||||||
self.window_sec = float(window_sec)
|
self.window_sec = float(window_sec)
|
||||||
|
self.max_fps = max(1.0, float(max_fps))
|
||||||
self.sample_rate_hz = (
|
self.sample_rate_hz = (
|
||||||
float(sample_rate_hz)
|
float(sample_rate_hz) if sample_rate_hz is not None else 30.0
|
||||||
if sample_rate_hz is not None
|
|
||||||
else (30.0 if self.window_sec <= 0 else 30.0)
|
|
||||||
)
|
)
|
||||||
|
# ``npoints`` overrides max capacity (tests / callers); else headroom cap.
|
||||||
if npoints is None:
|
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:
|
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.lowpass_cutoff_hz = float(lowpass_cutoff_hz)
|
||||||
self.y_axis_min_span = float(y_axis_min_span)
|
self.y_axis_min_span = float(y_axis_min_span)
|
||||||
|
self._capacity = self.max_points
|
||||||
self.reset()
|
self.reset()
|
||||||
|
|
||||||
def set_window_sec(self, window_sec: float) -> None:
|
def set_window_sec(self, window_sec: float) -> None:
|
||||||
self.window_sec = max(0.1, float(window_sec))
|
self.window_sec = max(0.1, float(window_sec))
|
||||||
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)
|
||||||
self.reset()
|
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:
|
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.sample_count = 0
|
||||||
self._t0: Optional[float] = None
|
self._t0: Optional[float] = None
|
||||||
self._y_range_smooth: list[Optional[tuple[float, float]]] = [
|
self._y_range_smooth: list[Optional[tuple[float, float]]] = [
|
||||||
None for _ in range(self.channels)
|
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:
|
def push(self, timestamp: float, *values: float) -> None:
|
||||||
if len(values) != self.channels:
|
if len(values) != self.channels:
|
||||||
raise ValueError(f"Expected {self.channels} values, got {len(values)}")
|
raise ValueError(f"Expected {self.channels} values, got {len(values)}")
|
||||||
@@ -91,12 +143,11 @@ class MetricHistory:
|
|||||||
self._t0 = float(timestamp)
|
self._t0 = float(timestamp)
|
||||||
t = float(timestamp) - self._t0
|
t = float(timestamp) - self._t0
|
||||||
row = (t, *map(float, values))
|
row = (t, *map(float, values))
|
||||||
if self.sample_count < self.npoints:
|
if self.sample_count >= self._capacity:
|
||||||
self._data[self.sample_count, :] = row
|
self._ensure_capacity(self.sample_count + 1)
|
||||||
self.sample_count += 1
|
self._data[self.sample_count, :] = row
|
||||||
return
|
self.sample_count += 1
|
||||||
self._data[:-1, :] = self._data[1:, :]
|
self._prune()
|
||||||
self._data[-1, :] = row
|
|
||||||
|
|
||||||
def _smooth_y_range(self, channel: int, ymin: float, ymax: float) -> tuple[float, float]:
|
def _smooth_y_range(self, channel: int, ymin: float, ymax: float) -> tuple[float, float]:
|
||||||
if self._y_range_smooth[channel] is None:
|
if self._y_range_smooth[channel] is None:
|
||||||
@@ -122,6 +173,24 @@ class MetricHistory:
|
|||||||
block = self._data[:n]
|
block = self._data[:n]
|
||||||
times = block[:, 0].copy()
|
times = block[:, 0].copy()
|
||||||
values = block[:, 1:].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:
|
if self.lowpass_cutoff_hz > 0:
|
||||||
for i in range(self.channels):
|
for i in range(self.channels):
|
||||||
values[:, i] = lowpass_filter(
|
values[:, i] = lowpass_filter(
|
||||||
@@ -129,11 +198,6 @@ class MetricHistory:
|
|||||||
self.lowpass_cutoff_hz,
|
self.lowpass_cutoff_hz,
|
||||||
sample_rate_hz=self.sample_rate_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 = []
|
y_ranges = []
|
||||||
for i in range(self.channels):
|
for i in range(self.channels):
|
||||||
col = values[:, i]
|
col = values[:, i]
|
||||||
|
|||||||
@@ -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
|
||||||
Reference in New Issue
Block a user