feat: ForceStreamSession 使用固定设计帧率的输出滤波(T07)
新增 FixedRateOutputFilter:设计帧率在会话创建时固定,实测帧率漂移和长间隔只报告、不重新设计也不清历史;request_output_settings 可跨线程排队,在力线程帧边界切换,只重建滤波链、清空历史,不改零点和参考版本,并发出 output_filter_changed StreamEvent(设置、指纹、生效帧、预热)。结果行带滤波设置身份与预热状态。已发布 ForceReadout 和旧 ForceSession/ForceOutputFilter 默认行为不变;设计帧率下与旧会话逐位一致。回归 391 项、lint 通过。
This commit is contained in:
@@ -1,6 +1,6 @@
|
||||
# 新力侧会话(T06a/T06b)
|
||||
# 新力侧会话(T06a/T06b、T07 输出滤波)
|
||||
|
||||
本文件是单路 SensorStream 计划 T06a/T06b 的 SDK 合同:新的力侧会话直接消费 T05 的
|
||||
本文件是单路 SensorStream 计划 T06a/T06b/T07 的 SDK 合同:新的力侧会话直接消费 T05 的
|
||||
`ImageMeasurement`(T03 `MeasurementFrame` 头 + 同帧 raw/field),参考版本取帧上的
|
||||
`reference_generation`。旧 `ForceSession` 仍为 App 保留到 T09/T10 迁移,本会话不提供
|
||||
`process_measurement`、`request_zero_external_reset`、`tracker_reset_seq` 等旧外部复位接口。
|
||||
@@ -40,14 +40,39 @@
|
||||
迟到、无效和带质量标记的结果不会被丢弃;拒绝帧仍保留 raw 特征。
|
||||
- **数值等价与滤波。** 零点/标定换算/读数滤波复用已发布算法;同一输入、无复位、同一标定时
|
||||
与旧 `ForceSession` 逐帧相等(T06a 覆盖标量曲线、无标定、median+lowpass 滤波器;T06b 覆盖
|
||||
`context` 与同帧 field)和 FN-only 全局增益模型(`outputs=["fn"]`);`settings` 为
|
||||
固定设计帧率与显式滤波切换属于 T07,不在本会话实现。
|
||||
field 模型与 FN-only 全局增益模型,三类标定都在同一平台同一次运行中对照旧会话)。
|
||||
- **三类标定与不匹配。** 标量曲线、field、FN-only 各有有效测试;无标定会话保持 raw 与
|
||||
`uncalibrated` 状态;field 模型缺 `context` 时构造报 `CalibrationError`,基线帧缺 field 或
|
||||
模型拒绝帧时零点失败并给出 `invalid_input`/原因;FN-only 的 FX/FY 永远保持 NaN +
|
||||
`uncalibrated`,不冒充三轴标定。
|
||||
- **输出滤波(T07)。** 设计帧率在构造时固定(`design_sample_rate_hz`),实测帧率漂移和长
|
||||
间隔只作为 `measured_sample_rate_hz`/`sample_rate_irregular` 诊断,不重新设计链路、不清空
|
||||
历史。`request_output_settings(settings)` 可从任意线程发出,只在力线程下一帧边界生效:只
|
||||
重建滤波链并清空其历史,图像参考与已完成的零点不变;相同设置是空操作。每次实际切换发出
|
||||
一个 `output_filter_changed` StreamEvent(规范化设置、指纹、算法版本、生效帧、设计帧率、
|
||||
预热信息,可 JSON 往返),由 `drain_events()` 取出。预热帧数为
|
||||
`max(1, median_window) + 2×lowpass_order(低通开启时)`;结果行提供当前设置身份与预热计数。
|
||||
全阶段关闭时输出与输入完全相同(含 NaN 与带符号的零)。已发布 `ForceOutputFilter`/旧会话
|
||||
的自适应调频默认行为不变。
|
||||
- **线程与只读。** 一个实例只属于一个力侧线程;输入数组只读,输出只含标量/状态/不可变进度。
|
||||
|
||||
## 输出滤波示例
|
||||
|
||||
```python
|
||||
from orisys.force_readout import OutputSettings
|
||||
from orisys.force_stream_session import ForceStreamSession
|
||||
|
||||
session = ForceStreamSession(
|
||||
sensor_family="box",
|
||||
settings=OutputSettings(sample_rate_hz=30.0, median_window=3, lowpass_hz=5.0),
|
||||
design_sample_rate_hz=30.0,
|
||||
)
|
||||
new_settings = OutputSettings(sample_rate_hz=30.0, median_window=5, lowpass_hz=10.0)
|
||||
session.request_output_settings(new_settings)
|
||||
assert session.output_settings.median_window == 3, "switch waits for a frame boundary"
|
||||
assert session.design_sample_rate_hz == 30.0
|
||||
```
|
||||
|
||||
## 示例
|
||||
|
||||
```python
|
||||
@@ -72,8 +97,9 @@ assert row.zero.phase in ("resetting", "collecting")
|
||||
|
||||
## 后续
|
||||
|
||||
T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价;spatial_gain/接触
|
||||
映射模型不在 T06b 验收范围,需要时另立任务。
|
||||
T07 才改变输出滤波的帧率与切换语义;T09/T10 迁移 App 后删除旧 `ForceSession`。
|
||||
T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价;T07 已交付固定
|
||||
设计帧率、显式切换事件与历史/直通合同。spatial_gain/接触映射模型不在 T06/T07 验收范围,
|
||||
需要时另立任务。
|
||||
T08 接单路流入口;T09/T10 迁移 App 后删除旧 `ForceSession`。
|
||||
另一项后续任务:删除旧 `ForceSession` 之前,先把零点、标定、读数引擎从旧类中抽出来,
|
||||
避免新会话依赖待删除的类型(T06b 或 T09 之前)。
|
||||
避免新会话依赖待删除的类型(T09 之前)。
|
||||
|
||||
@@ -0,0 +1,176 @@
|
||||
"""Fixed-frame-rate output filter for the new force stream session (T07).
|
||||
|
||||
The published :class:`~orisys.force_readout.ForceOutputFilter` re-designs its
|
||||
low-pass whenever the measured signal rate drifts more than 10% from the current
|
||||
design (``ForceOutputFilter._track_signal_rate``). The stage-1 contract (K3.3,
|
||||
K3.4) instead fixes the design rate when the session starts: later measured-rate
|
||||
drift and long gaps are *reported*, never used to re-tune or clear the filter.
|
||||
|
||||
:class:`FixedRateOutputFilter` subclasses the published filter, keeping its
|
||||
median → low-pass → threshold chain (and its exact pass-through/NaN behaviour),
|
||||
and overrides only:
|
||||
|
||||
* rate tracking: measure and report, never reconfigure;
|
||||
* :meth:`switch`: explicit, frame-boundary settings replacement that returns
|
||||
whether the chain changed and starts a documented warm-up counter;
|
||||
* warm-up and measured-rate diagnostics used by ``ForceStreamOutput``.
|
||||
|
||||
The algorithm version is ``output-filter/1``: its number is part of the settings
|
||||
identity emitted in ``StreamEvent`` payloads, so T14 can rebuild the same chain
|
||||
from an event.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from dataclasses import asdict, replace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .force_readout import ForceOutputFilter, OutputSettings
|
||||
|
||||
__all__ = [
|
||||
"FIXED_RATE_ALGORITHM",
|
||||
"FixedRateOutputFilter",
|
||||
"output_settings_fingerprint",
|
||||
"warmup_frames_for",
|
||||
]
|
||||
|
||||
#: Identity of this filter's algorithm; recorded with every settings switch.
|
||||
FIXED_RATE_ALGORITHM = "output-filter/1"
|
||||
|
||||
|
||||
def output_settings_fingerprint(settings: OutputSettings) -> str:
|
||||
"""Content identity of a normalised output-setting group."""
|
||||
payload = {
|
||||
"algorithm": FIXED_RATE_ALGORITHM,
|
||||
"settings": asdict(settings.normalised()),
|
||||
}
|
||||
text = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False)
|
||||
return hashlib.sha256(text.encode("utf-8")).hexdigest()
|
||||
|
||||
|
||||
def warmup_frames_for(settings: OutputSettings) -> int:
|
||||
"""Frames needed before a rebuilt chain is reported as warmed up.
|
||||
|
||||
Median: one full window. Low-pass: two orders of magnitude of settling is a
|
||||
conservative small count (each order costs about one sample time constant at
|
||||
the configured cutoff); thresholds are memory-less and add nothing. The value
|
||||
is an explicit contract, not a measured optimum.
|
||||
"""
|
||||
resolved = settings.normalised()
|
||||
frames = max(1, int(resolved.median_window))
|
||||
if resolved.lowpass_hz > 0:
|
||||
frames += 2 * max(1, int(resolved.lowpass_order))
|
||||
return int(frames)
|
||||
|
||||
|
||||
class FixedRateOutputFilter(ForceOutputFilter):
|
||||
"""Published filter chain with a fixed design rate and explicit switching."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
settings: OutputSettings,
|
||||
*,
|
||||
design_sample_rate_hz: float | None = None,
|
||||
) -> None:
|
||||
resolved = settings.normalised()
|
||||
design = (
|
||||
float(resolved.sample_rate_hz)
|
||||
if design_sample_rate_hz is None
|
||||
else float(design_sample_rate_hz)
|
||||
)
|
||||
if not np.isfinite(design) or design <= 0.0:
|
||||
raise ValueError("design_sample_rate_hz must be a positive finite number")
|
||||
# The chain itself is designed once, at the fixed rate.
|
||||
super().__init__(replace(resolved, sample_rate_hz=design))
|
||||
self.design_sample_rate_hz = design
|
||||
self._measured_sample_rate_hz: float | None = None
|
||||
self._sample_rate_irregular = False
|
||||
self._warmup_elapsed_frames = 0
|
||||
|
||||
# ------------------------------------------------------------ diagnostics
|
||||
@property
|
||||
def measured_sample_rate_hz(self) -> float | None:
|
||||
"""Measured signal rate (diagnostic only; never re-tunes the chain)."""
|
||||
return self._measured_sample_rate_hz
|
||||
|
||||
@property
|
||||
def sample_rate_irregular(self) -> bool:
|
||||
"""True when the measured rate differs from the design by more than 10%."""
|
||||
return bool(self._sample_rate_irregular)
|
||||
|
||||
@property
|
||||
def settings_fingerprint(self) -> str:
|
||||
return output_settings_fingerprint(self._settings)
|
||||
|
||||
@property
|
||||
def warmup_required_frames(self) -> int:
|
||||
return warmup_frames_for(self._settings)
|
||||
|
||||
@property
|
||||
def warmup_elapsed_frames(self) -> int:
|
||||
return int(self._warmup_elapsed_frames)
|
||||
|
||||
@property
|
||||
def warming_up(self) -> bool:
|
||||
return self._warmup_elapsed_frames < self.warmup_required_frames
|
||||
|
||||
# ------------------------------------------------------- rate observation
|
||||
def _track_signal_rate(self, signal_time_s: float | None) -> None:
|
||||
"""Measure the achieved rate for diagnostics only; never re-design."""
|
||||
if signal_time_s is None:
|
||||
return
|
||||
try:
|
||||
now = float(signal_time_s)
|
||||
except (TypeError, ValueError):
|
||||
return
|
||||
if not np.isfinite(now):
|
||||
return
|
||||
rate = float(self._rate.tick(now))
|
||||
if not self._rate.measured:
|
||||
return
|
||||
self._measured_sample_rate_hz = rate
|
||||
self._sample_rate_irregular = bool(
|
||||
abs(rate - self.design_sample_rate_hz) > 0.1 * self.design_sample_rate_hz
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------- processing
|
||||
def process(
|
||||
self, raw, newton, *, raw_valid=None, newton_valid=None, signal_time_s=None
|
||||
):
|
||||
result = super().process(
|
||||
raw,
|
||||
newton,
|
||||
raw_valid=raw_valid,
|
||||
newton_valid=newton_valid,
|
||||
signal_time_s=signal_time_s,
|
||||
)
|
||||
self._warmup_elapsed_frames += 1
|
||||
return result
|
||||
|
||||
# ----------------------------------------------------------------- switching
|
||||
def switch(self, settings: OutputSettings) -> bool:
|
||||
"""Replace the settings at a caller-controlled frame boundary.
|
||||
|
||||
Returns ``False`` when the normalised settings are unchanged (no chain
|
||||
rebuild, no history loss, no warm-up restart). An explicit switch is the
|
||||
only way to change the design: the new group's ``sample_rate_hz`` becomes
|
||||
the new fixed design rate (the caller only switches when that is intended).
|
||||
"""
|
||||
resolved = settings.normalised()
|
||||
if resolved == self._settings:
|
||||
return False
|
||||
self.design_sample_rate_hz = float(resolved.sample_rate_hz)
|
||||
self._warmup_elapsed_frames = 0
|
||||
self._measured_sample_rate_hz = None
|
||||
self._sample_rate_irregular = False
|
||||
self.configure(resolved)
|
||||
return True
|
||||
|
||||
def reset(self) -> None:
|
||||
super().reset()
|
||||
self._warmup_elapsed_frames = 0
|
||||
self._measured_sample_rate_hz = None
|
||||
self._sample_rate_irregular = False
|
||||
@@ -31,10 +31,12 @@ mutated; outputs carry scalars/statuses/immutable progress, not internal arrays.
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import threading
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .force_readout import OutputSettings
|
||||
from .force_session import (
|
||||
WORKFLOW_VERSION,
|
||||
ForceOutput,
|
||||
@@ -44,8 +46,12 @@ from .force_session import (
|
||||
from .force_session import (
|
||||
FrameStamp as ForceFrameStamp,
|
||||
)
|
||||
from .force_stream_filter import (
|
||||
FIXED_RATE_ALGORITHM,
|
||||
FixedRateOutputFilter,
|
||||
)
|
||||
from .sensor import FrameMeasurement
|
||||
from .stream_contract import MeasurementFrame
|
||||
from .stream_contract import MeasurementFrame, StreamEvent
|
||||
|
||||
__all__ = ["ForceStreamOutput", "ForceStreamSession"]
|
||||
|
||||
@@ -78,6 +84,15 @@ class ForceStreamOutput:
|
||||
late: bool = False
|
||||
calibration_fingerprint: str = ""
|
||||
workflow_version: str = WORKFLOW_VERSION
|
||||
# T07: the output-filter identity used for this frame and its warm-up state.
|
||||
output_settings: OutputSettings | None = None
|
||||
output_settings_fingerprint: str = ""
|
||||
design_sample_rate_hz: float | None = None
|
||||
measured_sample_rate_hz: float | None = None
|
||||
sample_rate_irregular: bool = False
|
||||
warmup_required_frames: int = 0
|
||||
warmup_elapsed_frames: int = 0
|
||||
warming_up: bool = False
|
||||
|
||||
@property
|
||||
def reference_generation(self) -> int:
|
||||
@@ -140,6 +155,7 @@ class ForceStreamSession:
|
||||
context=None,
|
||||
calibration=None,
|
||||
settings=None,
|
||||
design_sample_rate_hz: float | None = None,
|
||||
) -> None:
|
||||
self._source = _PendingMeasurementSource()
|
||||
self._engine = ForceSession(
|
||||
@@ -150,6 +166,17 @@ class ForceStreamSession:
|
||||
calibration=calibration,
|
||||
settings=settings,
|
||||
)
|
||||
# T07: the session owns the output filter so the design rate is fixed once
|
||||
# and explicit switches can be applied at a frame boundary. The engine's
|
||||
# zero/calibration code keeps calling the same filter interface.
|
||||
self._filter = FixedRateOutputFilter(
|
||||
self._engine.output_settings,
|
||||
design_sample_rate_hz=design_sample_rate_hz,
|
||||
)
|
||||
self._engine._filter = self._filter
|
||||
self._pending_filter_settings: OutputSettings | None = None
|
||||
self._filter_lock = threading.Lock()
|
||||
self._events: list[StreamEvent] = []
|
||||
self._family = sensor_family
|
||||
self._device_id = device_id
|
||||
self._version: int | None = None
|
||||
@@ -190,10 +217,40 @@ class ForceStreamSession:
|
||||
def output_settings(self):
|
||||
return self._engine.output_settings
|
||||
|
||||
@property
|
||||
def design_sample_rate_hz(self) -> float:
|
||||
"""The fixed filter design rate chosen when this session was created."""
|
||||
return float(self._filter.design_sample_rate_hz)
|
||||
|
||||
@property
|
||||
def output_filter(self) -> FixedRateOutputFilter:
|
||||
"""The session-owned filter (read-only diagnostic access)."""
|
||||
return self._filter
|
||||
|
||||
@property
|
||||
def last_output(self) -> ForceStreamOutput | None:
|
||||
return self._last_output
|
||||
|
||||
def request_output_settings(self, settings: OutputSettings) -> None:
|
||||
"""Queue an explicit output-filter switch for the next frame boundary.
|
||||
|
||||
Safe to call from another thread: the request only records the latest
|
||||
setting group. The force thread applies it at the start of the next
|
||||
:meth:`process` call (never in the middle of a frame), rebuilds the
|
||||
filter chain, clears its history and emits one ``output_filter_changed``
|
||||
event. Requesting the currently active settings is a no-op.
|
||||
"""
|
||||
if not isinstance(settings, OutputSettings):
|
||||
raise TypeError("settings must be an OutputSettings")
|
||||
with self._filter_lock:
|
||||
self._pending_filter_settings = settings.normalised()
|
||||
|
||||
def drain_events(self) -> tuple[StreamEvent, ...]:
|
||||
"""Return the events emitted since the previous drain, in order."""
|
||||
events = tuple(self._events)
|
||||
self._events.clear()
|
||||
return events
|
||||
|
||||
@property
|
||||
def measurement_requirements(self) -> tuple[bool, bool]:
|
||||
"""``(want_field, want_contact)`` the installed calibration model asks for.
|
||||
@@ -259,6 +316,36 @@ class ForceStreamSession:
|
||||
)
|
||||
|
||||
# ---------------------------------------------------------------- result rows
|
||||
def _apply_pending_output_filter(self, frame: MeasurementFrame) -> None:
|
||||
"""Apply a queued settings switch at this frame boundary (force thread)."""
|
||||
with self._filter_lock:
|
||||
pending = self._pending_filter_settings
|
||||
self._pending_filter_settings = None
|
||||
if pending is None:
|
||||
return
|
||||
if not self._filter.switch(pending):
|
||||
return
|
||||
settings = self._filter.settings
|
||||
self._events.append(
|
||||
StreamEvent(
|
||||
kind="output_filter_changed",
|
||||
stamp=frame.stamp,
|
||||
payload={
|
||||
"algorithm": FIXED_RATE_ALGORITHM,
|
||||
"settings": asdict(settings),
|
||||
"fingerprint": self._filter.settings_fingerprint,
|
||||
"design_sample_rate_hz": float(self._filter.design_sample_rate_hz),
|
||||
"effective_frame_id": int(frame.stamp.frame_id),
|
||||
"effective_source_frame_id": frame.stamp.source_frame_id,
|
||||
"effective_stamp": frame.stamp.to_dict(),
|
||||
"warmup": {
|
||||
"required_frames": int(self._filter.warmup_required_frames),
|
||||
"elapsed_frames": 0,
|
||||
},
|
||||
},
|
||||
)
|
||||
)
|
||||
|
||||
def _row(
|
||||
self,
|
||||
frame: MeasurementFrame,
|
||||
@@ -286,6 +373,14 @@ class ForceStreamSession:
|
||||
duplicate=duplicate,
|
||||
late=late,
|
||||
calibration_fingerprint=self._engine.calibration_fingerprint,
|
||||
output_settings=self._filter.settings,
|
||||
output_settings_fingerprint=self._filter.settings_fingerprint,
|
||||
design_sample_rate_hz=self._filter.design_sample_rate_hz,
|
||||
measured_sample_rate_hz=self._filter.measured_sample_rate_hz,
|
||||
sample_rate_irregular=self._filter.sample_rate_irregular,
|
||||
warmup_required_frames=self._filter.warmup_required_frames,
|
||||
warmup_elapsed_frames=self._filter.warmup_elapsed_frames,
|
||||
warming_up=self._filter.warming_up,
|
||||
)
|
||||
self._last_output = output
|
||||
return output
|
||||
@@ -398,12 +493,15 @@ class ForceStreamSession:
|
||||
zero window or readout filter, and without invalidating a completed zero.
|
||||
A reference-version or session change fails the old zero immediately (in
|
||||
any phase) and requires an explicit :meth:`request_zero` for the new one.
|
||||
An explicit output-filter switch queued by
|
||||
:meth:`request_output_settings` takes effect at this frame boundary.
|
||||
"""
|
||||
frame = getattr(measurement, "frame", None)
|
||||
if not isinstance(frame, MeasurementFrame):
|
||||
raise TypeError(
|
||||
"ForceStreamSession consumes an ImageMeasurement-like input"
|
||||
)
|
||||
self._apply_pending_output_filter(frame)
|
||||
identity = self._identity(frame)
|
||||
version = int(frame.reference_generation)
|
||||
session_id = str(frame.stamp.session_id)
|
||||
|
||||
@@ -0,0 +1,396 @@
|
||||
"""T07: fixed-rate output filtering, explicit switching and events.
|
||||
|
||||
The new session's filter is designed once (``design_sample_rate_hz``), reports
|
||||
measured-rate drift without re-tuning, applies a switch only at a frame boundary,
|
||||
emits exactly one ``output_filter_changed`` StreamEvent per change, and keeps the
|
||||
published ``ForceOutputFilter``/old session defaults untouched.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import json
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from orisys.force_readout import ForceOutputFilter, OutputSettings
|
||||
from orisys.force_stream_filter import (
|
||||
FixedRateOutputFilter,
|
||||
output_settings_fingerprint,
|
||||
warmup_frames_for,
|
||||
)
|
||||
from orisys.force_stream_session import ForceStreamOutput, ForceStreamSession
|
||||
from orisys.image_pipeline import ImageMeasurement
|
||||
from orisys.stream_contract import FrameStamp, MeasurementFrame, StreamEvent
|
||||
|
||||
FRAME_NS_30 = 33_333_333
|
||||
|
||||
|
||||
def _load_helpers():
|
||||
path = Path(__file__).with_name("test_force_session.py")
|
||||
spec = importlib.util.spec_from_file_location("_t07_helpers", path)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
assert spec.loader is not None
|
||||
spec.loader.exec_module(module)
|
||||
return module
|
||||
|
||||
|
||||
helpers = _load_helpers()
|
||||
|
||||
|
||||
def _settings(**overrides) -> OutputSettings:
|
||||
values = {
|
||||
"sample_rate_hz": 30.0,
|
||||
"median_window": 3,
|
||||
"lowpass_hz": 5.0,
|
||||
"lowpass_order": 2,
|
||||
}
|
||||
values.update(overrides)
|
||||
return OutputSettings(**values)
|
||||
|
||||
|
||||
def _measurement(
|
||||
frame_id: int,
|
||||
raw: tuple[float, float, float],
|
||||
*,
|
||||
version: int = 0,
|
||||
time_ns: int | None = None,
|
||||
) -> ImageMeasurement:
|
||||
stamp = FrameStamp(
|
||||
sensor_id="TEST-1",
|
||||
session_id="s1",
|
||||
frame_id=frame_id,
|
||||
source_frame_id=frame_id,
|
||||
capture_time_ns=(frame_id * FRAME_NS_30 if time_ns is None else int(time_ns)),
|
||||
clock_id="host",
|
||||
capture_time_basis="recorded_signal",
|
||||
)
|
||||
frame = MeasurementFrame(
|
||||
stamp=stamp,
|
||||
source_dt_s=None,
|
||||
reference_generation=version,
|
||||
)
|
||||
return ImageMeasurement(frame=frame, force_raw=raw, torque=0.0, flow=None)
|
||||
|
||||
|
||||
def _raw(frame_id: int) -> tuple[float, float, float]:
|
||||
return (1000.0 + frame_id, 2000.0 + 0.5 * frame_id, 3000.0 - 0.25 * frame_id)
|
||||
|
||||
|
||||
def _session(settings: OutputSettings | None = None, **kwargs) -> ForceStreamSession:
|
||||
return ForceStreamSession(
|
||||
sensor_family="box",
|
||||
settings=settings if settings is not None else _settings(),
|
||||
**kwargs,
|
||||
)
|
||||
|
||||
|
||||
def _ready(session: ForceStreamSession, *, count: int = 31) -> ForceStreamOutput:
|
||||
session.request_zero(
|
||||
reference_id="ref", declared_unloaded=True, expected_reference_generation=0
|
||||
)
|
||||
results = [
|
||||
session.process(_measurement(frame_id, _raw(frame_id)))
|
||||
for frame_id in range(1, count + 1)
|
||||
]
|
||||
assert results[-1].zero.phase == "ready"
|
||||
return results[-1]
|
||||
|
||||
|
||||
# --------------------------------------------------------------- fixed design rate
|
||||
def test_fixed_rate_reports_drift_without_redesigning_the_chain() -> None:
|
||||
settings = _settings(median_window=1, lowpass_hz=5.0)
|
||||
session = _session(settings, design_sample_rate_hz=30.0)
|
||||
reference = FixedRateOutputFilter(settings, design_sample_rate_hz=30.0)
|
||||
actual_hz = 20.0 # -33% from the design rate
|
||||
old = ForceOutputFilter(settings)
|
||||
|
||||
for frame_id in range(1, 26):
|
||||
time_ns = int(round(frame_id * 1e9 / actual_hz))
|
||||
raw = (1000.0 + frame_id, 2000.0, 3000.0)
|
||||
newton = (1.0, 2.0, 3.0)
|
||||
new_out = session.process(_measurement(frame_id, raw, time_ns=time_ns))
|
||||
expected_raw, _ = reference.process(
|
||||
raw,
|
||||
newton,
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(True, True, True),
|
||||
signal_time_s=time_ns / 1e9,
|
||||
)
|
||||
old.process(
|
||||
raw,
|
||||
newton,
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(True, True, True),
|
||||
signal_time_s=time_ns / 1e9,
|
||||
)
|
||||
assert new_out.raw == pytest.approx(expected_raw), frame_id
|
||||
|
||||
filter_ = session.output_filter
|
||||
assert session.design_sample_rate_hz == 30.0
|
||||
assert filter_.settings.sample_rate_hz == 30.0, "fixed design rate must not change"
|
||||
assert filter_.measured_sample_rate_hz == pytest.approx(actual_hz, rel=0.02)
|
||||
assert filter_.sample_rate_irregular is True
|
||||
assert new_out.design_sample_rate_hz == 30.0
|
||||
assert new_out.measured_sample_rate_hz == pytest.approx(actual_hz, rel=0.02)
|
||||
assert new_out.sample_rate_irregular is True
|
||||
# The published filter keeps its adaptive behaviour: this is what the new
|
||||
# fixed-rate contract deliberately does not do.
|
||||
assert old.settings.sample_rate_hz == pytest.approx(actual_hz, rel=0.02)
|
||||
assert old.settings.sample_rate_hz != pytest.approx(30.0, rel=0.01)
|
||||
|
||||
|
||||
def test_long_gap_does_not_clear_history_or_retune() -> None:
|
||||
session = _session(
|
||||
_settings(median_window=3, lowpass_hz=5.0), design_sample_rate_hz=30.0
|
||||
)
|
||||
reference = FixedRateOutputFilter(
|
||||
_settings(median_window=3, lowpass_hz=5.0), design_sample_rate_hz=30.0
|
||||
)
|
||||
outputs = []
|
||||
for frame_id in range(1, 16):
|
||||
# Regular frames, then a 0.5 s gap at frame 10.
|
||||
time_ns = frame_id * FRAME_NS_30 + (500_000_000 if frame_id >= 10 else 0)
|
||||
out = session.process(_measurement(frame_id, _raw(frame_id), time_ns=time_ns))
|
||||
expected, _ = reference.process(
|
||||
_raw(frame_id),
|
||||
(0.0, 0.0, 0.0),
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(False, False, False),
|
||||
signal_time_s=time_ns / 1e9,
|
||||
)
|
||||
assert out.raw == pytest.approx(expected), frame_id
|
||||
outputs.append(out.raw)
|
||||
assert session.output_filter.settings.sample_rate_hz == 30.0
|
||||
assert session.output_filter.measured_sample_rate_hz is not None
|
||||
assert any(a != b for a, b in zip(outputs[:9], outputs[9:])), (
|
||||
"filter history was used"
|
||||
)
|
||||
|
||||
|
||||
# -------------------------------------------------------------- equality at design
|
||||
def test_matches_old_session_filter_at_the_design_rate() -> None:
|
||||
settings = _settings(median_window=3, lowpass_hz=5.0)
|
||||
source = helpers.FakeMeasurementSource(_raw)
|
||||
old = helpers.ForceSession(
|
||||
source=source,
|
||||
sensor_family="box",
|
||||
settings=settings,
|
||||
)
|
||||
new = _session(settings)
|
||||
old.request_zero(reference_id="ref", declared_unloaded=True)
|
||||
new.request_zero(
|
||||
reference_id="ref", declared_unloaded=True, expected_reference_generation=0
|
||||
)
|
||||
old_raw, new_raw = [], []
|
||||
for frame_id in range(1, 41):
|
||||
old_out = helpers.process(old, source, frame_id)
|
||||
new_out = new.process(_measurement(frame_id, _raw(frame_id)))
|
||||
old_raw.append(np.asarray(old_out.raw))
|
||||
new_raw.append(np.asarray(new_out.raw))
|
||||
assert np.array_equal(np.stack(old_raw), np.stack(new_raw), equal_nan=True)
|
||||
|
||||
|
||||
# --------------------------------------------------------------- explicit switching
|
||||
def test_switch_at_frame_boundary_keeps_zero_and_emits_one_event() -> None:
|
||||
session = _session(_settings(median_window=3, lowpass_hz=5.0))
|
||||
_ready(session)
|
||||
before_zero = session.zero
|
||||
before_version = session.reference_generation
|
||||
before_settings = session.output_settings
|
||||
|
||||
session.request_output_settings(_settings(median_window=5, lowpass_hz=10.0))
|
||||
assert session.output_settings == before_settings, "not applied before a frame"
|
||||
assert session.drain_events() == (), "no event before the boundary frame"
|
||||
|
||||
frame = session.process(_measurement(40, _raw(40)))
|
||||
events = session.drain_events()
|
||||
assert len(events) == 1
|
||||
event = events[0]
|
||||
assert event.kind == "output_filter_changed"
|
||||
assert isinstance(event, StreamEvent)
|
||||
assert session.zero.phase == "ready", "a filter switch must not invalidate the zero"
|
||||
assert session.reference_generation == before_version
|
||||
assert session.zero == before_zero
|
||||
assert frame.output_settings == _settings(median_window=5, lowpass_hz=10.0)
|
||||
assert frame.output_settings_fingerprint == output_settings_fingerprint(
|
||||
frame.output_settings
|
||||
)
|
||||
assert event.payload["fingerprint"] == frame.output_settings_fingerprint
|
||||
assert event.payload["effective_frame_id"] == 40
|
||||
assert event.payload["warmup"]["required_frames"] == warmup_frames_for(
|
||||
frame.output_settings
|
||||
)
|
||||
assert event.payload["warmup"]["elapsed_frames"] == 0
|
||||
assert event.stamp.frame_id == 40
|
||||
|
||||
# The event is a T03 StreamEvent and survives JSON round-trip (T14 input).
|
||||
encoded = json.loads(json.dumps(event.to_dict()))
|
||||
assert StreamEvent.from_dict(encoded) == event
|
||||
|
||||
# History starts from the effective frame: the first post-switch outputs match
|
||||
# a fresh chain fed only the post-switch raw sequence (frame 40 included).
|
||||
reference = FixedRateOutputFilter(
|
||||
_settings(median_window=5, lowpass_hz=10.0), design_sample_rate_hz=30.0
|
||||
)
|
||||
expected, _ = reference.process(
|
||||
_raw(40),
|
||||
(0.0, 0.0, 0.0),
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(False, False, False),
|
||||
signal_time_s=40 * FRAME_NS_30 / 1e9,
|
||||
)
|
||||
assert frame.raw == pytest.approx(expected)
|
||||
for frame_id in range(41, 46):
|
||||
out = session.process(_measurement(frame_id, _raw(frame_id)))
|
||||
expected, _ = reference.process(
|
||||
_raw(frame_id),
|
||||
(0.0, 0.0, 0.0),
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(False, False, False),
|
||||
signal_time_s=frame_id * FRAME_NS_30 / 1e9,
|
||||
)
|
||||
assert out.raw == pytest.approx(expected), (
|
||||
"post-switch history must start from the effective frame"
|
||||
)
|
||||
|
||||
|
||||
def test_requesting_the_same_settings_is_a_no_op() -> None:
|
||||
session = _session(_settings(median_window=3, lowpass_hz=5.0))
|
||||
ready = _ready(session)
|
||||
same = session.output_settings
|
||||
session.request_output_settings(same)
|
||||
frame = session.process(_measurement(40, _raw(40)))
|
||||
assert session.drain_events() == ()
|
||||
assert frame.output_settings == same
|
||||
# Zero completion already restarted the filter history; a no-op request must
|
||||
# not restart it again: the warm-up counter simply continues.
|
||||
assert frame.warmup_elapsed_frames == ready.warmup_elapsed_frames + 1
|
||||
|
||||
|
||||
def test_switch_coalesces_to_the_latest_request_before_the_boundary() -> None:
|
||||
session = _session(_settings(median_window=3, lowpass_hz=5.0))
|
||||
_ready(session)
|
||||
first = _settings(median_window=5, lowpass_hz=10.0)
|
||||
latest = _settings(median_window=5, lowpass_hz=12.0)
|
||||
session.request_output_settings(first)
|
||||
session.request_output_settings(latest)
|
||||
frame = session.process(_measurement(40, _raw(40)))
|
||||
events = session.drain_events()
|
||||
assert len(events) == 1
|
||||
assert frame.output_settings == latest
|
||||
assert events[0].payload["settings"] == latest.__dict__
|
||||
|
||||
|
||||
def test_warmup_counts_frames_after_a_switch() -> None:
|
||||
session = _session(_settings(median_window=3, lowpass_hz=5.0))
|
||||
_ready(session)
|
||||
new_settings = _settings(median_window=5, lowpass_hz=10.0)
|
||||
required = warmup_frames_for(new_settings)
|
||||
session.request_output_settings(new_settings)
|
||||
rows = [
|
||||
session.process(_measurement(40 + index, _raw(40 + index)))
|
||||
for index in range(required + 2)
|
||||
]
|
||||
assert rows[0].warmup_elapsed_frames == 1
|
||||
assert rows[0].warming_up is True
|
||||
assert rows[0].warmup_required_frames == required
|
||||
assert rows[required - 1].warming_up is False
|
||||
assert rows[required - 1].warmup_elapsed_frames == required
|
||||
assert rows[-1].warming_up is False
|
||||
|
||||
|
||||
# -------------------------------------------------------------- thread boundary
|
||||
class _GatedSession(ForceStreamSession):
|
||||
"""Hold one frame inside the engine to prove switching waits for a boundary."""
|
||||
|
||||
def __init__(self, **kwargs) -> None:
|
||||
super().__init__(**kwargs)
|
||||
self.entered = threading.Event()
|
||||
self.release = threading.Event()
|
||||
self.gate_frame = 40
|
||||
|
||||
def _feed_engine(self, measurement, frame):
|
||||
if int(frame.stamp.frame_id) == self.gate_frame:
|
||||
self.entered.set()
|
||||
assert self.release.wait(timeout=5.0)
|
||||
return super()._feed_engine(measurement, frame)
|
||||
|
||||
|
||||
def test_threaded_switch_waits_for_the_next_frame_boundary() -> None:
|
||||
session = _GatedSession(
|
||||
sensor_family="box", settings=_settings(median_window=3, lowpass_hz=5.0)
|
||||
)
|
||||
_ready(session)
|
||||
old_settings = session.output_settings
|
||||
new_settings = _settings(median_window=5, lowpass_hz=10.0)
|
||||
results: list[ForceStreamOutput] = []
|
||||
|
||||
def run_frame() -> None:
|
||||
results.append(session.process(_measurement(40, _raw(40))))
|
||||
|
||||
worker = threading.Thread(target=run_frame)
|
||||
worker.start()
|
||||
try:
|
||||
assert session.entered.wait(timeout=5.0), "frame never reached the engine gate"
|
||||
# The frame is provably in flight: the switch request must not touch it.
|
||||
session.request_output_settings(new_settings)
|
||||
assert session.output_settings == old_settings, "no mid-frame reconfiguration"
|
||||
assert session.drain_events() == ()
|
||||
finally:
|
||||
session.release.set()
|
||||
worker.join(timeout=5.0)
|
||||
assert not worker.is_alive()
|
||||
assert results[0].output_settings == old_settings
|
||||
|
||||
next_frame = session.process(_measurement(41, _raw(41)))
|
||||
events = session.drain_events()
|
||||
assert len(events) == 1
|
||||
assert next_frame.output_settings == new_settings
|
||||
assert events[0].payload["effective_frame_id"] == 41
|
||||
|
||||
|
||||
# ---------------------------------------------------------- pass-through and history
|
||||
def test_all_stages_off_is_an_exact_passthrough_including_nan_and_signed_zero() -> None:
|
||||
passthrough = OutputSettings(sample_rate_hz=30.0, median_window=1, lowpass_hz=0.0)
|
||||
filter_ = FixedRateOutputFilter(passthrough, design_sample_rate_hz=30.0)
|
||||
raw = (0.0, -0.0, float("nan"))
|
||||
newton = (-0.0, 1.5, float("nan"))
|
||||
out_raw, out_newton = filter_.process(
|
||||
raw,
|
||||
newton,
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(True, True, True),
|
||||
signal_time_s=1.0,
|
||||
)
|
||||
assert out_raw[0] == raw[0] and out_raw[1] == raw[1]
|
||||
assert np.isnan(out_raw[2])
|
||||
assert bool(np.signbit(out_raw[1])) is True
|
||||
assert out_newton[0] == newton[0] and out_newton[1] == newton[1]
|
||||
assert np.isnan(out_newton[2])
|
||||
assert bool(np.signbit(out_newton[0])) is True
|
||||
|
||||
session = _session(passthrough)
|
||||
_ready(session)
|
||||
out = session.process(_measurement(40, (0.0, -0.0, float("nan"))))
|
||||
assert out.raw[0] == 0.0 and bool(np.signbit(out.raw[1])) is True
|
||||
assert np.isnan(out.raw[2])
|
||||
|
||||
|
||||
def test_published_force_output_filter_default_behaviour_is_unchanged() -> None:
|
||||
"""The old filter still re-designs on rate drift; the new one is opt-in."""
|
||||
settings = _settings(median_window=1, lowpass_hz=5.0)
|
||||
old = ForceOutputFilter(settings)
|
||||
for frame_id in range(1, 15):
|
||||
old.process(
|
||||
(float(frame_id), 0.0, 0.0),
|
||||
(0.0, 0.0, 0.0),
|
||||
raw_valid=(True, True, True),
|
||||
newton_valid=(False, False, False),
|
||||
signal_time_s=frame_id * 0.05,
|
||||
)
|
||||
assert old.settings.sample_rate_hz == pytest.approx(20.0, rel=0.02)
|
||||
assert old.settings.sample_rate_hz != settings.sample_rate_hz
|
||||
Reference in New Issue
Block a user