feat: 新增单路流入口 SensorStream（T08a）
SensorStream 串起 Sensor/SensorContext、ImagePipeline 与 ForceStreamSession：实时模式只保留最新候选并计数覆盖；StreamPipeline 负责线程与原子复位版本，力侧只在 HHD 线程的 on_frame 中运行；reset_and_zero 原子领取目标版本，在力线程帧边界发起同版本空载归零，每次调用只等待自己的命令结果；借用的 Sensor 不被关闭，stop 后恢复原来的 measurement session 状态，拥有的 Sensor 在 stop 时关闭。回归 402 项、lint 通过。
This commit is contained in:
ykcheungab
@@ -0,0 +1,556 @@
|
||||
"""Single-stream entry point: SensorStream (T08a).
|
||||
|
||||
``SensorStream`` ties the accepted T04–T07 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
|
||||
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;
|
||||
* the HHD stage's ``on_frame`` callback is the single force-side thread. It
|
||||
builds the T03 ``MeasurementFrame`` for the 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;
|
||||
* ``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.
|
||||
|
||||
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.
|
||||
|
||||
One instance belongs to one running stream; one thread owns the force session.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Mapping
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .force_readout import OutputSettings
|
||||
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
|
||||
|
||||
__all__ = ["SensorStream", "SensorStreamError"]
|
||||
|
||||
#: How long reset_and_zero waits for the force session to report ready.
|
||||
DEFAULT_ZERO_TIMEOUT_S = 30.0
|
||||
|
||||
|
||||
class SensorStreamError(RuntimeError):
|
||||
"""A stream stage, command or zero operation failed; the caller sees it."""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _QueuedFrame:
|
||||
image: np.ndarray
|
||||
source_frame_id: int
|
||||
capture_time_ns: int
|
||||
time_basis: str
|
||||
clock_id: str
|
||||
source_pts: int | float | None = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class _ZeroCommand:
|
||||
reference_id: str
|
||||
declared_unloaded: bool
|
||||
max_gap_s: float | None
|
||||
max_window_s: float | None
|
||||
target_reference_generation: int
|
||||
done: threading.Event = field(default_factory=threading.Event)
|
||||
result: ZeroProgress | None = None
|
||||
error: BaseException | None = None
|
||||
|
||||
|
||||
class SensorStream:
|
||||
"""One single-sensor stream: capture → ImagePipeline → ForceStreamSession."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
sensor: Sensor,
|
||||
*,
|
||||
sensor_family: str | None = None,
|
||||
calibration=None,
|
||||
context=None,
|
||||
settings: OutputSettings | None = None,
|
||||
design_sample_rate_hz: float | None = None,
|
||||
sensor_id: str | None = None,
|
||||
session_id: str = "stream",
|
||||
realtime: bool = True,
|
||||
depth: int = 1,
|
||||
owns_sensor: bool = False,
|
||||
verbose: bool = False,
|
||||
) -> 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._session_id = str(session_id)
|
||||
self._sensor_id = (
|
||||
str(sensor_id)
|
||||
if sensor_id is not None
|
||||
else getattr(getattr(sensor, "context", None), "sensor_id", None)
|
||||
)
|
||||
if not self._sensor_id:
|
||||
raise ValueError(
|
||||
"SensorStream needs an explicit sensor_id or a SensorContext with a bound SN"
|
||||
)
|
||||
if calibration is None:
|
||||
calibration = getattr(sensor, "calibration", None)
|
||||
family = sensor_family or self._derive_family(sensor)
|
||||
self.session = ForceStreamSession(
|
||||
sensor_family=family,
|
||||
device_id=getattr(sensor, "device_id", None),
|
||||
context=context
|
||||
if context is not None
|
||||
else getattr(sensor, "context", None),
|
||||
calibration=calibration,
|
||||
settings=settings,
|
||||
design_sample_rate_hz=design_sample_rate_hz,
|
||||
)
|
||||
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,
|
||||
"measurement_session",
|
||||
getattr(sensor, "_measurement_session", False),
|
||||
)
|
||||
)
|
||||
self._sensor_session_enabled_by_stream = False
|
||||
self._cond = threading.Condition()
|
||||
self._candidate: _QueuedFrame | None = None
|
||||
self._overwritten = 0
|
||||
self._input_counter = 0
|
||||
self._stop = threading.Event()
|
||||
self._latest_output: ForceStreamOutput | None = None
|
||||
self._outputs_seen = 0
|
||||
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._pipeline = StreamPipeline(
|
||||
sensor,
|
||||
self._produce,
|
||||
depth=max(1, int(depth)),
|
||||
include=(),
|
||||
on_frame=self._on_snapshot,
|
||||
verbose=self._verbose,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------ ctors
|
||||
@classmethod
|
||||
def open(
|
||||
cls,
|
||||
source,
|
||||
*,
|
||||
config_name: str = "box",
|
||||
calibration=None,
|
||||
context=None,
|
||||
settings: OutputSettings | None = None,
|
||||
design_sample_rate_hz: float | None = None,
|
||||
sensor_id: str | None = None,
|
||||
session_id: str = "stream",
|
||||
realtime: bool = True,
|
||||
depth: int = 1,
|
||||
verbose: bool = False,
|
||||
sensor_options: Mapping[str, Any] | None = None,
|
||||
) -> SensorStream:
|
||||
"""Open a built-in capture source; the stream owns and closes the Sensor."""
|
||||
sensor = Sensor(
|
||||
source,
|
||||
config_name=config_name,
|
||||
verbose=verbose,
|
||||
**(dict(sensor_options or {})),
|
||||
)
|
||||
return cls(
|
||||
sensor,
|
||||
sensor_family=None,
|
||||
calibration=calibration,
|
||||
context=context,
|
||||
settings=settings,
|
||||
design_sample_rate_hz=design_sample_rate_hz,
|
||||
sensor_id=sensor_id,
|
||||
session_id=session_id,
|
||||
realtime=realtime,
|
||||
depth=depth,
|
||||
owns_sensor=True,
|
||||
verbose=verbose,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _derive_family(sensor: Sensor) -> str:
|
||||
context = getattr(sensor, "context", None)
|
||||
family = getattr(context, "product_family", None)
|
||||
if family:
|
||||
return str(family)
|
||||
text = str(getattr(sensor, "config_name", "") or "").replace("\\", "/")
|
||||
for name in ("box", "mini", "finger"):
|
||||
if name in text:
|
||||
return name
|
||||
return "box"
|
||||
|
||||
# --------------------------------------------------------------- lifecycle
|
||||
def start(self) -> SensorStream:
|
||||
if self._pipeline.running:
|
||||
return self
|
||||
self._stop.clear()
|
||||
if self._has_measurement_session:
|
||||
self._sensor_session_was_enabled = bool(
|
||||
getattr(
|
||||
self.sensor,
|
||||
"measurement_session",
|
||||
getattr(self.sensor, "_measurement_session", False),
|
||||
)
|
||||
)
|
||||
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:
|
||||
self._pipeline.start()
|
||||
except BaseException:
|
||||
self._restore_sensor_session()
|
||||
raise
|
||||
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
|
||||
disable = getattr(self.sensor, "disable_measurement_session", None)
|
||||
if callable(disable):
|
||||
disable()
|
||||
|
||||
def stop(self, timeout: float = 5.0) -> None:
|
||||
"""Stop all stages; the borrowed source stays open, the owned one closes."""
|
||||
self._stop.set()
|
||||
with self._cond:
|
||||
self._candidate = None
|
||||
for command in (self._pending_zero, self._active_zero):
|
||||
if command is not None and not command.done.is_set():
|
||||
command.error = SensorStreamError(
|
||||
"stream stopped before zero completed"
|
||||
)
|
||||
command.done.set()
|
||||
self._pending_zero = None
|
||||
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()
|
||||
error = self._pipeline.error
|
||||
if error is not None:
|
||||
raise SensorStreamError(f"stream stage failed: {error}") from error
|
||||
|
||||
@property
|
||||
def running(self) -> bool:
|
||||
return self._pipeline.running
|
||||
|
||||
@property
|
||||
def error(self) -> BaseException | None:
|
||||
return self._pipeline.error
|
||||
|
||||
@property
|
||||
def overwritten_frames(self) -> int:
|
||||
"""Candidate frames replaced before they were handed to the pipeline."""
|
||||
return int(self._overwritten)
|
||||
|
||||
@property
|
||||
def latest_output(self) -> ForceStreamOutput | None:
|
||||
return self._latest_output
|
||||
|
||||
@property
|
||||
def pipeline(self) -> StreamPipeline:
|
||||
return self._pipeline
|
||||
|
||||
# ------------------------------------------------------------ external input
|
||||
def push_frame(
|
||||
self,
|
||||
frame: np.ndarray,
|
||||
*,
|
||||
source_frame_id: int | None = None,
|
||||
capture_time_ns: int | None = None,
|
||||
time_basis: str = "capture_monotonic",
|
||||
clock_id: str = "host",
|
||||
source_pts: int | float | None = None,
|
||||
) -> None:
|
||||
"""Offer one already-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.
|
||||
"""
|
||||
if not self._external_input:
|
||||
raise RuntimeError(
|
||||
"push_frame() is for external-frame streams; built-in capture produces frames itself"
|
||||
)
|
||||
if not isinstance(frame, np.ndarray) or frame.size == 0:
|
||||
raise ValueError("frame must be a non-empty numpy array")
|
||||
with self._cond:
|
||||
if self._candidate is not None:
|
||||
self._overwritten += 1
|
||||
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._cond.notify_all()
|
||||
|
||||
# ----------------------------------------------------------------- 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
|
||||
prepared = self.sensor.get_img(
|
||||
frame=item.image,
|
||||
already_processed=True,
|
||||
already_gray=item.image.ndim == 2,
|
||||
)
|
||||
if prepared is None:
|
||||
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
|
||||
image = self.sensor.get_img()
|
||||
if image is None:
|
||||
return None
|
||||
item = _QueuedFrame(
|
||||
image=image,
|
||||
source_frame_id=self._input_counter - 1,
|
||||
capture_time_ns=time.perf_counter_ns(),
|
||||
time_basis="capture_monotonic",
|
||||
clock_id="host",
|
||||
)
|
||||
return ProducedFrame(image, metadata=item)
|
||||
|
||||
# --------------------------------------------------------------- force stage
|
||||
def _on_snapshot(self, snapshot) -> None:
|
||||
"""Runs on the pipeline's HHD thread: the only place touching the session."""
|
||||
metadata = snapshot.source_metadata
|
||||
if not isinstance(metadata, _QueuedFrame):
|
||||
self._pipeline._error = SensorStreamError(
|
||||
"snapshot has no source metadata frame contract"
|
||||
)
|
||||
return
|
||||
command_to_start: _ZeroCommand | None = None
|
||||
with self._cond:
|
||||
if self._active_zero is None and self._pending_zero is not None:
|
||||
pending = self._pending_zero
|
||||
version = int(snapshot.tracker_reset_seq)
|
||||
if version >= pending.target_reference_generation:
|
||||
self._pending_zero = None
|
||||
if version > pending.target_reference_generation:
|
||||
pending.error = SensorStreamError(
|
||||
f"reference generation {version} advanced past the "
|
||||
f"requested zero target {pending.target_reference_generation}"
|
||||
)
|
||||
pending.done.set()
|
||||
self._cond.notify_all()
|
||||
else:
|
||||
self._active_zero = pending
|
||||
command_to_start = pending
|
||||
if command_to_start is not None:
|
||||
try:
|
||||
self.session.request_zero(
|
||||
reference_id=command_to_start.reference_id,
|
||||
declared_unloaded=command_to_start.declared_unloaded,
|
||||
expected_reference_generation=command_to_start.target_reference_generation,
|
||||
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
|
||||
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
|
||||
if self._last_source_time_ns is not None:
|
||||
source_dt_s = (metadata.capture_time_ns - self._last_source_time_ns) / 1e9
|
||||
stamp = FrameStamp(
|
||||
sensor_id=self._sensor_id,
|
||||
session_id=self._session_id,
|
||||
frame_id=int(snapshot.seq),
|
||||
source_frame_id=metadata.source_frame_id,
|
||||
capture_time_ns=metadata.capture_time_ns,
|
||||
clock_id=metadata.clock_id,
|
||||
capture_time_basis=metadata.time_basis,
|
||||
source_pts=metadata.source_pts,
|
||||
)
|
||||
frame = MeasurementFrame(
|
||||
stamp=stamp,
|
||||
source_dt_s=source_dt_s,
|
||||
reference_generation=int(snapshot.tracker_reset_seq),
|
||||
)
|
||||
from .image_pipeline import ImageMeasurement
|
||||
|
||||
measurement = ImageMeasurement(
|
||||
frame=frame,
|
||||
force_raw=(
|
||||
float(snapshot.normal_force_raw),
|
||||
float(snapshot.shear_x_raw),
|
||||
float(snapshot.shear_y_raw),
|
||||
),
|
||||
torque=float(snapshot.torque),
|
||||
flow=None,
|
||||
field=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
|
||||
with self._cond:
|
||||
if self._active_zero is not None:
|
||||
self._active_zero.error = exc
|
||||
self._active_zero.done.set()
|
||||
self._active_zero = None
|
||||
self._cond.notify_all()
|
||||
self._pipeline._error = exc
|
||||
return
|
||||
self._last_source_time_ns = metadata.capture_time_ns
|
||||
with self._cond:
|
||||
self._latest_output = output
|
||||
self._outputs_seen += 1
|
||||
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()
|
||||
|
||||
# ------------------------------------------------------------------ outputs
|
||||
def wait_output(self, timeout: float | None = None) -> ForceStreamOutput | None:
|
||||
"""Latest-wins output view: wait for the next-published row, return newest."""
|
||||
deadline = None if timeout is None else time.perf_counter() + timeout
|
||||
with self._cond:
|
||||
start = self._outputs_seen
|
||||
while self._outputs_seen == start:
|
||||
remaining = None
|
||||
if deadline is not None:
|
||||
remaining = deadline - time.perf_counter()
|
||||
if remaining <= 0:
|
||||
return None
|
||||
self._cond.wait(timeout=remaining)
|
||||
self._delivered = self._outputs_seen
|
||||
return self._latest_output
|
||||
|
||||
def drain_events(self) -> tuple[StreamEvent, ...]:
|
||||
"""Session events emitted by the force thread since the previous drain."""
|
||||
with self._cond:
|
||||
events = tuple(self._events)
|
||||
self._events.clear()
|
||||
return events
|
||||
|
||||
# ------------------------------------------------------------ combined zero
|
||||
def reset_and_zero(
|
||||
self,
|
||||
*,
|
||||
reference_id: str,
|
||||
declared_unloaded: bool = True,
|
||||
timeout: float = DEFAULT_ZERO_TIMEOUT_S,
|
||||
max_gap_s: float | None = None,
|
||||
max_window_s: float | None = None,
|
||||
):
|
||||
"""Image reset → same-version unloaded baseline → force ready (K8.5).
|
||||
|
||||
The image version is claimed atomically by StreamPipeline; the force
|
||||
session receives it on its own thread. The caller only names the zero,
|
||||
never a version number. Each call owns one command object: the force
|
||||
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 not self._pipeline.running:
|
||||
raise SensorStreamError("start the stream before reset_and_zero()")
|
||||
with self._cond:
|
||||
if self._pending_zero is not None or self._active_zero is not None:
|
||||
raise SensorStreamError("another reset_and_zero is already in progress")
|
||||
target = int(self._pipeline.request_reset())
|
||||
command = _ZeroCommand(
|
||||
reference_id=str(reference_id),
|
||||
declared_unloaded=bool(declared_unloaded),
|
||||
max_gap_s=max_gap_s,
|
||||
max_window_s=max_window_s,
|
||||
target_reference_generation=target,
|
||||
)
|
||||
self._pending_zero = command
|
||||
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})"
|
||||
)
|
||||
if command.error is not None:
|
||||
raise SensorStreamError(str(command.error)) from command.error
|
||||
progress = command.result
|
||||
if progress is None:
|
||||
raise SensorStreamError(
|
||||
f"reset_and_zero({reference_id!r}) finished without progress"
|
||||
)
|
||||
if progress.phase == "failed":
|
||||
raise SensorStreamError(f"unloaded zero failed: {progress.failure_reason}")
|
||||
if progress.phase != "ready":
|
||||
raise SensorStreamError(
|
||||
f"reset_and_zero({reference_id!r}) finished in phase "
|
||||
f"{progress.phase!r}, not ready"
|
||||
)
|
||||
return progress
|
||||
Reference in New Issue
Block a user