From f5738375fd0329fe3b68e1fb08ab2b6153216622 Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Wed, 21 Oct 2026 20:52:11 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20SensorStream=20=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E5=AE=8C=E6=95=B4=E6=A8=A1=E5=BC=8F=E3=80=81?= =?UTF-8?q?=E6=BA=90=E6=97=B6=E9=97=B4=E4=B8=8E=20field=20=E8=BE=93?= =?UTF-8?q?=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit T08b 补齐文件/debug 完整处理与背压、源帧号和时间基、同帧 field 快照，输入结束时未完成归零明确报错。独立验收见 docs/workflow/sdk-sensorstream-implementation-plan.zh.md 第 44 轮。 Co-Authored-By: Claude Opus 5.5 --- docs/force-stream-session.zh.md | 42 +- src/orisys/pipeline.py | 10 +- src/orisys/sensor.py | 20 +- src/orisys/sensor_stream.py | 536 +++++++++++++++++++---- tests/test_sensor_stream.py | 729 ++++++++++++++++++++++++++++++++ 5 files changed, 1234 insertions(+), 103 deletions(-) diff --git a/docs/force-stream-session.zh.md b/docs/force-stream-session.zh.md index c04cd75..74196bc 100644 --- a/docs/force-stream-session.zh.md +++ b/docs/force-stream-session.zh.md @@ -1,4 +1,4 @@ -# 新力侧会话与单路入口（T06a/T06b、T07 输出滤波、T08a） +# 新力侧会话与单路入口（T06a/T06b、T07 输出滤波、T08a/T08b） 本文件是单路 SensorStream 计划 T06a/T06b/T07 的 SDK 合同：新的力侧会话直接消费 T05 的 `ImageMeasurement`（T03 `MeasurementFrame` 头 + 同帧 raw/field），参考版本取帧上的 @@ -73,7 +73,7 @@ assert session.output_settings.median_window == 3, "switch waits for a frame bou assert session.design_sample_rate_hz == 30.0 ``` -## SensorStream 入口（T08a） +## SensorStream 入口（T08a/T08b） `SensorStream` 把 T04–T07 串成一个运行中的单路入口： @@ -81,23 +81,35 @@ assert session.design_sample_rate_hz == 30.0 已打开的 Sensor 或外部喂帧时只借用，`stop()` 不关闭借用源。运行中只有一个采集所有者； 流在 `start()` 启用 measurement session，`stop()`/启动失败时恢复借用 Sensor 原来的模式 （原本已启用的保持启用）。 -- **实时 latest。** `push_frame(...)` 的未领取候选帧会被最新输入覆盖并计入 +- **实时 latest（T08a）。** `push_frame(...)` 的未领取候选帧会被最新输入覆盖并计入 `overwritten_frames`；已经交给流水线的帧照常完成。`wait_output()` 是 latest-wins 结果流， `drain_events()` 取会话事件。 +- **文件/debug 完整（T08b）。** `realtime=False` 时逐帧处理每个被接受的输入：外部帧进入 + 有上限的 FIFO（`depth`），计算跟不上时 `push_frame` 阻塞并计入 `backpressure_waits`，不丢帧； + `end_input()` 结束序列，`wait_outputs(n)`/`drain_outputs()` 按发布顺序返回结果。内置文件源由 + `SensorStream.open(path, realtime=False, nominal_fps=...)` 打开，同样逐帧处理。文件模式下， + 只有在**所有已接受帧都已通过力阶段**（`published >= accepted`）且零点仍未完成时， + `reset_and_zero` 才以 `SensorStreamError`（“input ended before unloaded zero completed”） + 结束；生产者刚读到结束标记、管线中还有已接受帧时不会提前判失败，足够的已接受帧仍会正常完成零点。 - **线程/版本。** 复用 `StreamPipeline` 的线程与原子 `request_reset()` 版本；力侧只在 HHD 线程的 `on_frame` 中调用 `ForceStreamSession`，因此 T07 的 `drain_events` 线程约束由门面满足， 没有第二套版本或复位记账。 - **组合归零。** `reset_and_zero(reference_id=...)` 执行“图像复位确认 → 同版本 30 帧无载基线 → 力 ready”；用户不管理版本号。在途旧版本帧不会进入新基线（T06a 的规则）。每次调用只等待 本条命令的结果：力线程把该命令的 `ZeroProgress`/错误写回命令对象并通知，调用方线程不读取 - `session.zero`，因此连续两次调用时第二次不会复用第一次的零点。 -- **T08a 边界。** 实时模式、无标定/标量会话、逐帧结果最新视图已实现；field/contact 的 - `include`、文件/debug 完整逐帧、有界背压、时间来源与 PTS 标注、`source_dt_s`/长间隔诊断和 - 完整生命周期诊断属于 **T08b**。 -- **时间基准（T08b 实现约定）。** 实时相机 `capture_monotonic`；Orisys 自己录像（文件内保存 - 信号时间）`recorded_signal`；外部调用方提供可证明的容器 PTS 才 `video_pts`；内置 OpenCV - 读取普通视频文件用 `estimated_source`，`capture_time_ns` 按 `source_frame_id ÷ 名义帧率` 推算， - POS_MSEC 只进 diagnostics。T08a 的外部帧默认 `capture_monotonic`。 + `session.zero`，因此连续两次调用时第二次不会复用第一次的零点。**超时后命令仍在力线程进行** + （仍可能稍后 ready），调用方不应假设它被取消；在下一条命令完成前再次调用会被拒绝。 +- **时间来源（T08b）。** 实时相机 `capture_monotonic`，内置采集用 `CapturedFrame.host_mono_ns` + 和 `sequence`；Orisys 自己录像（文件内保存信号时间）`recorded_signal`；外部调用方提供可证明 + 的容器 PTS 才 `video_pts`（`source_pts` 原值保留）；内置 OpenCV 普通视频用 + `estimated_source`，`capture_time_ns = source_frame_id ÷ nominal_fps`，`CAP_PROP_POS_MSEC` + 只作为 `diagnostics` 的 `pos_msec`，不填 `source_pts`。没有时间的输入按帧号推算并标 + `estimated_source`。同一 PTS 的不同源帧是两帧；入口按源身份忽略重复/迟到输入并计数。 +- **诊断与 field（T08b）。** 结果行的 `diagnostics` 含覆盖/重复/迟到计数、处理延迟、 + `long_gap`、`source_dt_s`、`source_time_estimated` 和 `pos_msec`（有则）；`source_dt_s` 取同路 + 上一条实际处理帧的源时间差。`include=("field",)` 让 `StreamPipeline` 在 HHD 线程 `_build` + 中复制同帧 field；门面在标定模型需要 field 时自动启用。默认快照不含 field，已发布场不会被 + 后续帧改写。 ```python import numpy as np @@ -144,8 +156,8 @@ assert row.zero.phase in ("resetting", "collecting") ## 后续 T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价；T07 已交付固定 -设计帧率、显式切换事件与历史/直通合同；T08a 已交付 SensorStream 实时入口、所有权与组合归零。 +设计帧率、显式切换事件与历史/直通合同；T08a 已交付 SensorStream 实时入口、所有权与组合归零； +T08b 已交付文件/debug 完整逐帧、有界背压、时间来源/PTS 标注、`include="field"` 与诊断。 spatial_gain/接触映射模型不在 T06/T07 验收范围，需要时另立任务。 -T08b 补齐文件/debug 完整逐帧、有界背压、时间来源/PTS 标注与完整诊断后再进入 T09/T10（App 迁移）。 -另一项后续任务：删除旧 `ForceSession` 之前，先把零点、标定、读数引擎从旧类中抽出来， -避免新会话依赖待删除的类型（T09 之前）。 +下一步进入 T09/T10（App 迁移）；删除旧 `ForceSession` 前先把零点、标定、读数引擎从旧类中 +抽出来，避免新会话依赖待删除的类型。 diff --git a/src/orisys/pipeline.py b/src/orisys/pipeline.py index a40c548..0c4b49d 100644 --- a/src/orisys/pipeline.py +++ b/src/orisys/pipeline.py @@ -84,7 +84,7 @@ __all__ = [ #: Array fields a snapshot can carry. ``image`` is the working grey frame; the #: rest come from ``Sensor.read_info`` (already copied by the SDK). -_ARRAY_FIELDS = ("image", "flow", "vnormal", "vshear", "depth", "centroid", "contour") +_ARRAY_FIELDS = ("image", "flow", "vnormal", "vshear", "depth", "centroid", "contour", "field") #: How many periods the stages wait on the pause event before re-checking for a #: stop. Small enough to resume promptly, large enough not to spin a core. @@ -165,6 +165,11 @@ class FrameSnapshot: #: reset is queued, not synchronous). Additive field: producers that do not #: use it leave it at 0. tracker_reset_seq: int = 0 + #: Same-frame HHD field payload, copied on the HHD thread during ``_build`` + #: when requested through ``StreamPipeline(include=("field",))``. ``None`` on + #: the default path; the copy belongs to this snapshot and later frames cannot + #: rewrite it. Additive field for T08b field-calibration handoff. + field: dict | None = None @dataclass @@ -759,6 +764,9 @@ class StreamPipeline: # This frame's own measurement version, carried through the flow queue: # never the live counter, which a later frame may already have bumped. tracker_reset_seq=int(reference_version), + # Same-frame field, copied here on the HHD thread that computed it; + # current_field() already returns a private payload copy. + field=dict(sensor.current_field()) if "field" in include else None, ) def _fps(self) -> float: diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 6f49b6b..062c236 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -36,6 +36,7 @@ from .Stitcher import Stitcher from .capture import ( CaptureConfig, CaptureSource, + CapturedFrame, ReconnectPolicy, is_live_camera, normalize_source, @@ -274,6 +275,10 @@ class Sensor: self.last_log_time = time.perf_counter() self._fps = 0 self._last_capture_time = 0.0 + #: Metadata of the last frame the built-in capture path read (additive, + #: for streams that need the source identity and host clocks). Stays + #: None for externally supplied frames and for bare test doubles. + self._last_captured_frame: CapturedFrame | None = None self._last_processing_time = 0.0 self._last_stage_times = { # Frame-post-processing roll-ups (seconds). @@ -2054,9 +2059,20 @@ class Sensor: # return self.cap is not None and self.cap.isOpened() def _read_frame(self): + captured: CapturedFrame | None = None if self._capture_source is not None and self._capture_source.running: - return self._capture_source.read_latest(copy=True, wait_s=1.0) - return self._read_frame_raw() + captured = self._capture_source.read_latest_stamped(copy=True, wait_s=1.0) + elif self._capture_source is not None: + captured = self._capture_source.read_raw_stamped() + self._last_captured_frame = captured + if captured is None: + return False, None + return True, captured.image + + @property + def last_captured_frame(self) -> CapturedFrame | None: + """Source identity/clocks of the frame the built-in capture path read last.""" + return self._last_captured_frame def _read_frame_raw(self): if self._capture_source is not None: diff --git a/src/orisys/sensor_stream.py b/src/orisys/sensor_stream.py index 428bb49..02190a9 100644 --- a/src/orisys/sensor_stream.py +++ b/src/orisys/sensor_stream.py @@ -1,42 +1,57 @@ -"""Single-stream entry point: SensorStream (T08a). +"""Single-stream entry point: SensorStream (T08a + T08b). -``SensorStream`` ties the accepted T04–T07 blocks into one place: +``SensorStream`` ties the accepted T04–T08a blocks into one place: * one capture owner per running stream: either it creates the ``Sensor`` (:meth:`SensorStream.open`, closes it on stop) or the caller lends an opened - Sensor/feeds prepared frames (:meth:`push_frame`, the frame source is the + Sensor/feeds prepared frames (:meth:`push_frame`; the frame source is the caller's and stays open); -* ``StreamPipeline`` remains the threading/reset engine (bounded producer queue, - atomic ``request_reset`` version, flow/hhd ownership), so no second version or - reset bookkeeping is introduced; +* ``StreamPipeline`` remains the threading/reset engine (bounded queues, atomic + ``request_reset`` version, flow/hhd ownership), so no second version or reset + bookkeeping is introduced; * the HHD stage's ``on_frame`` callback is the single force-side thread. It - builds the T03 ``MeasurementFrame`` for the finished snapshot and calls + builds the T03 ``MeasurementFrame`` for a finished snapshot and calls ``ForceStreamSession.process`` exactly once per published snapshot. The - facade also applies queued ``reset_and_zero`` / output-filter commands there, - so the force session is never touched from a consumer thread; -* real-time mode keeps only the newest candidate input (a new push overwrites an - unclaimed candidate and counts the overwrite); a frame already handed to the - pipeline finishes normally; + facade also applies queued ``reset_and_zero`` commands there, so the force + session is never touched from a consumer thread; +* **real-time** mode keeps only the newest candidate input (a new push overwrites + an unclaimed candidate and counts the overwrite); a frame already handed to + the pipeline finishes normally; +* **file/debug** mode (``realtime=False``) processes every accepted input in + order: external frames go through a bounded input queue whose producer blocks + when the computation is behind (backpressure), and built-in capture reads every + file frame; no frame is dropped; +* source timing is explicit (K4.2/K4.3): + - ``capture_monotonic`` for live capture (``CapturedFrame.host_mono_ns``); + - ``recorded_signal`` for an Orisys recording that stores its own signal time; + - ``video_pts`` only when the caller supplies a provable container PTS + (``source_pts`` preserved verbatim); + - ``estimated_source`` for a built-in OpenCV video: ``capture_time_ns`` is + computed from ``source_frame_id / nominal_fps`` and ``CAP_PROP_POS_MSEC`` is + reported in diagnostics only, never as ``source_pts``; +* the entry layer ignores duplicate and late source identities and counts them; + a duplicate/late input never advances or invalidates a zero; * ``reset_and_zero`` combines the image reset (via ``StreamPipeline.request_reset`` atomic version) with the unloaded-zero request (via the force thread), and - returns once the session reports ready. Users do not manage version numbers. + returns once that command reports ready. Users do not manage version numbers. + If the caller times out, the command is still in progress in the force thread; + another ``reset_and_zero`` is rejected until it finishes. -T08a deliberately stops at real-time mode, a single force output stream and the -combined zero. File/debug complete processing, bounded backpressure, source-time -provenance (real PTS vs estimated source time) and the full lifecycle diagnostics -are the T08b scope. Field/contact calibration inputs are not wired yet either -(``include="field"`` is T08b); the current facade covers uncalibrated and scalar -sessions. +``include=("field",)`` asks the pipeline to attach the same-frame HHD field +payload; the facade adds it automatically when the calibration model needs it. +The field is copied in the HHD stage's ``_build`` (never read from a consumer +thread), and the default snapshot path is unchanged. One instance belongs to one running stream; one thread owns the force session. """ from __future__ import annotations +import queue import threading import time -from collections.abc import Mapping -from dataclasses import dataclass, field +from collections.abc import Iterable, Mapping +from dataclasses import dataclass, field, replace from typing import Any import numpy as np @@ -46,12 +61,18 @@ from .force_session import ZeroProgress from .force_stream_session import ForceStreamOutput, ForceStreamSession from .pipeline import ProducedFrame, StreamPipeline from .sensor import Sensor -from .stream_contract import FrameStamp, MeasurementFrame, StreamEvent +from .stream_contract import TIME_BASES, FrameStamp, MeasurementFrame, StreamEvent __all__ = ["SensorStream", "SensorStreamError"] #: How long reset_and_zero waits for the force session to report ready. DEFAULT_ZERO_TIMEOUT_S = 30.0 +#: Source-time gap above which a processed frame is flagged as a long gap. +DEFAULT_LONG_GAP_S = 0.5 +#: Upper bound on retained output rows (the latest-view row is always kept). +_HISTORY_LIMIT = 4096 +#: Upper bound on remembered source identities for duplicate detection. +_SEEN_LIMIT = 4096 class SensorStreamError(RuntimeError): @@ -66,6 +87,7 @@ class _QueuedFrame: time_basis: str clock_id: str source_pts: int | float | None = None + media_time_ms: float | None = None @dataclass @@ -96,22 +118,27 @@ class SensorStream: session_id: str = "stream", realtime: bool = True, depth: int = 1, + nominal_fps: float | None = None, + include: Iterable[str] = (), + long_gap_s: float = DEFAULT_LONG_GAP_S, owns_sensor: bool = False, verbose: bool = False, + external_input: bool | None = None, ) -> None: - if not realtime: - raise NotImplementedError( - "file/debug complete processing is the T08b scope" - ) self.sensor = sensor self._owns_sensor = bool(owns_sensor) self._verbose = bool(verbose) - self._external_input = bool(getattr(sensor, "_external_only", False)) - if not self._external_input: - # Built-in capture owns the candidate inside the Sensor's capture - # source; external prepared frames arrive through push_frame(). - self._external_input = False + self._external_input = ( + bool(getattr(sensor, "_external_only", False)) + if external_input is None + else bool(external_input) + ) + self._realtime = bool(realtime) self._session_id = str(session_id) + self._depth = max(1, int(depth)) + self._long_gap_s = float(long_gap_s) + if self._long_gap_s <= 0: + raise ValueError("long_gap_s must be positive") self._sensor_id = ( str(sensor_id) if sensor_id is not None @@ -134,12 +161,21 @@ class SensorStream: settings=settings, design_sample_rate_hz=design_sample_rate_hz, ) + self._nominal_fps = self._resolve_nominal_fps(sensor, nominal_fps) + requested = set(include) + unknown = requested - {"field"} + if unknown: + raise ValueError( + f"unknown include fields {sorted(unknown)}; SensorStream supports only 'field'" + ) + want_field = "field" in requested or bool( + self.session.measurement_requirements[0] + ) + self._include = ("field",) if want_field else () enable = getattr(sensor, "enable_measurement_session", None) self._has_measurement_session = callable(enable) and hasattr( sensor, "disable_measurement_session" ) - #: The borrowed Sensor's published measurement-session mode before we - #: touched it; start() records it, stop()/failed start restores it. self._sensor_session_was_enabled = bool( getattr( sensor, @@ -150,21 +186,36 @@ class SensorStream: self._sensor_session_enabled_by_stream = False self._cond = threading.Condition() self._candidate: _QueuedFrame | None = None + self._input_queue: queue.Queue | None = ( + None if self._realtime else queue.Queue(maxsize=self._depth) + ) self._overwritten = 0 + self._duplicate_inputs = 0 + self._late_inputs = 0 + self._backpressure_waits = 0 + self._seen_source_ids: dict[int, None] = {} + self._last_input_source_frame_id: int | None = None self._input_counter = 0 + self._accepted_inputs = 0 self._stop = threading.Event() + self._end_of_input = False + self._end_of_input_seen = False self._latest_output: ForceStreamOutput | None = None self._outputs_seen = 0 + self._published_outputs = 0 + self._history: list[ForceStreamOutput] = [] self._delivered = 0 self._events: list[StreamEvent] = [] self._pending_zero: _ZeroCommand | None = None self._active_zero: _ZeroCommand | None = None self._last_source_time_ns: int | None = None + self._last_source_dt_s: float | None = None + self._last_long_gap = False self._pipeline = StreamPipeline( sensor, self._produce, - depth=max(1, int(depth)), - include=(), + depth=self._depth, + include=self._include, on_frame=self._on_snapshot, verbose=self._verbose, ) @@ -184,10 +235,16 @@ class SensorStream: session_id: str = "stream", realtime: bool = True, depth: int = 1, + nominal_fps: float | None = None, + include: Iterable[str] = (), + long_gap_s: float = DEFAULT_LONG_GAP_S, verbose: bool = False, sensor_options: Mapping[str, Any] | None = None, ) -> SensorStream: - """Open a built-in capture source; the stream owns and closes the Sensor.""" + """Open a built-in source; the stream owns and closes the Sensor. + + ``realtime=False`` uses file/debug complete processing (every file frame). + """ sensor = Sensor( source, config_name=config_name, @@ -205,6 +262,9 @@ class SensorStream: session_id=session_id, realtime=realtime, depth=depth, + nominal_fps=nominal_fps, + include=include, + long_gap_s=long_gap_s, owns_sensor=True, verbose=verbose, ) @@ -221,6 +281,19 @@ class SensorStream: return name return "box" + @staticmethod + def _resolve_nominal_fps(sensor: Sensor, nominal_fps: float | None) -> float: + if nominal_fps is not None: + value = float(nominal_fps) + else: + readout = getattr(sensor, "_force_readout", None) + value = float(getattr(readout, "config_fps", 0.0) or 0.0) + if value <= 0: + value = 30.0 + if not np.isfinite(value) or value <= 0: + raise ValueError("nominal_fps must be a positive finite number") + return value + # --------------------------------------------------------------- lifecycle def start(self) -> SensorStream: if self._pipeline.running: @@ -235,8 +308,6 @@ class SensorStream: ) ) if not self._sensor_session_was_enabled: - # The new session owns zero/calibration/filter; the published - # automatic path must not run in parallel on the hhd thread. self.sensor.enable_measurement_session() self._sensor_session_enabled_by_stream = True try: @@ -247,7 +318,6 @@ class SensorStream: return self def _restore_sensor_session(self) -> None: - """Give a borrowed Sensor its pre-stream measurement-session mode back.""" if not self._sensor_session_enabled_by_stream: return self._sensor_session_enabled_by_stream = False @@ -270,11 +340,16 @@ class SensorStream: self._active_zero = None self._cond.notify_all() try: - self._pipeline.stop(timeout=timeout) - finally: - self._restore_sensor_session() - if self._owns_sensor: - self.sensor.disconnect() + try: + self._pipeline.stop(timeout=timeout) + finally: + self._restore_sensor_session() + if self._owns_sensor: + self.sensor.disconnect() + except SensorStreamError: + raise + except BaseException as exc: + raise SensorStreamError(f"stream stage failed: {exc}") from exc error = self._pipeline.error if error is not None: raise SensorStreamError(f"stream stage failed: {error}") from error @@ -288,17 +363,62 @@ class SensorStream: return self._pipeline.error @property - def overwritten_frames(self) -> int: - """Candidate frames replaced before they were handed to the pipeline.""" - return int(self._overwritten) + def pipeline(self) -> StreamPipeline: + return self._pipeline @property def latest_output(self) -> ForceStreamOutput | None: return self._latest_output @property - def pipeline(self) -> StreamPipeline: - return self._pipeline + def overwritten_frames(self) -> int: + """Real-time candidate frames replaced before the pipeline claimed them.""" + return int(self._overwritten) + + @property + def duplicate_inputs(self) -> int: + return int(self._duplicate_inputs) + + @property + def late_inputs(self) -> int: + return int(self._late_inputs) + + @property + def backpressure_waits(self) -> int: + """Number of producer waits caused by the bounded file/debug input queue.""" + return int(self._backpressure_waits) + + @property + def input_queue_depth(self) -> int: + return 0 if self._input_queue is None else int(self._input_queue.qsize()) + + @property + def input_queue_maxsize(self) -> int: + return self._depth + + @property + def nominal_fps(self) -> float: + return float(self._nominal_fps) + + @property + def realtime(self) -> bool: + return bool(self._realtime) + + def diagnostics(self) -> dict[str, Any]: + """Stream-level diagnostics that accompany the per-frame result rows.""" + return { + "realtime": self._realtime, + "overwritten_frames": self._overwritten, + "duplicate_inputs": self._duplicate_inputs, + "late_inputs": self._late_inputs, + "backpressure_waits": self._backpressure_waits, + "input_queue_depth": self.input_queue_depth, + "input_queue_maxsize": self._depth, + "published": self._published_outputs, + "last_source_dt_s": self._last_source_dt_s, + "last_long_gap": self._last_long_gap, + "design_sample_rate_hz": self.session.design_sample_rate_hz, + } # ------------------------------------------------------------ external input def push_frame( @@ -307,15 +427,24 @@ class SensorStream: *, source_frame_id: int | None = None, capture_time_ns: int | None = None, - time_basis: str = "capture_monotonic", + time_basis: str | None = None, clock_id: str = "host", source_pts: int | float | None = None, - ) -> None: - """Offer one already-prepared frame from a borrowed source. + media_time_ms: float | None = None, + ) -> bool: + """Offer one prepared frame from a borrowed source. Real-time candidate semantics: if the producer has not claimed the previous candidate yet, this frame replaces it and increments :attr:`overwritten_frames`; a claimed frame is never interrupted. + File/debug complete semantics: the frame enters a bounded FIFO; this call + blocks while the queue is full (observed through + :attr:`backpressure_waits`) and never drops an accepted frame. + + ``time_basis=None`` chooses ``capture_monotonic`` for real-time and + ``estimated_source`` (frame id ÷ nominal fps) for file/debug. A + ``video_pts`` frame must carry ``source_pts``; POS_MSEC is only a + diagnostic and never becomes ``source_pts``. """ if not self._external_input: raise RuntimeError( @@ -323,34 +452,131 @@ class SensorStream: ) if not isinstance(frame, np.ndarray) or frame.size == 0: raise ValueError("frame must be a non-empty numpy array") + if self._stop.is_set(): + raise SensorStreamError("stream is stopped") + if source_frame_id is None: + with self._cond: + source_frame_id = self._input_counter + source_frame_id = int(source_frame_id) + if source_frame_id < 0: + raise ValueError("source_frame_id must be non-negative") with self._cond: - if self._candidate is not None: - self._overwritten += 1 + if source_frame_id in self._seen_source_ids: + self._duplicate_inputs += 1 + return False + if ( + self._last_input_source_frame_id is not None + and source_frame_id <= self._last_input_source_frame_id + ): + self._late_inputs += 1 + return False + self._seen_source_ids[source_frame_id] = None + if len(self._seen_source_ids) > _SEEN_LIMIT: + dropped = next(iter(self._seen_source_ids)) + del self._seen_source_ids[dropped] + self._last_input_source_frame_id = source_frame_id self._input_counter += 1 - if source_frame_id is None: - source_frame_id = self._input_counter - 1 - if capture_time_ns is None: - capture_time_ns = time.perf_counter_ns() - self._candidate = _QueuedFrame( - image=frame, - source_frame_id=int(source_frame_id), - capture_time_ns=int(capture_time_ns), - time_basis=str(time_basis), - clock_id=str(clock_id), - source_pts=source_pts, + self._accepted_inputs += 1 + item = self._resolve_input_frame( + frame, + source_frame_id=source_frame_id, + capture_time_ns=capture_time_ns, + time_basis=time_basis, + clock_id=clock_id, + source_pts=source_pts, + media_time_ms=media_time_ms, + ) + if self._realtime: + with self._cond: + if self._candidate is not None: + self._overwritten += 1 + self._candidate = item + self._cond.notify_all() + return True + + assert self._input_queue is not None + while True: + try: + self._input_queue.put(item, timeout=0.05) + return True + except queue.Full: + if self._stop.is_set(): + raise SensorStreamError( + "stream stopped while waiting for backpressure" + ) + with self._cond: + self._backpressure_waits += 1 + + def end_input(self) -> None: + """Signal end of an external file/debug input sequence (complete mode).""" + if self._realtime: + return + assert self._input_queue is not None + self._input_queue.put(None) + + def _resolve_input_frame( + self, + frame: np.ndarray, + *, + source_frame_id: int, + capture_time_ns: int | None, + time_basis: str | None, + clock_id: str, + source_pts: int | float | None, + media_time_ms: float | None, + ) -> _QueuedFrame: + basis = time_basis + if basis is None: + basis = "capture_monotonic" if self._realtime else "estimated_source" + if basis not in TIME_BASES: + raise ValueError( + f"unknown time_basis {basis!r}; expected one of {TIME_BASES}" ) - self._cond.notify_all() + if basis == "capture_monotonic": + if source_pts is not None: + raise ValueError("capture_monotonic frames cannot carry source_pts") + stamp_ns = ( + time.perf_counter_ns() + if capture_time_ns is None + else int(capture_time_ns) + ) + elif basis == "recorded_signal": + if capture_time_ns is None: + raise ValueError("recorded_signal frames need capture_time_ns") + stamp_ns = int(capture_time_ns) + elif basis == "video_pts": + if source_pts is None: + raise ValueError( + "video_pts requires a provable source_pts from the container" + ) + stamp_ns = ( + int(source_pts) if capture_time_ns is None else int(capture_time_ns) + ) + else: # estimated_source + stamp_ns = ( + self._estimated_source_ns(source_frame_id) + if capture_time_ns is None + else int(capture_time_ns) + ) + return _QueuedFrame( + image=frame, + source_frame_id=source_frame_id, + capture_time_ns=stamp_ns, + time_basis=basis, + clock_id=str(clock_id), + source_pts=None if source_pts is None else source_pts, + media_time_ms=None if media_time_ms is None else float(media_time_ms), + ) + + def _estimated_source_ns(self, source_frame_id: int) -> int: + return int(round(source_frame_id / self._nominal_fps * 1e9)) # ----------------------------------------------------------------- producer def _produce(self): if self._external_input: - with self._cond: - while self._candidate is None and not self._stop.is_set(): - self._cond.wait(timeout=0.05) - if self._stop.is_set() or self._candidate is None: - return None - item = self._candidate - self._candidate = None + item = self._next_external_frame() + if item is None: + return None prepared = self.sensor.get_img( frame=item.image, already_processed=True, @@ -360,21 +586,104 @@ class SensorStream: return None return ProducedFrame(prepared, metadata=item) - # Built-in capture: the Sensor owns the source; the frame is produced by - # its normal get_img path and the capture source keeps only its latest. - self._input_counter += 1 + # Built-in capture: identity and clocks come from CapturedFrame. The + # Sensor's own capture path keeps the source's latest/next frame. image = self.sensor.get_img() + captured = getattr(self.sensor, "last_captured_frame", None) if image is None: + self._end_of_input_seen = True + self._maybe_fail_zero_after_input_end() return None + self._input_counter += 1 + with self._cond: + self._accepted_inputs += 1 + source_frame_id = ( + int(captured.sequence) + if captured is not None + else max(0, self._input_counter - 1) + ) + if self._realtime: + basis = "capture_monotonic" + stamp_ns = ( + int(captured.host_mono_ns) + if captured is not None + else time.perf_counter_ns() + ) + media_time_ms = None + else: + basis = "estimated_source" + stamp_ns = self._estimated_source_ns(source_frame_id) + media_time_ms = None if captured is None else captured.media_time_ms item = _QueuedFrame( image=image, - source_frame_id=self._input_counter - 1, - capture_time_ns=time.perf_counter_ns(), - time_basis="capture_monotonic", + source_frame_id=source_frame_id, + capture_time_ns=stamp_ns, + time_basis=basis, clock_id="host", + source_pts=None, + media_time_ms=media_time_ms, ) return ProducedFrame(image, metadata=item) + def _fail_incomplete_zero_after_input_end(self) -> None: + """Complete an in-progress reset_and_zero with a clear input-end error.""" + with self._cond: + failed = False + for command in (self._pending_zero, self._active_zero): + if command is not None and not command.done.is_set(): + command.error = SensorStreamError( + "input ended before unloaded zero completed" + ) + command.done.set() + failed = True + self._pending_zero = None + self._active_zero = None + if failed: + self._cond.notify_all() + + def _maybe_fail_zero_after_input_end(self) -> None: + """Fail an incomplete zero only after every accepted frame was processed. + + The producer sees the end marker possibly long before the flow/HHD stages + have drained the frames already accepted; those frames can still complete + the 30-frame baseline. The input-end judgment is therefore deferred until + the published output count reaches the accepted input count. + """ + with self._cond: + drained = ( + self._end_of_input_seen + and self._published_outputs >= self._accepted_inputs + ) + if drained: + self._fail_incomplete_zero_after_input_end() + + def _next_external_frame(self) -> _QueuedFrame | None: + if self._realtime: + with self._cond: + while self._candidate is None and not self._stop.is_set(): + self._cond.wait(timeout=0.05) + if self._stop.is_set() or self._candidate is None: + return None + item = self._candidate + self._candidate = None + return item + assert self._input_queue is not None + while True: + try: + item = self._input_queue.get(timeout=0.05) + except queue.Empty: + if self._stop.is_set(): + return None + continue + if item is None: + self._end_of_input = True + self._end_of_input_seen = True + self._maybe_fail_zero_after_input_end() + return None + if self._stop.is_set(): + return None + return item + # --------------------------------------------------------------- force stage def _on_snapshot(self, snapshot) -> None: """Runs on the pipeline's HHD thread: the only place touching the session.""" @@ -410,15 +719,17 @@ class SensorStream: max_gap_s=command_to_start.max_gap_s, max_window_s=command_to_start.max_window_s, ) - except BaseException as exc: # surfaced through this command only + except BaseException as exc: with self._cond: command_to_start.error = exc command_to_start.done.set() self._active_zero = None self._cond.notify_all() source_dt_s = None + long_gap = False if self._last_source_time_ns is not None: source_dt_s = (metadata.capture_time_ns - self._last_source_time_ns) / 1e9 + long_gap = source_dt_s > self._long_gap_s stamp = FrameStamp( sensor_id=self._sensor_id, session_id=self._session_id, @@ -445,13 +756,13 @@ class SensorStream: ), torque=float(snapshot.torque), flow=None, - field=None, + field=snapshot.field if "field" in self._include else None, valid_mask=None, ) try: output = self.session.process(measurement) events = self.session.drain_events() - except BaseException as exc: # make the stage error visible through error/stop + except BaseException as exc: with self._cond: if self._active_zero is not None: self._active_zero.error = exc @@ -460,20 +771,41 @@ class SensorStream: self._cond.notify_all() self._pipeline._error = exc return + extra: list[tuple[str, str | float]] = [ + ("overwritten_frames", float(self._overwritten)), + ("duplicate_inputs", float(self._duplicate_inputs)), + ("late_inputs", float(self._late_inputs)), + ("processing_latency_s", round(float(snapshot.age_ms) / 1000.0, 6)), + ("long_gap", "true" if long_gap else "false"), + ( + "source_time_estimated", + "true" if metadata.time_basis == "estimated_source" else "false", + ), + ] + if metadata.media_time_ms is not None: + extra.append(("pos_msec", float(metadata.media_time_ms))) + if source_dt_s is not None: + extra.append(("source_dt_s", float(source_dt_s))) + output = replace(output, diagnostics=tuple(output.diagnostics) + tuple(extra)) self._last_source_time_ns = metadata.capture_time_ns + self._last_source_dt_s = source_dt_s + self._last_long_gap = long_gap with self._cond: self._latest_output = output self._outputs_seen += 1 + self._published_outputs += 1 + self._history.append(output) + if len(self._history) > _HISTORY_LIMIT: + del self._history[: len(self._history) - _HISTORY_LIMIT] self._events.extend(events) command = self._active_zero if command is not None: - # Only the force thread reads session.zero; the caller waits on - # its own command object instead. command.result = self.session.zero if command.result.phase in ("ready", "failed"): command.done.set() self._active_zero = None self._cond.notify_all() + self._maybe_fail_zero_after_input_end() # ------------------------------------------------------------------ outputs def wait_output(self, timeout: float | None = None) -> ForceStreamOutput | None: @@ -491,6 +823,33 @@ class SensorStream: self._delivered = self._outputs_seen return self._latest_output + def wait_outputs( + self, count: int, timeout: float | None = None + ) -> tuple[ForceStreamOutput, ...]: + """Wait until at least ``count`` rows exist, then return all retained rows. + + File/debug complete mode uses this to check that N inputs produced N + results; real-time users normally use :meth:`wait_output` instead. + """ + want = max(0, int(count)) + deadline = None if timeout is None else time.perf_counter() + timeout + with self._cond: + while self._published_outputs < want: + remaining = None + if deadline is not None: + remaining = deadline - time.perf_counter() + if remaining <= 0: + return self.drain_outputs() + self._cond.wait(timeout=remaining) + return self.drain_outputs() + + def drain_outputs(self) -> tuple[ForceStreamOutput, ...]: + """All output rows since the previous drain (in publication order).""" + with self._cond: + rows = tuple(self._history) + self._history.clear() + return rows + def drain_events(self) -> tuple[StreamEvent, ...]: """Session events emitted by the force thread since the previous drain.""" with self._cond: @@ -516,6 +875,12 @@ class SensorStream: thread writes that command's ZeroProgress/error and signals it, so a second call can never observe the previous zero's state. The caller thread never reads ``session.zero``. + + If the call times out, the command is **still in progress** in the force + thread: the zero can still become ready later and another + ``reset_and_zero`` is rejected until that command finishes. Callers that + time out should stop the stream (or keep waiting via the stream's zero + state on the force side) rather than assuming the zero was cancelled. """ if not self._pipeline.running: raise SensorStreamError("start the stream before reset_and_zero()") @@ -531,13 +896,14 @@ class SensorStream: target_reference_generation=target, ) self._pending_zero = command + self._maybe_fail_zero_after_input_end() if not command.done.wait(float(timeout)): with self._cond: if self._pending_zero is command: self._pending_zero = None raise TimeoutError( f"reset_and_zero({reference_id!r}) did not finish within {timeout}s " - f"(target={target})" + f"(target={target}); the command is still in progress in the force thread" ) if command.error is not None: raise SensorStreamError(str(command.error)) from command.error diff --git a/tests/test_sensor_stream.py b/tests/test_sensor_stream.py index 73c66a4..ecfa067 100644 --- a/tests/test_sensor_stream.py +++ b/tests/test_sensor_stream.py @@ -15,8 +15,13 @@ import time import numpy as np import pytest +from orisys.capture import CapturedFrame +from orisys.force_stream_session import ForceStreamSession +from orisys.image_pipeline import ImageMeasurement +from orisys.pipeline import StreamPipeline from orisys.sensor import Sensor from orisys.sensor_stream import SensorStream, SensorStreamError +from orisys.stream_contract import FrameStamp, MeasurementFrame SIZE = 365 @@ -281,3 +286,727 @@ def test_failed_start_restores_the_borrowed_sensor_session(monkeypatch) -> None: with pytest.raises(RuntimeError, match="pipeline start failed"): stream.start() assert sensor._measurement_session is False, "a failed start must restore the mode" + + +# ------------------------------------------------- T08b: complete file/debug mode +def test_file_mode_processes_every_input_in_source_order() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=2, nominal_fps=30.0) + stream.start() + try: + for index in range(8): + assert stream.push_frame( + _frame(40.0 + index), + source_frame_id=index, + capture_time_ns=1_000_000_000 + index * 33_000_000, + ) + stream.end_input() + rows = stream.wait_outputs(8, timeout=10.0) + assert len(rows) == 8, "complete mode must not drop frames" + assert [row.frame.stamp.source_frame_id for row in rows] == list(range(8)) + assert [row.frame.stamp.frame_id for row in rows] == list(range(8)) + assert stream.overwritten_frames == 0 + finally: + stream.stop() + + +def test_file_mode_backpressure_is_bounded_and_observable() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=2, nominal_fps=30.0) + entered = threading.Event() + release = threading.Event() + original = stream.pipeline._on_frame + + def gated(snapshot) -> None: + entered.set() + assert release.wait(timeout=5.0) + original(snapshot) + + stream.pipeline._on_frame = gated + stream.start() + pushed: list[int] = [] + + def pusher() -> None: + for index in range(30): + stream.push_frame( + _frame(40.0 + index), + source_frame_id=index, + capture_time_ns=1_000_000_000 + index * 33_000_000, + ) + pushed.append(index) + + worker = threading.Thread(target=pusher) + worker.start() + try: + assert entered.wait(timeout=5.0), "the HHD stage never reached the gate" + time.sleep(0.3) + assert stream.input_queue_maxsize == 2 + assert stream.input_queue_depth <= 2 + assert stream.backpressure_waits > 0, ( + "the producer must wait on the bounded queue" + ) + assert worker.is_alive(), "a full bounded queue must make the producer wait" + release.set() + worker.join(timeout=10.0) + assert not worker.is_alive() + assert len(pushed) == 30 + stream.end_input() + rows = stream.wait_outputs(30, timeout=15.0) + assert len(rows) == 30 + assert [row.frame.stamp.source_frame_id for row in rows] == list(range(30)) + finally: + release.set() + stream.stop() + + +# ------------------------------------------------------- T08b: time provenance +def test_time_bases_fake_sources_and_same_pts_frames() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=10.0) + stream.start() + try: + frames = [ + dict( + source_pts=123456, capture_time_ns=1_000_000_000, time_basis="video_pts" + ), + dict( + source_pts=123456, capture_time_ns=1_000_000_000, time_basis="video_pts" + ), + dict(capture_time_ns=1_100_000_000, time_basis="recorded_signal"), + dict(time_basis="estimated_source", media_time_ms=12.5), + dict(time_basis=None), + ] + source_ids = [0, 1, 2, 12, 13] + for index, (source_id, timing) in enumerate(zip(source_ids, frames)): + stream.push_frame(_frame(40.0 + index), source_frame_id=source_id, **timing) + stream.end_input() + rows = stream.wait_outputs(len(frames), timeout=10.0) + assert len(rows) == len(frames) + stamps = [row.frame.stamp for row in rows] + assert [stamp.capture_time_basis for stamp in stamps] == [ + "video_pts", + "video_pts", + "recorded_signal", + "estimated_source", + "estimated_source", + ] + # Same PTS, different source frames: two frames, not a duplicate. + assert stamps[0].source_pts == 123456 + assert stamps[1].source_pts == 123456 + assert stamps[0].source_frame_id != stamps[1].source_frame_id + # Estimated source time is frame id / nominal fps; POS_MSEC is diagnostics only. + assert stamps[3].capture_time_ns == round(12 / 10.0 * 1e9) + assert stamps[4].capture_time_ns == round(13 / 10.0 * 1e9) + diag3 = dict(rows[3].diagnostics) + diag4 = dict(rows[4].diagnostics) + assert diag3["pos_msec"] == 12.5 + assert "pos_msec" not in diag4 + assert diag3["source_time_estimated"] == "true" + assert dict(rows[0].diagnostics)["source_time_estimated"] == "false" + finally: + stream.stop() + + +def _fake_builtin_sensor(frames, *, media_times=None): + sensor = _sensor() + original = sensor.get_img + index = {"next": 0} + captures = [] + + def fake_get_img( + frame=None, already_processed=False, valid_mask=None, already_gray=False + ): + if index["next"] >= len(frames): + return None + position = index["next"] + index["next"] += 1 + captured = CapturedFrame( + frames[position], + position + 1, + 1_000_000_000 + position * 33_000_000, + 2_000_000_000 + position * 33_000_000, + None if media_times is None else media_times[position], + ) + captures.append(captured) + sensor._last_captured_frame = captured + return original( + frame=frames[position], already_processed=True, already_gray=True + ) + + sensor.get_img = fake_get_img + return sensor, captures + + +def test_builtin_capture_uses_captured_frame_host_clocks_and_sequence() -> None: + frames = [_frame(40.0 + index) for index in range(5)] + sensor, captures = _fake_builtin_sensor(frames) + stream = _stream(sensor, realtime=True, external_input=False) + stream.start() + try: + rows = stream.wait_outputs(5, timeout=10.0) + assert len(rows) == 5 + for row, captured in zip(rows, captures): + stamp = row.frame.stamp + assert stamp.capture_time_basis == "capture_monotonic" + assert stamp.capture_time_ns == captured.host_mono_ns + assert stamp.source_frame_id == captured.sequence + finally: + stream.stop() + + +def test_builtin_file_mode_estimates_time_and_reports_pos_msec() -> None: + frames = [_frame(40.0 + index) for index in range(5)] + media = [0.0, 33.0, 66.0, 99.0, 132.0] + sensor, captures = _fake_builtin_sensor(frames, media_times=media) + stream = _stream( + sensor, + realtime=False, + nominal_fps=30.0, + external_input=False, + ) + stream.start() + try: + rows = stream.wait_outputs(5, timeout=10.0) + assert len(rows) == 5 + for row, captured in zip(rows, captures): + stamp = row.frame.stamp + assert stamp.capture_time_basis == "estimated_source" + assert stamp.source_pts is None + assert stamp.source_frame_id == captured.sequence + assert stamp.capture_time_ns == round(captured.sequence / 30.0 * 1e9) + assert dict(row.diagnostics)["pos_msec"] == captured.media_time_ms + finally: + stream.stop() + + +# ------------------------------------------------- T08b: optional field include +def _field_equal(left, right) -> None: + assert left is not None and right is not None + assert set(left) == set(right) + for key in left: + a = left[key] + b = right[key] + if isinstance(a, np.ndarray) or isinstance(b, np.ndarray): + assert np.array_equal(np.asarray(a), np.asarray(b), equal_nan=True), key + else: + assert a == b, key + + +def _run_pipeline_capture(include): + sensor = _sensor() + frames = [ + np.tile((np.arange(SIZE, dtype=np.uint8) + index * 3)[None, :], (SIZE, 1)) + for index in range(6) + ] + index = {"next": 0} + records = [] + + def produce(): + if index["next"] >= len(frames): + return None + position = index["next"] + index["next"] += 1 + return sensor.get_img( + frame=frames[position], already_processed=True, already_gray=True + ) + + def on_frame(snapshot) -> None: + expected = sensor.current_field() + field_copy = snapshot.field + records.append((snapshot, field_copy, expected)) + + pipeline = StreamPipeline(sensor, produce, include=include, on_frame=on_frame) + pipeline.start() + deadline = time.perf_counter() + 10.0 + while len(records) < len(frames) and time.perf_counter() < deadline: + time.sleep(0.01) + pipeline.stop(timeout=5.0) + return records + + +def test_pipeline_field_include_is_opt_in_and_same_frame() -> None: + default_records = _run_pipeline_capture(()) + assert len(default_records) == 6 + assert all(snapshot.field is None for snapshot, _, _ in default_records) + + field_records = _run_pipeline_capture(("field",)) + assert len(field_records) == 6 + first_copy = None + for snapshot, recorded_field, expected in field_records: + assert snapshot.field is not None + _field_equal(snapshot.field, expected) + if first_copy is None: + first_copy = { + key: value.copy() if isinstance(value, np.ndarray) else value + for key, value in snapshot.field.items() + } + # Later frames must not rewrite the field payload published with frame 0. + _field_equal(field_records[0][1], first_copy) + + +# ------------------------------------------------- T08b: end-to-end and in-flight +def _feed_complete(stream, frames, *, start=0, sleep_s=0.002, stop=None): + def feeder() -> None: + for source_id in range(start, len(frames)): + if stop is not None and stop.is_set(): + return + stream.push_frame( + frames[source_id], + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_333_333, + ) + time.sleep(sleep_s) + + thread = threading.Thread(target=feeder) + thread.start() + return thread + + +def _wait_until(predicate, timeout: float = 20.0) -> None: + """Wait for a state condition; never used to tune frame ordering by delay.""" + deadline = time.perf_counter() + timeout + while time.perf_counter() < deadline: + if predicate(): + return + time.sleep(0.005) + raise AssertionError("condition was not reached before the timeout") + + +def test_end_to_end_matches_manual_sensor_image_pipeline_session() -> None: + count = 90 + initial = 10 + frames = [ + np.tile( + ((np.arange(SIZE, dtype=np.uint16) + index * 3) % 256).astype(np.uint8)[ + None, : + ], + (SIZE, 1), + ) + for index in range(count) + ] + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + stop = threading.Event() + pressed_initial = threading.Event() + + def feeder() -> None: + for source_id in range(count): + if stop.is_set(): + return + stream.push_frame( + frames[source_id], + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_333_333, + ) + if source_id + 1 == initial: + pressed_initial.set() + pressed_initial.set() + + feeder_thread = threading.Thread(target=feeder) + feeder_thread.start() + try: + assert pressed_initial.wait(timeout=20.0) + # The reset is requested only after every initial frame was published, so + # the number of new-version frames left for the baseline is deterministic. + _wait_until(lambda: stream.diagnostics()["published"] >= initial) + zero = stream.reset_and_zero(reference_id="eq", timeout=20.0) + assert zero.phase == "ready" + feeder_thread.join(timeout=20.0) + assert not feeder_thread.is_alive() + _wait_until(lambda: stream.diagnostics()["published"] >= count) + rows = stream.drain_outputs() + finally: + stop.set() + stream.stop() + assert len(rows) == count + + first_version = next( + row.reference_generation for row in rows if row.reference_generation > 0 + ) + first_source_id = next( + row.frame.stamp.source_frame_id + for row in rows + if row.reference_generation == first_version + ) + + manual = _sensor() + session = ForceStreamSession( + sensor_family="box", + calibration=manual.calibration, + device_id=None, + ) + session.request_zero( + reference_id="eq", + declared_unloaded=True, + expected_reference_generation=first_version, + ) + manual_outputs = {} + for source_id in range(first_source_id, count): + manual.get_img( + frame=frames[source_id], already_processed=True, already_gray=True + ) + manual.compute_flow() + manual.compute_hhd() + stamp = FrameStamp( + sensor_id="BOX-1", + session_id="run-1", + frame_id=source_id, + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_333_333, + clock_id="host", + capture_time_basis="capture_monotonic", + ) + measurement = MeasurementFrame( + stamp=stamp, source_dt_s=None, reference_generation=first_version + ) + manual_outputs[source_id] = session.process( + ImageMeasurement( + frame=measurement, + force_raw=( + float(manual._force_raw[0]), + float(manual._force_raw[1]), + float(manual._force_raw[2]), + ), + torque=float(manual.ftor), + flow=None, + ) + ) + compared = 0 + for row in rows: + if row.reference_generation != first_version or row.zero.phase != "ready": + continue + expected = manual_outputs.get(row.frame.stamp.source_frame_id) + if expected is None: + continue + compared += 1 + assert np.array_equal( + np.asarray(row.raw), np.asarray(expected.raw), equal_nan=True + ) + assert np.array_equal( + np.asarray(row.newton), np.asarray(expected.newton), equal_nan=True + ) + assert row.raw_status == expected.raw_status + assert row.newton_status == expected.newton_status + assert compared >= 5, "facade and manual chain were not compared frame by frame" + + +def test_inflight_old_version_frames_do_not_enter_the_new_baseline() -> None: + initial = 5 + count = 50 + frames = [ + np.tile((np.arange(SIZE, dtype=np.uint8) + index * 2)[None, :], (SIZE, 1)) + for index in range(count) + ] + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + stop = threading.Event() + pressed_initial = threading.Event() + + def feeder() -> None: + for source_id in range(count): + if stop.is_set(): + return + stream.push_frame( + frames[source_id], + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_000_000, + ) + if source_id + 1 == initial: + pressed_initial.set() + + feeder_thread = threading.Thread(target=feeder) + feeder_thread.start() + try: + assert pressed_initial.wait(timeout=20.0) + _wait_until(lambda: stream.diagnostics()["published"] >= initial) + zero = stream.reset_and_zero(reference_id="v1", timeout=20.0) + feeder_thread.join(timeout=20.0) + _wait_until(lambda: stream.diagnostics()["published"] >= count) + rows = stream.drain_outputs() + finally: + stop.set() + stream.stop() + assert zero.phase == "ready" + assert zero.collected_frames == 30 + old_rows = [row for row in rows if row.reference_generation == 0] + version_one = [row for row in rows if row.reference_generation == 1] + assert old_rows, "the test needs in-flight old-version frames" + assert version_one, "no new-version frame was processed" + assert all(row.zero.phase != "ready" for row in old_rows) + assert len(version_one) >= 31 + assert all(row.zero.phase != "ready" for row in version_one[:30]) + assert version_one[30].zero.phase == "ready" + + +def test_stage_error_is_visible_to_the_caller(monkeypatch) -> None: + sensor = _sensor() + stream = _stream(sensor) + stream.start() + try: + + def fail(_measurement): + raise RuntimeError("force stage failure") + + monkeypatch.setattr(stream.session, "process", fail) + stream.push_frame(_frame(), source_frame_id=0) + time.sleep(0.2) + with pytest.raises(SensorStreamError, match="force stage failure"): + stream.stop() + finally: + if stream.running: + stream.stop() + + +# --------------------------------------------- T08b: duplicate/late and diagnostics +def test_duplicate_and_late_inputs_are_counted_not_fed() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + try: + assert stream.push_frame( + _frame(40.0), source_frame_id=0, capture_time_ns=1_000_000_000 + ) + assert not stream.push_frame( + _frame(41.0), source_frame_id=0, capture_time_ns=1_100_000_000 + ) + assert stream.push_frame( + _frame(42.0), source_frame_id=5, capture_time_ns=1_200_000_000 + ) + assert not stream.push_frame( + _frame(43.0), source_frame_id=3, capture_time_ns=1_300_000_000 + ) + assert stream.duplicate_inputs == 1 + assert stream.late_inputs == 1 + stream.end_input() + rows = stream.wait_outputs(2, timeout=10.0) + assert [row.frame.stamp.source_frame_id for row in rows] == [0, 5] + finally: + stream.stop() + + +def test_source_dt_and_long_gap_diagnostics() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0, long_gap_s=0.5) + stream.start() + try: + times = [1_000_000_000, 2_000_000_000, 2_033_000_000] + for index, when in enumerate(times): + stream.push_frame( + _frame(40.0 + index), source_frame_id=index, capture_time_ns=when + ) + stream.end_input() + rows = stream.wait_outputs(3, timeout=10.0) + assert dict(rows[0].diagnostics)["long_gap"] == "false" + assert dict(rows[1].diagnostics)["long_gap"] == "true" + assert dict(rows[1].diagnostics)["source_dt_s"] == 1.0 + assert dict(rows[2].diagnostics)["long_gap"] == "false" + assert dict(rows[2].diagnostics)["source_dt_s"] == 0.033 + assert stream.diagnostics()["last_long_gap"] is False + finally: + stream.stop() + + +def test_duplicate_after_ready_does_not_invalidate_the_zero() -> None: + initial = 40 + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + stop = threading.Event() + pressed_initial = threading.Event() + + def frame(source_id: int) -> np.ndarray: + return np.tile( + (np.arange(SIZE, dtype=np.uint8) + source_id * 2)[None, :], (SIZE, 1) + ) + + def feeder() -> None: + source_id = 0 + while not stop.is_set(): + stream.push_frame( + frame(source_id), + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_000_000, + ) + source_id += 1 + if source_id >= initial: + pressed_initial.set() + + feeder_thread = threading.Thread(target=feeder) + feeder_thread.start() + try: + assert pressed_initial.wait(timeout=20.0) + # Every initial source id is published before the reset request, exactly + # the condition the reviewer's probe used to force the old flake. + _wait_until(lambda: stream.diagnostics()["published"] >= initial) + zero = stream.reset_and_zero(reference_id="dup-zero", timeout=20.0) + stop.set() + feeder_thread.join(timeout=10.0) + assert not feeder_thread.is_alive() + assert ( + stream.push_frame( + frame(0), source_frame_id=0, capture_time_ns=1_000_000_000 + ) + is False + ) + assert stream.duplicate_inputs >= 1 + assert stream.session.zero.phase == "ready" + assert stream.session.zero.reference_id == zero.reference_id + finally: + stop.set() + stream.stop() + + +def test_input_end_fails_an_incomplete_reset_and_zero() -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + for source_id in range(5): + stream.push_frame( + _frame(40.0 + source_id), + source_frame_id=source_id, + capture_time_ns=1_000_000_000 + source_id * 33_000_000, + ) + _wait_until(lambda: stream.diagnostics()["published"] >= 5) + outcome: dict[str, BaseException] = {} + + def run_zero() -> None: + try: + stream.reset_and_zero(reference_id="short-input", timeout=10.0) + except BaseException as exc: # asserted below + outcome["error"] = exc + + worker = threading.Thread(target=run_zero) + worker.start() + try: + _wait_until( + lambda: stream._pending_zero is not None or stream._active_zero is not None + ) + stream.end_input() + worker.join(timeout=5.0) + assert not worker.is_alive(), "input end must complete the zero command" + error = outcome.get("error") + assert isinstance(error, SensorStreamError) + assert "input ended" in str(error) + finally: + if worker.is_alive(): + stream.stop() + else: + stream.stop() + + +@pytest.mark.parametrize("extra", [31, 33, 36]) +def test_input_end_after_enough_frames_still_completes_zero(extra: int) -> None: + sensor = _sensor() + stream = _stream(sensor, realtime=False, depth=4, nominal_fps=30.0) + stream.start() + + def frame(index: int) -> np.ndarray: + return np.tile( + (np.arange(SIZE, dtype=np.uint8) + index * 2)[None, :], (SIZE, 1) + ) + + for index in range(5): + stream.push_frame( + frame(index), + source_frame_id=index, + capture_time_ns=1_000_000_000 + index * 33_000_000, + ) + _wait_until(lambda: stream.diagnostics()["published"] >= 5) + outcome: dict[str, object] = {} + + def run_zero() -> None: + try: + outcome["progress"] = stream.reset_and_zero( + reference_id="tight", timeout=20.0 + ) + except BaseException as exc: # asserted below + outcome["error"] = exc + + worker = threading.Thread(target=run_zero) + worker.start() + try: + _wait_until( + lambda: stream._pending_zero is not None or stream._active_zero is not None + ) + for index in range(5, 5 + extra): + stream.push_frame( + frame(index), + source_frame_id=index, + capture_time_ns=1_000_000_000 + index * 33_000_000, + ) + stream.end_input() + worker.join(timeout=25.0) + assert not worker.is_alive() + assert "error" not in outcome, outcome.get("error") + progress = outcome.get("progress") + assert getattr(progress, "phase", None) == "ready" + assert stream.session.zero.phase == "ready" + assert stream.session.zero.reference_id == "tight" + finally: + if worker.is_alive(): + stream.end_input() + stream.stop() + + +def test_builtin_input_end_after_enough_frames_completes_zero() -> None: + prepared = [ + np.tile((np.arange(SIZE, dtype=np.uint8) + index * 2)[None, :], (SIZE, 1)) + for index in range(5 + 31) + ] + sensor = _sensor() + original = sensor.get_img + state = {"next": 0} + gate = threading.Event() + + def fake_get_img( + frame=None, already_processed=False, valid_mask=None, already_gray=False + ): + position = state["next"] + if position == 5: + assert gate.wait(timeout=20.0) + if position >= len(prepared): + return None + state["next"] += 1 + captured = CapturedFrame( + prepared[position], + position + 1, + 1_000_000_000 + position * 33_000_000, + 2_000_000_000 + position * 33_000_000, + None, + ) + sensor._last_captured_frame = captured + return original( + frame=prepared[position], already_processed=True, already_gray=True + ) + + sensor.get_img = fake_get_img + stream = _stream(sensor, realtime=False, nominal_fps=30.0, external_input=False) + stream.start() + outcome: dict[str, object] = {} + + def run_zero() -> None: + try: + outcome["progress"] = stream.reset_and_zero( + reference_id="builtin-tight", timeout=20.0 + ) + except BaseException as exc: # asserted below + outcome["error"] = exc + + try: + _wait_until(lambda: stream.diagnostics()["published"] >= 5) + worker = threading.Thread(target=run_zero) + worker.start() + _wait_until( + lambda: stream._pending_zero is not None or stream._active_zero is not None + ) + gate.set() + worker.join(timeout=20.0) + assert not worker.is_alive() + assert "error" not in outcome, outcome.get("error") + assert getattr(outcome.get("progress"), "phase", None) == "ready" + assert stream.session.zero.phase == "ready" + finally: + gate.set() + stream.stop()