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:
@@ -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:
|
||||
|
||||
+18
-2
@@ -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:
|
||||
|
||||
+451
-85
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user