feat: SensorStream 支持文件完整模式、源时间与 field 输出

T08b 补齐文件/debug 完整处理与背压、源帧号和时间基、同帧 field 快照,输入结束时未完成归零明确报错。独立验收见 docs/workflow/sdk-sensorstream-implementation-plan.zh.md 第 44 轮。

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
ykcheungab
2026-10-21 20:52:11 +08:00
parent d31503330d
commit f5738375fd
5 changed files with 1234 additions and 103 deletions
+27 -15
View File
@@ -1,4 +1,4 @@
# 新力侧会话与单路入口(T06a/T06b、T07 输出滤波、T08a) # 新力侧会话与单路入口(T06a/T06b、T07 输出滤波、T08a/T08b)
本文件是单路 SensorStream 计划 T06a/T06b/T07 的 SDK 合同:新的力侧会话直接消费 T05 的 本文件是单路 SensorStream 计划 T06a/T06b/T07 的 SDK 合同:新的力侧会话直接消费 T05 的
`ImageMeasurement`(T03 `MeasurementFrame` 头 + 同帧 raw/field),参考版本取帧上的 `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 assert session.design_sample_rate_hz == 30.0
``` ```
## SensorStream 入口(T08a) ## SensorStream 入口(T08a/T08b)
`SensorStream` 把 T04–T07 串成一个运行中的单路入口: `SensorStream` 把 T04–T07 串成一个运行中的单路入口:
@@ -81,23 +81,35 @@ assert session.design_sample_rate_hz == 30.0
已打开的 Sensor 或外部喂帧时只借用,`stop()` 不关闭借用源。运行中只有一个采集所有者; 已打开的 Sensor 或外部喂帧时只借用,`stop()` 不关闭借用源。运行中只有一个采集所有者;
流在 `start()` 启用 measurement session,`stop()`/启动失败时恢复借用 Sensor 原来的模式 流在 `start()` 启用 measurement session,`stop()`/启动失败时恢复借用 Sensor 原来的模式
(原本已启用的保持启用)。 (原本已启用的保持启用)。
- **实时 latest。** `push_frame(...)` 的未领取候选帧会被最新输入覆盖并计入 - **实时 latest(T08a)。** `push_frame(...)` 的未领取候选帧会被最新输入覆盖并计入
`overwritten_frames`;已经交给流水线的帧照常完成。`wait_output()` 是 latest-wins 结果流, `overwritten_frames`;已经交给流水线的帧照常完成。`wait_output()` 是 latest-wins 结果流,
`drain_events()` 取会话事件。 `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 - **线程/版本。** 复用 `StreamPipeline` 的线程与原子 `request_reset()` 版本;力侧只在 HHD
线程的 `on_frame` 中调用 `ForceStreamSession`,因此 T07 的 `drain_events` 线程约束由门面满足, 线程的 `on_frame` 中调用 `ForceStreamSession`,因此 T07 的 `drain_events` 线程约束由门面满足,
没有第二套版本或复位记账。 没有第二套版本或复位记账。
- **组合归零。** `reset_and_zero(reference_id=...)` 执行“图像复位确认 → 同版本 30 帧无载基线 → - **组合归零。** `reset_and_zero(reference_id=...)` 执行“图像复位确认 → 同版本 30 帧无载基线 →
力 ready”;用户不管理版本号。在途旧版本帧不会进入新基线(T06a 的规则)。每次调用只等待 力 ready”;用户不管理版本号。在途旧版本帧不会进入新基线(T06a 的规则)。每次调用只等待
本条命令的结果:力线程把该命令的 `ZeroProgress`/错误写回命令对象并通知,调用方线程不读取 本条命令的结果:力线程把该命令的 `ZeroProgress`/错误写回命令对象并通知,调用方线程不读取
`session.zero`,因此连续两次调用时第二次不会复用第一次的零点。 `session.zero`,因此连续两次调用时第二次不会复用第一次的零点。**超时后命令仍在力线程进行**
- **T08a 边界。** 实时模式、无标定/标量会话、逐帧结果最新视图已实现;field/contact 的 (仍可能稍后 ready),调用方不应假设它被取消;在下一条命令完成前再次调用会被拒绝。
`include`、文件/debug 完整逐帧、有界背压、时间来源与 PTS 标注、`source_dt_s`/长间隔诊断和 - **时间来源(T08b)。** 实时相机 `capture_monotonic`,内置采集用 `CapturedFrame.host_mono_ns`
完整生命周期诊断属于 **T08b**。 和 `sequence`;Orisys 自己录像(文件内保存信号时间)`recorded_signal`;外部调用方提供可证明
- **时间基准(T08b 实现约定)。** 实时相机 `capture_monotonic`;Orisys 自己录像(文件内保存 的容器 PTS 才 `video_pts`(`source_pts` 原值保留);内置 OpenCV 普通视频用
信号时间)`recorded_signal`;外部调用方提供可证明的容器 PTS 才 `video_pts`;内置 OpenCV `estimated_source`,`capture_time_ns = source_frame_id ÷ nominal_fps`,`CAP_PROP_POS_MSEC`
读取普通视频文件用 `estimated_source`,`capture_time_ns` 按 `source_frame_id ÷ 名义帧率` 推算, 只作为 `diagnostics` 的 `pos_msec`,不填 `source_pts`。没有时间的输入按帧号推算并标
POS_MSEC 只进 diagnostics。T08a 的外部帧默认 `capture_monotonic`。 `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 ```python
import numpy as np import numpy as np
@@ -144,8 +156,8 @@ assert row.zero.phase in ("resetting", "collecting")
## 后续 ## 后续
T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价;T07 已交付固定 T06b 已交付标量/field/FN-only 三类标定与旧 ForceSession 的完整数值等价;T07 已交付固定
设计帧率、显式切换事件与历史/直通合同;T08a 已交付 SensorStream 实时入口、所有权与组合归零。 设计帧率、显式切换事件与历史/直通合同;T08a 已交付 SensorStream 实时入口、所有权与组合归零;
T08b 已交付文件/debug 完整逐帧、有界背压、时间来源/PTS 标注、`include="field"` 与诊断。
spatial_gain/接触映射模型不在 T06/T07 验收范围,需要时另立任务。 spatial_gain/接触映射模型不在 T06/T07 验收范围,需要时另立任务。
T08b 补齐文件/debug 完整逐帧、有界背压、时间来源/PTS 标注与完整诊断后再进入 T09/T10(App 迁移)。 下一步进入 T09/T10(App 迁移);删除旧 `ForceSession` 前先把零点、标定、读数引擎从旧类中
另一项后续任务:删除旧 `ForceSession` 之前,先把零点、标定、读数引擎从旧类中抽出来, 抽出来,避免新会话依赖待删除的类型。
避免新会话依赖待删除的类型(T09 之前)。
+9 -1
View File
@@ -84,7 +84,7 @@ __all__ = [
#: Array fields a snapshot can carry. ``image`` is the working grey frame; the #: Array fields a snapshot can carry. ``image`` is the working grey frame; the
#: rest come from ``Sensor.read_info`` (already copied by the SDK). #: 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 #: 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. #: 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 #: reset is queued, not synchronous). Additive field: producers that do not
#: use it leave it at 0. #: use it leave it at 0.
tracker_reset_seq: int = 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 @dataclass
@@ -759,6 +764,9 @@ class StreamPipeline:
# This frame's own measurement version, carried through the flow queue: # This frame's own measurement version, carried through the flow queue:
# never the live counter, which a later frame may already have bumped. # never the live counter, which a later frame may already have bumped.
tracker_reset_seq=int(reference_version), 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: def _fps(self) -> float:
+18 -2
View File
@@ -36,6 +36,7 @@ from .Stitcher import Stitcher
from .capture import ( from .capture import (
CaptureConfig, CaptureConfig,
CaptureSource, CaptureSource,
CapturedFrame,
ReconnectPolicy, ReconnectPolicy,
is_live_camera, is_live_camera,
normalize_source, normalize_source,
@@ -274,6 +275,10 @@ class Sensor:
self.last_log_time = time.perf_counter() self.last_log_time = time.perf_counter()
self._fps = 0 self._fps = 0
self._last_capture_time = 0.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_processing_time = 0.0
self._last_stage_times = { self._last_stage_times = {
# Frame-post-processing roll-ups (seconds). # Frame-post-processing roll-ups (seconds).
@@ -2054,9 +2059,20 @@ class Sensor:
# return self.cap is not None and self.cap.isOpened() # return self.cap is not None and self.cap.isOpened()
def _read_frame(self): def _read_frame(self):
captured: CapturedFrame | None = None
if self._capture_source is not None and self._capture_source.running: if self._capture_source is not None and self._capture_source.running:
return self._capture_source.read_latest(copy=True, wait_s=1.0) captured = self._capture_source.read_latest_stamped(copy=True, wait_s=1.0)
return self._read_frame_raw() 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): def _read_frame_raw(self):
if self._capture_source is not None: if self._capture_source is not None:
+451 -85
View File
@@ -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`` * one capture owner per running stream: either it creates the ``Sensor``
(:meth:`SensorStream.open`, closes it on stop) or the caller lends an opened (: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); caller's and stays open);
* ``StreamPipeline`` remains the threading/reset engine (bounded producer queue, * ``StreamPipeline`` remains the threading/reset engine (bounded queues, atomic
atomic ``request_reset`` version, flow/hhd ownership), so no second version or ``request_reset`` version, flow/hhd ownership), so no second version or reset
reset bookkeeping is introduced; bookkeeping is introduced;
* the HHD stage's ``on_frame`` callback is the single force-side thread. It * 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 ``ForceStreamSession.process`` exactly once per published snapshot. The
facade also applies queued ``reset_and_zero`` / output-filter commands there, facade also applies queued ``reset_and_zero`` commands there, so the force
so the force session is never touched from a consumer thread; session is never touched from a consumer thread;
* real-time mode keeps only the newest candidate input (a new push overwrites an * **real-time** mode keeps only the newest candidate input (a new push overwrites
unclaimed candidate and counts the overwrite); a frame already handed to the an unclaimed candidate and counts the overwrite); a frame already handed to
pipeline finishes normally; 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`` * ``reset_and_zero`` combines the image reset (via ``StreamPipeline.request_reset``
atomic version) with the unloaded-zero request (via the force thread), and 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 ``include=("field",)`` asks the pipeline to attach the same-frame HHD field
combined zero. File/debug complete processing, bounded backpressure, source-time payload; the facade adds it automatically when the calibration model needs it.
provenance (real PTS vs estimated source time) and the full lifecycle diagnostics The field is copied in the HHD stage's ``_build`` (never read from a consumer
are the T08b scope. Field/contact calibration inputs are not wired yet either thread), and the default snapshot path is unchanged.
(``include="field"`` is T08b); the current facade covers uncalibrated and scalar
sessions.
One instance belongs to one running stream; one thread owns the force session. One instance belongs to one running stream; one thread owns the force session.
""" """
from __future__ import annotations from __future__ import annotations
import queue
import threading import threading
import time import time
from collections.abc import Mapping from collections.abc import Iterable, Mapping
from dataclasses import dataclass, field from dataclasses import dataclass, field, replace
from typing import Any from typing import Any
import numpy as np import numpy as np
@@ -46,12 +61,18 @@ from .force_session import ZeroProgress
from .force_stream_session import ForceStreamOutput, ForceStreamSession from .force_stream_session import ForceStreamOutput, ForceStreamSession
from .pipeline import ProducedFrame, StreamPipeline from .pipeline import ProducedFrame, StreamPipeline
from .sensor import Sensor from .sensor import Sensor
from .stream_contract import FrameStamp, MeasurementFrame, StreamEvent from .stream_contract import TIME_BASES, FrameStamp, MeasurementFrame, StreamEvent
__all__ = ["SensorStream", "SensorStreamError"] __all__ = ["SensorStream", "SensorStreamError"]
#: How long reset_and_zero waits for the force session to report ready. #: How long reset_and_zero waits for the force session to report ready.
DEFAULT_ZERO_TIMEOUT_S = 30.0 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): class SensorStreamError(RuntimeError):
@@ -66,6 +87,7 @@ class _QueuedFrame:
time_basis: str time_basis: str
clock_id: str clock_id: str
source_pts: int | float | None = None source_pts: int | float | None = None
media_time_ms: float | None = None
@dataclass @dataclass
@@ -96,22 +118,27 @@ class SensorStream:
session_id: str = "stream", session_id: str = "stream",
realtime: bool = True, realtime: bool = True,
depth: int = 1, depth: int = 1,
nominal_fps: float | None = None,
include: Iterable[str] = (),
long_gap_s: float = DEFAULT_LONG_GAP_S,
owns_sensor: bool = False, owns_sensor: bool = False,
verbose: bool = False, verbose: bool = False,
external_input: bool | None = None,
) -> None: ) -> None:
if not realtime:
raise NotImplementedError(
"file/debug complete processing is the T08b scope"
)
self.sensor = sensor self.sensor = sensor
self._owns_sensor = bool(owns_sensor) self._owns_sensor = bool(owns_sensor)
self._verbose = bool(verbose) self._verbose = bool(verbose)
self._external_input = bool(getattr(sensor, "_external_only", False)) self._external_input = (
if not self._external_input: bool(getattr(sensor, "_external_only", False))
# Built-in capture owns the candidate inside the Sensor's capture if external_input is None
# source; external prepared frames arrive through push_frame(). else bool(external_input)
self._external_input = False )
self._realtime = bool(realtime)
self._session_id = str(session_id) 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 = ( self._sensor_id = (
str(sensor_id) str(sensor_id)
if sensor_id is not None if sensor_id is not None
@@ -134,12 +161,21 @@ class SensorStream:
settings=settings, settings=settings,
design_sample_rate_hz=design_sample_rate_hz, 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) enable = getattr(sensor, "enable_measurement_session", None)
self._has_measurement_session = callable(enable) and hasattr( self._has_measurement_session = callable(enable) and hasattr(
sensor, "disable_measurement_session" 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( self._sensor_session_was_enabled = bool(
getattr( getattr(
sensor, sensor,
@@ -150,21 +186,36 @@ class SensorStream:
self._sensor_session_enabled_by_stream = False self._sensor_session_enabled_by_stream = False
self._cond = threading.Condition() self._cond = threading.Condition()
self._candidate: _QueuedFrame | None = None 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._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._input_counter = 0
self._accepted_inputs = 0
self._stop = threading.Event() self._stop = threading.Event()
self._end_of_input = False
self._end_of_input_seen = False
self._latest_output: ForceStreamOutput | None = None self._latest_output: ForceStreamOutput | None = None
self._outputs_seen = 0 self._outputs_seen = 0
self._published_outputs = 0
self._history: list[ForceStreamOutput] = []
self._delivered = 0 self._delivered = 0
self._events: list[StreamEvent] = [] self._events: list[StreamEvent] = []
self._pending_zero: _ZeroCommand | None = None self._pending_zero: _ZeroCommand | None = None
self._active_zero: _ZeroCommand | None = None self._active_zero: _ZeroCommand | None = None
self._last_source_time_ns: int | 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( self._pipeline = StreamPipeline(
sensor, sensor,
self._produce, self._produce,
depth=max(1, int(depth)), depth=self._depth,
include=(), include=self._include,
on_frame=self._on_snapshot, on_frame=self._on_snapshot,
verbose=self._verbose, verbose=self._verbose,
) )
@@ -184,10 +235,16 @@ class SensorStream:
session_id: str = "stream", session_id: str = "stream",
realtime: bool = True, realtime: bool = True,
depth: int = 1, depth: int = 1,
nominal_fps: float | None = None,
include: Iterable[str] = (),
long_gap_s: float = DEFAULT_LONG_GAP_S,
verbose: bool = False, verbose: bool = False,
sensor_options: Mapping[str, Any] | None = None, sensor_options: Mapping[str, Any] | None = None,
) -> SensorStream: ) -> 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( sensor = Sensor(
source, source,
config_name=config_name, config_name=config_name,
@@ -205,6 +262,9 @@ class SensorStream:
session_id=session_id, session_id=session_id,
realtime=realtime, realtime=realtime,
depth=depth, depth=depth,
nominal_fps=nominal_fps,
include=include,
long_gap_s=long_gap_s,
owns_sensor=True, owns_sensor=True,
verbose=verbose, verbose=verbose,
) )
@@ -221,6 +281,19 @@ class SensorStream:
return name return name
return "box" 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 # --------------------------------------------------------------- lifecycle
def start(self) -> SensorStream: def start(self) -> SensorStream:
if self._pipeline.running: if self._pipeline.running:
@@ -235,8 +308,6 @@ class SensorStream:
) )
) )
if not self._sensor_session_was_enabled: 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.enable_measurement_session()
self._sensor_session_enabled_by_stream = True self._sensor_session_enabled_by_stream = True
try: try:
@@ -247,7 +318,6 @@ class SensorStream:
return self return self
def _restore_sensor_session(self) -> None: 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: if not self._sensor_session_enabled_by_stream:
return return
self._sensor_session_enabled_by_stream = False self._sensor_session_enabled_by_stream = False
@@ -270,11 +340,16 @@ class SensorStream:
self._active_zero = None self._active_zero = None
self._cond.notify_all() self._cond.notify_all()
try: try:
self._pipeline.stop(timeout=timeout) try:
finally: self._pipeline.stop(timeout=timeout)
self._restore_sensor_session() finally:
if self._owns_sensor: self._restore_sensor_session()
self.sensor.disconnect() 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 error = self._pipeline.error
if error is not None: if error is not None:
raise SensorStreamError(f"stream stage failed: {error}") from error raise SensorStreamError(f"stream stage failed: {error}") from error
@@ -288,17 +363,62 @@ class SensorStream:
return self._pipeline.error return self._pipeline.error
@property @property
def overwritten_frames(self) -> int: def pipeline(self) -> StreamPipeline:
"""Candidate frames replaced before they were handed to the pipeline.""" return self._pipeline
return int(self._overwritten)
@property @property
def latest_output(self) -> ForceStreamOutput | None: def latest_output(self) -> ForceStreamOutput | None:
return self._latest_output return self._latest_output
@property @property
def pipeline(self) -> StreamPipeline: def overwritten_frames(self) -> int:
return self._pipeline """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 # ------------------------------------------------------------ external input
def push_frame( def push_frame(
@@ -307,15 +427,24 @@ class SensorStream:
*, *,
source_frame_id: int | None = None, source_frame_id: int | None = None,
capture_time_ns: int | None = None, capture_time_ns: int | None = None,
time_basis: str = "capture_monotonic", time_basis: str | None = None,
clock_id: str = "host", clock_id: str = "host",
source_pts: int | float | None = None, source_pts: int | float | None = None,
) -> None: media_time_ms: float | None = None,
"""Offer one already-prepared frame from a borrowed source. ) -> bool:
"""Offer one prepared frame from a borrowed source.
Real-time candidate semantics: if the producer has not claimed the Real-time candidate semantics: if the producer has not claimed the
previous candidate yet, this frame replaces it and increments previous candidate yet, this frame replaces it and increments
:attr:`overwritten_frames`; a claimed frame is never interrupted. :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: if not self._external_input:
raise RuntimeError( raise RuntimeError(
@@ -323,34 +452,131 @@ class SensorStream:
) )
if not isinstance(frame, np.ndarray) or frame.size == 0: if not isinstance(frame, np.ndarray) or frame.size == 0:
raise ValueError("frame must be a non-empty numpy array") 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: with self._cond:
if self._candidate is not None: if source_frame_id in self._seen_source_ids:
self._overwritten += 1 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 self._input_counter += 1
if source_frame_id is None: self._accepted_inputs += 1
source_frame_id = self._input_counter - 1 item = self._resolve_input_frame(
if capture_time_ns is None: frame,
capture_time_ns = time.perf_counter_ns() source_frame_id=source_frame_id,
self._candidate = _QueuedFrame( capture_time_ns=capture_time_ns,
image=frame, time_basis=time_basis,
source_frame_id=int(source_frame_id), clock_id=clock_id,
capture_time_ns=int(capture_time_ns), source_pts=source_pts,
time_basis=str(time_basis), media_time_ms=media_time_ms,
clock_id=str(clock_id), )
source_pts=source_pts, 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 # ----------------------------------------------------------------- producer
def _produce(self): def _produce(self):
if self._external_input: if self._external_input:
with self._cond: item = self._next_external_frame()
while self._candidate is None and not self._stop.is_set(): if item is None:
self._cond.wait(timeout=0.05) return None
if self._stop.is_set() or self._candidate is None:
return None
item = self._candidate
self._candidate = None
prepared = self.sensor.get_img( prepared = self.sensor.get_img(
frame=item.image, frame=item.image,
already_processed=True, already_processed=True,
@@ -360,21 +586,104 @@ class SensorStream:
return None return None
return ProducedFrame(prepared, metadata=item) return ProducedFrame(prepared, metadata=item)
# Built-in capture: the Sensor owns the source; the frame is produced by # Built-in capture: identity and clocks come from CapturedFrame. The
# its normal get_img path and the capture source keeps only its latest. # Sensor's own capture path keeps the source's latest/next frame.
self._input_counter += 1
image = self.sensor.get_img() image = self.sensor.get_img()
captured = getattr(self.sensor, "last_captured_frame", None)
if image is None: if image is None:
self._end_of_input_seen = True
self._maybe_fail_zero_after_input_end()
return None 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( item = _QueuedFrame(
image=image, image=image,
source_frame_id=self._input_counter - 1, source_frame_id=source_frame_id,
capture_time_ns=time.perf_counter_ns(), capture_time_ns=stamp_ns,
time_basis="capture_monotonic", time_basis=basis,
clock_id="host", clock_id="host",
source_pts=None,
media_time_ms=media_time_ms,
) )
return ProducedFrame(image, metadata=item) 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 # --------------------------------------------------------------- force stage
def _on_snapshot(self, snapshot) -> None: def _on_snapshot(self, snapshot) -> None:
"""Runs on the pipeline's HHD thread: the only place touching the session.""" """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_gap_s=command_to_start.max_gap_s,
max_window_s=command_to_start.max_window_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: with self._cond:
command_to_start.error = exc command_to_start.error = exc
command_to_start.done.set() command_to_start.done.set()
self._active_zero = None self._active_zero = None
self._cond.notify_all() self._cond.notify_all()
source_dt_s = None source_dt_s = None
long_gap = False
if self._last_source_time_ns is not None: if self._last_source_time_ns is not None:
source_dt_s = (metadata.capture_time_ns - self._last_source_time_ns) / 1e9 source_dt_s = (metadata.capture_time_ns - self._last_source_time_ns) / 1e9
long_gap = source_dt_s > self._long_gap_s
stamp = FrameStamp( stamp = FrameStamp(
sensor_id=self._sensor_id, sensor_id=self._sensor_id,
session_id=self._session_id, session_id=self._session_id,
@@ -445,13 +756,13 @@ class SensorStream:
), ),
torque=float(snapshot.torque), torque=float(snapshot.torque),
flow=None, flow=None,
field=None, field=snapshot.field if "field" in self._include else None,
valid_mask=None, valid_mask=None,
) )
try: try:
output = self.session.process(measurement) output = self.session.process(measurement)
events = self.session.drain_events() events = self.session.drain_events()
except BaseException as exc: # make the stage error visible through error/stop except BaseException as exc:
with self._cond: with self._cond:
if self._active_zero is not None: if self._active_zero is not None:
self._active_zero.error = exc self._active_zero.error = exc
@@ -460,20 +771,41 @@ class SensorStream:
self._cond.notify_all() self._cond.notify_all()
self._pipeline._error = exc self._pipeline._error = exc
return 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_time_ns = metadata.capture_time_ns
self._last_source_dt_s = source_dt_s
self._last_long_gap = long_gap
with self._cond: with self._cond:
self._latest_output = output self._latest_output = output
self._outputs_seen += 1 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) self._events.extend(events)
command = self._active_zero command = self._active_zero
if command is not None: 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 command.result = self.session.zero
if command.result.phase in ("ready", "failed"): if command.result.phase in ("ready", "failed"):
command.done.set() command.done.set()
self._active_zero = None self._active_zero = None
self._cond.notify_all() self._cond.notify_all()
self._maybe_fail_zero_after_input_end()
# ------------------------------------------------------------------ outputs # ------------------------------------------------------------------ outputs
def wait_output(self, timeout: float | None = None) -> ForceStreamOutput | None: def wait_output(self, timeout: float | None = None) -> ForceStreamOutput | None:
@@ -491,6 +823,33 @@ class SensorStream:
self._delivered = self._outputs_seen self._delivered = self._outputs_seen
return self._latest_output 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, ...]: def drain_events(self) -> tuple[StreamEvent, ...]:
"""Session events emitted by the force thread since the previous drain.""" """Session events emitted by the force thread since the previous drain."""
with self._cond: with self._cond:
@@ -516,6 +875,12 @@ class SensorStream:
thread writes that command's ZeroProgress/error and signals it, so a thread writes that command's ZeroProgress/error and signals it, so a
second call can never observe the previous zero's state. The caller second call can never observe the previous zero's state. The caller
thread never reads ``session.zero``. 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: if not self._pipeline.running:
raise SensorStreamError("start the stream before reset_and_zero()") raise SensorStreamError("start the stream before reset_and_zero()")
@@ -531,13 +896,14 @@ class SensorStream:
target_reference_generation=target, target_reference_generation=target,
) )
self._pending_zero = command self._pending_zero = command
self._maybe_fail_zero_after_input_end()
if not command.done.wait(float(timeout)): if not command.done.wait(float(timeout)):
with self._cond: with self._cond:
if self._pending_zero is command: if self._pending_zero is command:
self._pending_zero = None self._pending_zero = None
raise TimeoutError( raise TimeoutError(
f"reset_and_zero({reference_id!r}) did not finish within {timeout}s " 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: if command.error is not None:
raise SensorStreamError(str(command.error)) from command.error raise SensorStreamError(str(command.error)) from command.error
+729
View File
@@ -15,8 +15,13 @@ import time
import numpy as np import numpy as np
import pytest 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 import Sensor
from orisys.sensor_stream import SensorStream, SensorStreamError from orisys.sensor_stream import SensorStream, SensorStreamError
from orisys.stream_contract import FrameStamp, MeasurementFrame
SIZE = 365 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"): with pytest.raises(RuntimeError, match="pipeline start failed"):
stream.start() stream.start()
assert sensor._measurement_session is False, "a failed start must restore the mode" 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()