orisys-tech/orisys-app
3
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

migrated sensor processing to stream

Browse Source 
added agent doc and coding rules
This commit is contained in:
ykcheungab
2026-09-30 21:22:41 +08:00
parent f875378503
commit d914b81242
8 changed files with 1798 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
shared/src/orisys_app_shared/acquisition/finger_worker.py
+477 -5
View File
@@ -5,6 +5,7 @@ from __future__ import annotations
import logging
import threading
import time
from collections import deque
from collections.abc import Callable
from pathlib import Path
@@ -22,6 +23,9 @@ from orisys_app_shared.acquisition.models import (
from orisys_app_shared.acquisition.models import (
ensure_bgr as _ensure_bgr,
)
from orisys_app_shared.acquisition.models import (
from_snapshot as _packet_from_snapshot,
)
from orisys_app_shared.acquisition.models import (
is_file_source as _is_file_source,
)
@@ -33,6 +37,7 @@ from orisys_app_shared.acquisition.sdk_worker import (
PAUSE_POLL_S,
STOP_JOIN_TIMEOUT_S,
VIDEO_EOF_TIMEOUT_S,
_capture_dropped,
_capture_sequence,
calibration_digest,
capture_status_of,
@@ -49,6 +54,121 @@ logger = logging.getLogger(__name__)
_EXPORT_REQUIRED = ("mesh.npz", "camera_intrinsics.npz", "world_to_cam.npy")
class _RuntimeCapture:
"""Expose a ``FingerRuntime``'s capture source to the capture-rate helpers.
Those helpers (``_capture_sequence``, ``_capture_dropped``) read
``sensor.capture_source``; the finger runtime owns the source itself and the
tactile processor in front of the sensor has no such attribute, so this adapter
is what they are given.
"""
def __init__(self, runtime) -> None:
self._runtime = runtime
@property
def capture_source(self):
return getattr(self._runtime, "capture_source", None)
class _FingerProducer:
"""The ``StreamPipeline`` produce callable for a ``FingerRuntime``.
The finger pipeline's producer is ``runtime.grab_and_process`` — read the
source and UV-unroll it — so this wrapper adds the two things that have to
happen on the producer thread:
* **One published frame per delivered frame.** ``CaptureSource.read_latest``
returns the newest frame and then that same frame again until its background
reader produces another, so a loop that read without looking would publish
each frame as many times as it could poll. Only a source with a running
background reader has the sequence counter that says so; a file decodes a
new frame per call and is read once per call.
* **The pre-processing frame.** ``runtime.process_frame`` hands the unrolled
frame to the tactile sensor and drops the source frame; the record channel's
pre-roll history wants the source frame, so it is copied here while it is
still this frame's, and handed on by sequence.
"""
CACHE = 8
def __init__(self, runtime, *, is_file: bool, frame_period: float = 0.0) -> None:
self.runtime = runtime
self.is_file = bool(is_file)
#: A file has no background reader to pace on, so the pass is paced by the
#: declared frame period (see the box worker's producer).
self.frame_period = float(frame_period)
self.produced = 0
self.capture_source = _RuntimeCapture(runtime)
self.capture_seq = _capture_sequence(self.capture_source)
self._cache: deque = deque(maxlen=self.CACHE)
self._times: deque = deque()
self._last_read_at: float | None = None
def __call__(self):
self._wait_for_a_new_frame()
self._pace()
t0 = time.perf_counter()
ok, source_frame = self.runtime.read_source()
if not ok or source_frame is None:
return None
unrolled = self.runtime.process_frame(source_frame, color=False)
if unrolled is None:
return None
self._last_read_at = t0
self.produced += 1
self._times.append((self.produced, t0))
cutoff = t0 - FPS_WINDOW_S
while self._times and self._times[0][1] < cutoff:
self._times.popleft()
self._cache.append((self.produced, np.array(source_frame, copy=True)))
return unrolled
def _pace(self) -> None:
if not self.is_file or self.frame_period <= 0.0 or self._last_read_at is None:
return
remaining = self.frame_period - (time.perf_counter() - self._last_read_at)
if remaining > PAUSE_POLL_S:
time.sleep(min(remaining, self.frame_period * 0.5))
def _wait_for_a_new_frame(self) -> None:
if self.is_file or self.capture_seq is None:
return
deadline = time.perf_counter() + CAMERA_DISCONNECT_TIMEOUT_S
while True:
current = _capture_sequence(self.capture_source)
if current is None or current != self.capture_seq:
self.capture_seq = current
return
if time.perf_counter() >= deadline:
# The camera stopped publishing but the device is still open: read
# on, so the session survives for the consumer's liveness check to
# see and report rather than stalling here.
return
time.sleep(0)
def raw_frame(self, seq: int) -> np.ndarray | None:
for number, frame in reversed(self._cache):
if number == seq:
return frame
return None
def collect_perf(self, seq: int) -> float | None:
self._trim()
for number, stamp in reversed(self._times):
if number == seq:
return stamp
return None
def _trim(self) -> None:
"""Drop collection times older than the frame-rate window."""
if not self._times:
return
cutoff = self._times[-1][1] - FPS_WINDOW_S
while self._times and self._times[0][1] < cutoff:
self._times.popleft()
def resolve_finger_export_dir(config_ref: str | Path) -> Path | None:
"""Return local ``assets/30fps-0703`` when present and complete.
@@ -82,8 +202,15 @@ class FingerAcquisitionWorker(RecordSinkMixin):
on_status: Callable[[str], None] | None = None,
on_calibration_applied: Callable[[str, str, int, float, int], None] | None = None,
on_reset_applied: Callable[[int, float, int], None] | None = None,
use_stream_pipeline: bool = True,
):
self._init_sinks()
#: ``runtime.pipeline()`` is the acquisition path; the serial chain is kept
#: so a change can be checked against it frame-for-frame on the same input
#: (``False`` selects the reference).
self._use_stream_pipeline = bool(use_stream_pipeline)
#: The running session's ``StreamPipeline``, when there is one.
self._pipe = None
self._on_error = on_error
self._on_status = on_status
#: Called from the acquisition thread once new coefficients are loaded,
@@ -253,13 +380,39 @@ class FingerAcquisitionWorker(RecordSinkMixin):
Same contract as the box worker's: the source keeps being read so the device
is not released and the disconnect watchdog stays quiet, but no frame is
processed (no flow, no depth, no contact) and no packet is emitted, so the
display freezes on its last frame and the record channel sees a gap.
display freezes on its last frame and the record channel sees a gap. The
concurrent pipeline holds at ``PauseLevel.CAPTURE_ONLY``: the read continues
— which is what keeps exposure, white balance and frame rate converged —
while the flow and HHD stages are skipped.
"""
self._pause.set()
pipe = self._pipe
if pipe is not None:
try:
pipe.pause(self._pause_level())
except Exception: # noqa: BLE001 - the loop honours ``_pause`` anyway
logger.exception("Could not hold the finger pipeline for a pause")
def resume(self) -> None:
"""Start computing and publishing again, from the *newest* frame."""
"""Start computing and publishing again, from the *newest* frame.
The tracker is rebased, because a ``CAPTURE_ONLY`` hold read frames that were
never computed: keeping the pre-pause reference frame would report the hold
itself as a large deformation.
"""
self._pause.clear()
pipe = self._pipe
if pipe is not None:
try:
pipe.resume(reset=True)
except Exception: # noqa: BLE001 - the loop honours ``_pause`` anyway
logger.exception("Could not release the finger pipeline for a resume")
@staticmethod
def _pause_level():
from orisys.pipeline import PauseLevel
return PauseLevel.CAPTURE_ONLY
@property
def paused(self) -> bool:
@@ -373,6 +526,22 @@ class FingerAcquisitionWorker(RecordSinkMixin):
self._phase = "idle"
def _run_session(self, session: int, previous: threading.Thread | None) -> None:
opened = self._open_runtime(session, previous)
if opened is None:
return
mode, source, runtime, is_file = opened
if self._use_stream_pipeline:
# The recommended path: the SDK runs produce (capture + UV unroll) ->
# flow -> HHD on three threads, so throughput is bounded by the slowest
# stage rather than by their sum. The app's own layer — session and
# frame bookkeeping, the record sink, calibration reload, contact
# provenance — stays on top of the frames the pipeline finishes.
self._run_stream_pipeline(session, mode, source, runtime, is_file)
return
self._run_serial_loop(session, mode, source, runtime, is_file)
def _open_runtime(self, session: int, previous: threading.Thread | None):
"""Open the runtime and configure it; ``None`` when it cannot run."""
from orisys.finger import FingerRuntime
if previous is not None and previous.is_alive():
@@ -384,7 +553,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
mode = self._mode
source = self._video_source
if mode is None or source is None:
return
return None
is_file = _is_file_source(source)
try:
self._status(f"Opening FingerRuntime ({mode.label()})…")
@@ -400,11 +569,13 @@ class FingerAcquisitionWorker(RecordSinkMixin):
self._runtimes[session] = runtime
if self._stop.is_set() or session != self._session_id:
self._close_runtime(session)
return
return None
# One frame through the unroll and one HHD pass, so the first
# *published* frame is not a warm-up outlier.
runtime.warmup(color=False)
if self._stop.is_set() or session != self._session_id:
self._close_runtime(session)
return
return None
raw_sensor = getattr(runtime.tactile, "raw_sensor", None)
if raw_sensor is not None and hasattr(raw_sensor, "configure"):
# The filter chain lives in the SDK; the app says what the
@@ -432,12 +603,313 @@ class FingerAcquisitionWorker(RecordSinkMixin):
str(loaded_path(tactile) or ""), loaded, 0, 0.0, int(session)
)
except Exception as exc:
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
self._set_phase("idle")
return None
return mode, source, runtime, is_file
# ---------------------------------------------------------- concurrent path
def _run_stream_pipeline(
self, session: int, mode: FixedCameraMode, source, runtime, is_file: bool
) -> None:
"""Drive the SDK's concurrent ``StreamPipeline`` for the finger runtime.
The pipeline is built the way ``runtime.pipeline()`` builds it — a
``StreamPipeline`` over the runtime's tactile sensor — but with this
worker's own produce callable in place of ``grab_and_process``. The callable
is what reads the source, so it is where the two things that must happen on
the producer thread live: one published frame per *delivered* frame (a live
source hands the same frame back until its reader produces another), and the
copy of the pre-unroll source frame the record channel's history wants. See
:class:`_FingerProducer`.
"""
import queue as queue_module
from orisys.pipeline import PauseLevel, StreamPipeline
tactile = runtime.tactile
raw_sensor = getattr(tactile, "raw_sensor", None)
frame_period = 1.0 / max(1.0, float(mode.fps or 30.0))
producer = _FingerProducer(runtime, is_file=is_file, frame_period=frame_period)
snapshots: queue_module.Queue = queue_module.Queue(maxsize=64)
mono_t0 = time.perf_counter()
frame_times: list[float] = []
capture = _RuntimeCapture(runtime)
live_capture = _capture_sequence(capture) is not None
frame_id = 0
capture_fps = 0.0
capture_dropped = 0
session_started = time.perf_counter()
last_frame_at = session_started
saw_frame = False
errors: list[str] = []
with self._lock:
self._phase = "running"
try:
pipe = StreamPipeline(
raw_sensor,
producer,
# The SDK's default: run contact analysis on every frame, because
# the pane's contact overlay and the 2D arrow both read it.
contact=True,
include=self._stream_include(),
on_frame=snapshots.put,
)
except Exception as exc: # noqa: BLE001 - reported, not swallowed
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
self._set_phase("idle")
return
# Published before ``start``: a pause clicked while the runtime was still
# opening must not be lost (see the box worker).
self._pipe = pipe
try:
pipe.start()
if self._pause.is_set():
pipe.pause(self._pause_level())
while not self._stop.is_set() and session == self._session_id:
if pipe.paused:
# ``wait`` returns at once while paused: sleep out the hold
# rather than spinning on it.
time.sleep(PAUSE_POLL_S)
continue
if pipe.error is not None:
# A stage raised. ``wait`` would re-raise it; this loop does not
# call ``wait``, so it is reported here instead.
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(pipe.error))
break
# How many frames the producer has finished, and how many this loop
# has taken: a ``wait`` is only worth making while the first is
# ahead. ``producer.produced`` also latches the moment the first read
# has happened, which is what keeps the "no frames" deadline below
# from racing the very first frame.
expect = max(producer.produced, frame_id)
snap = None
if expect > 0:
try:
snap = snapshots.get(timeout=0.05)
except queue_module.Empty:
snap = None
except Exception as exc: # noqa: BLE001 - a stage failure ends the session
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
break
now = time.perf_counter()
if snap is not None:
saw_frame = True
last_frame_at = now
try:
packet = self._packet_from_stream_snapshot(
session=session,
mode=mode,
runtime=runtime,
tactile=tactile,
producer=producer,
live_capture=live_capture,
snap=snap,
frame_id=frame_id,
frame_period=frame_period,
is_file=is_file,
frame_times=frame_times,
mono_t0=mono_t0,
capture_fps=capture_fps,
capture_dropped=capture_dropped,
compute_error="; ".join(errors),
)
except Exception as exc: # noqa: BLE001
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
break
capture_fps = packet.capture_fps
capture_dropped = packet.capture_dropped
if self._pause.is_set():
# The hold arrived after this frame was published but before
# it was built: see the box worker. A paused worker publishes
# nothing, including frames already in flight.
continue
# A zeroing and new coefficients are applied by this thread at a
# frame boundary, then reported with the frame and time they took
# effect at — the same confirmation rule as the serial loop.
try:
self._consume_pending_calibration(
tactile, frame_id + 1, float(packet.mono_t or 0.0)
)
except Exception as exc: # noqa: BLE001
errors.append(f"calibration rejected: {type(exc).__name__}: {exc}")
packet.valid = False
packet.stale_forces = True
packet.forces_valid = False
packet.invalid_reason = packet.invalid_reason or errors[-1]
if self._on_error is not None and session == self._session_id:
self._on_error(errors[-1])
try:
self._consume_pending_reset(
runtime, frame_id + 1, float(packet.mono_t or 0.0)
)
except Exception as exc: # noqa: BLE001
message = f"reset failed: {type(exc).__name__}: {exc}"
if self._on_error is not None and session == self._session_id:
self._on_error(message)
with self._lock:
self._latest = packet
self._phase = "running"
# Record channel: every published packet, exactly as the serial
# loop emitted them.
self._emit(packet)
frame_id += 1
# Liveness. The pipeline reports "input ended" and "no newer frame"
# with the same ``None``, so these facts are what tell a finished
# pass, a dead camera and a slow start apart.
producer_done = not pipe.running and producer.produced <= frame_id
if not saw_frame:
if is_file and (now - session_started) >= FILE_FIRST_FRAME_TIMEOUT_S:
message = (
f"this video produced no frames in "
f"{FILE_FIRST_FRAME_TIMEOUT_S:.0f} s: {source} may be "
"unreadable, truncated, or in a codec this build cannot decode"
)
if self._on_error is not None and session == self._session_id:
self._on_error(message)
self._set_end_reason("error")
break
if producer_done:
# The read itself ended and nothing was ever measured: say so
# now instead of waiting out the deadline above.
if self._on_error is not None and session == self._session_id:
self._on_error(
f"this source produced no frames: {source} may be "
"unreadable, truncated, or in a codec this build cannot "
"decode"
)
self._set_end_reason("error" if is_file else "disconnected")
break
elif producer_done:
# Every frame the producer made has become a packet and there is
# no newer one: the clean end-of-input the serial loop reported.
self._set_end_reason("ended")
break
elif not is_file and (now - last_frame_at) >= CAMERA_DISCONNECT_TIMEOUT_S:
# The producer is still reading, but a live camera has published
# nothing for a whole timeout: the device is gone.
self._set_end_reason("disconnected")
break
finally:
self._pipe = None
try:
pipe.pause(PauseLevel.BOTH)
except Exception: # noqa: BLE001 - a failed pause must not block teardown
logger.debug("Could not hold the finger pipeline while stopping", exc_info=True)
try:
pipe.stop(timeout=STOP_JOIN_TIMEOUT_S)
except Exception as exc: # noqa: BLE001 - a stage error is reported here
if self._stop.is_set():
logger.debug("finger pipeline stopped with %r", exc, exc_info=True)
else:
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
if self._stop.is_set():
self._set_end_reason("stopped")
elif saw_frame and self._end_reason is None:
self._set_end_reason("ended")
self._set_phase("idle")
self._status("Finger acquisition stopped")
def _packet_from_stream_snapshot(
self,
*,
session: int,
mode: FixedCameraMode,
runtime,
tactile,
producer: "_FingerProducer",
live_capture: bool,
snap,
frame_id: int,
frame_period: float,
is_file: bool,
frame_times: list[float],
mono_t0: float,
capture_fps: float,
capture_dropped: int,
compute_error: str,
):
"""One :class:`SdkPacket` from one finished finger snapshot."""
produced_seq = frame_id + 1
t_collect = producer.collect_perf(produced_seq) or time.perf_counter()
mono = frame_id * frame_period if is_file else max(0.0, t_collect - mono_t0)
frame_times.append(t_collect)
cutoff = t_collect - FPS_WINDOW_S
del frame_times[
: next(
(index for index, stamp in enumerate(frame_times) if stamp >= cutoff),
len(frame_times),
)
]
span = frame_times[-1] - frame_times[0] if len(frame_times) > 1 else 0.0
fps = (len(frame_times) - 1) / span if span > 0.0 else 0.0
if live_capture:
measured = producer.capture_fps()
if measured > 0.0:
capture_fps = measured
taken = _capture_dropped(producer.capture_source)
if taken is not None:
capture_dropped = int(taken)
elif producer.produced > produced_seq:
capture_dropped = int(producer.produced - produced_seq)
with self._lock:
self._measured_capture_fps = capture_fps
raw_sensor = getattr(tactile, "raw_sensor", None)
return _packet_from_snapshot(
snap,
raw_sensor,
session_id=session,
frame_id=frame_id,
mono_t=mono,
fps=fps,
capture_fps=capture_fps,
capture_dropped=capture_dropped,
raw_frame=producer.raw_frame(produced_seq),
valid_mask=runtime.valid_mask,
contact_analysed=True,
contact_found=bool(getattr(raw_sensor, "centroid_found", False)),
compute_error=compute_error,
calibration_version=self._calibration_version,
valid=not compute_error,
)
def _stream_include(self) -> tuple[str, ...]:
"""The arrays the finger views display, so the SDK copies only those.
``image`` is the unrolled tactile frame the 2D canvas paints, ``flow`` and
``depth`` are the two quantities the 3D pane offers, and the contact arrays
are what its overlay draws. The serial loop read every one of them
unconditionally.
"""
return ("image", "flow", "depth", "centroid", "contour")
# ------------------------------------------------------------- serial path
def _run_serial_loop(
self, session: int, mode: FixedCameraMode, source, runtime, is_file: bool
) -> None:
"""The single-threaded chain, kept whole for the packet-diff check.
``use_stream_pipeline=False`` selects it: read, unroll, decompose and read
the forces back to back on this one thread, which bounds throughput by the
sum of the stages. It is the reference the concurrent path is measured
against, and it is not the path a session uses.
"""
frame_times: list[float] = []
#: (collection time, published sequence), so the camera's own rate
#: can be told apart from the rate this loop consumes (see the box
shared/src/orisys_app_shared/acquisition/models.py
+180
View File
@@ -122,6 +122,186 @@ class SdkPacket:
contact: ContactObservation | None = None
def contact_from_arrays(
centroid,
*,
found: bool | None = None,
frame_id: int = 0,
valid: bool = True,
) -> ContactObservation:
"""A :class:`ContactObservation` from the SDK's contact result.
``found`` is the SDK's own ``centroid_found`` flag and is what decides the
answer: the SDK leaves ``centroid`` at its default ``(0, 0)`` when it found no
expansion contour, so a corner centre is a *default*, not a measurement.
Reporting it as one would put a contact marker in the frame's corner that
nothing measured. Pass ``found=None`` only for a producer that has no such flag,
where the centre itself is the only evidence there is.
"""
xy: tuple[float, float] | None = None
if centroid is not None:
try:
arr = np.asarray(centroid, dtype=np.float64).reshape(-1)
if arr.size >= 2 and bool(np.all(np.isfinite(arr[:2]))):
xy = (float(arr[0]), float(arr[1]))
except Exception: # noqa: BLE001 - an unusable centre is not a centre
xy = None
located = xy is not None if found is None else bool(found and xy is not None)
return ContactObservation(
centroid_uv_xy=xy if located else None,
found=located,
valid=bool(valid and located),
frame_id=int(frame_id),
)
def from_snapshot(
snapshot,
sensor=None,
*,
session_id: int = 0,
frame_id: int | None = None,
mono_t: float | None = None,
wall_t: float | None = None,
fps: float | None = None,
process_ms: float | None = None,
capture_fps: float = 0.0,
capture_dropped: int = 0,
raw_frame: np.ndarray | None = None,
valid_mask: np.ndarray | None = None,
contact: ContactObservation | None = None,
contact_analysed: bool = False,
contact_found: bool | None = None,
compute_error: str = "",
calibration_version: str = "",
valid: bool | None = None,
) -> SdkPacket:
"""One :class:`SdkPacket` from one SDK ``FrameSnapshot`` plus the app's own facts.
The SDK owns the measurement; the app owns the provenance. Everything the
snapshot carries is mapped straight across, and everything the snapshot cannot
know — session identity, the record's frame counter, the capture counters, the
calibration in effect, whether the compute chain failed — is passed in
explicitly, so a reader of the packet can still tell a measurement from a
carry-over.
The force triplet is used **as the SDK computed it**: ``normal_force`` and the
shears are the SDK's filtered/calibrated output and ``*_raw`` is its unfiltered
integrator pair. The app must not filter either of them again (see
``FilterSettings.apply_to``: the filter chain lives in the SDK and the app only
asks for it and reads back what the device reports).
"""
err = str(compute_error or "")
if valid is None:
valid = not err
stale = bool(err)
frame = int(snapshot.seq if frame_id is None else frame_id)
image = snapshot.image
processed = ensure_bgr(image) if image is not None else image
wall = float(snapshot.timestamp if wall_t is None else wall_t)
if mono_t is None:
# The snapshot's age is measured from its own capture, so this is the
# frame's acquisition time as the SDK saw it, on the app's session origin.
mono_t = max(0.0, wall - float(snapshot.age_ms) / 1000.0)
if contact is None and contact_analysed:
contact = contact_from_arrays(
snapshot.centroid,
# The SDK's own "I found a contour" flag; not inferred from the centre,
# which it leaves at (0, 0) when it found nothing.
found=contact_found,
frame_id=frame,
valid=not stale,
)
flow = snapshot.flow
depth = snapshot.depth
flow_valid = flow is not None and not stale
depth_valid = depth is not None and not stale
calibrated = _force_calibrated(sensor)
return SdkPacket(
raw_frame=ensure_bgr(raw_frame) if raw_frame is not None else processed,
processed_frame=processed,
timestamp=wall,
wall_t=wall,
mono_t=float(mono_t),
fps=float(snapshot.fps if fps is None else fps),
process_ms=float(
(snapshot.produce_ms + snapshot.flow_ms + snapshot.hhd_ms)
if process_ms is None
else process_ms
),
capture_fps=float(capture_fps),
capture_dropped=int(capture_dropped),
normal_force=float(snapshot.normal_force),
shear_x=float(snapshot.shear_x),
shear_y=float(snapshot.shear_y),
normal_force_raw=float(snapshot.normal_force_raw),
shear_x_raw=float(snapshot.shear_x_raw),
shear_y_raw=float(snapshot.shear_y_raw),
force_unit=str(snapshot.unit),
force_calibrated=calibrated,
calibrated_axes=_calibrated_axes(sensor),
calibration_version=str(calibration_version or ""),
session_id=int(session_id),
frame_id=frame,
valid=bool(valid),
invalid_reason=err,
stale_forces=stale,
raw_available=_raw_available(sensor),
flow=flow,
depth=depth,
valid_mask=valid_mask,
flow_valid=flow_valid,
flow_frame_id=frame if flow_valid else None,
depth_valid=depth_valid,
depth_frame_id=frame if depth_valid else None,
forces_valid=bool(valid),
contact=contact,
)
def _force_calibrated(sensor) -> bool:
"""Whether the device carries coefficients, mirroring ``sdk_worker``'s rule."""
if sensor is None:
return False
if getattr(sensor, "force_calibrated", False):
return True
return getattr(sensor, "_force_cal_sensor_name", None) is not None
def _raw_available(sensor) -> bool:
"""False when the device exposes no RAW force channels at all."""
if sensor is None:
return False
return hasattr(sensor, "_force_raw") or hasattr(sensor, "fnormal_raw")
#: Maps the SDK's canonical channel keys to the app's short axis names.
_CALIBRATION_AXIS_ALIASES = {
"fn": ("fn", "normal", "normal_force"),
"fx": ("fx", "shear_x", "shearx"),
"fy": ("fy", "shear_y", "sheary"),
}
def _calibrated_axes(sensor) -> tuple[str, ...]:
"""Which axes have a loaded calibration curve (see ``sdk_worker``)."""
if sensor is None:
return ()
calibration = getattr(sensor, "calibration", None)
channels = getattr(calibration, "channels", None)
if not channels:
return ()
try:
keys = {str(key).lower() for key in channels}
except TypeError:
return ()
axes: list[str] = []
for axis, aliases in _CALIBRATION_AXIS_ALIASES.items():
if keys & set(aliases):
axes.append(axis)
return tuple(axes)
def load_fixed_mode(config_ref: str | Path) -> FixedCameraMode:
"""Resolve a config reference (a file path or a product name) to its geometry."""
ref = str(config_ref).strip()
shared/src/orisys_app_shared/acquisition/sdk_worker.py
+647 -33
View File
@@ -8,6 +8,7 @@ import logging
import sys
import threading
import time
from collections import deque
from collections.abc import Callable, Mapping
from pathlib import Path
from typing import Any
@@ -28,6 +29,12 @@ from orisys_app_shared.acquisition.models import (
from orisys_app_shared.acquisition.models import (
is_file_source as _is_file_source,
)
from orisys_app_shared.acquisition.models import (
from_snapshot as _packet_from_snapshot,
)
from orisys_app_shared.acquisition.models import (
_calibrated_axes,
)
from orisys_app_shared.acquisition.sinks import RecordSinkMixin
from orisys_app_shared.force.filters import FilterSettings, effective_filters
@@ -155,12 +162,19 @@ def _capture_dropped(sensor) -> int | None:
The SDK's background reader counts one of these every time it replaces a
frame that was never consumed, so this is the consumer's shortfall — the
number that says "the camera had it and we did not take it".
``None`` when the source cannot say: a file source has no background reader,
and a derived source (a finger runtime's tactile processor, say) may expose no
capture source at all. The caller then falls back to the pipeline's own
produced-versus-published count.
"""
source = _capture_source(sensor)
status = getattr(source, "status", None) if source is not None else None
if status is None:
return None
try:
return int(getattr(status, "dropped_frames"))
except (TypeError, ValueError):
except (AttributeError, TypeError, ValueError):
return None
@@ -397,34 +411,220 @@ def read_forces_from_sensor(sensor, *, config_ref: str | Path | None = None):
}
#: Maps the SDK's canonical channel keys to the app's short axis names.
_CALIBRATION_AXIS_ALIASES = {
"fn": ("fn", "normal", "normal_force"),
"fx": ("fx", "shear_x", "shearx"),
"fy": ("fy", "shear_y", "sheary"),
}
#: The axis helper lives in ``models`` beside
#: :func:`~orisys_app_shared.acquisition.models.from_snapshot`, which is the one
#: place that fills a packet from an SDK snapshot. ``_calibrated_axes`` is imported
#: above and re-exported from this module, its historical home.
class _FrameProducer:
"""The ``StreamPipeline`` produce callable for a box/mini ``Sensor``.
def _calibrated_axes(sensor) -> tuple[str, ...]:
"""Which axes have a loaded calibration curve.
Three jobs, all of which have to happen on the producer thread because they are
about the *read* rather than about the computation:
The SDK stores one curve per channel, so a device may be calibrated on its
normal axis only. Reporting a single sensor-level flag would make the app
write raw counts into the newton columns for the uncalibrated axes.
* **One published frame per delivered frame.** ``CaptureSource.read_latest``
hands back the newest frame and, until its background reader produces
another, that same frame again. The loop this replaces detected the repeat
through the source's sequence counter and skipped it, and the record channel
depends on that: without it a 30 fps device is recorded at the loop's polling
rate and every measurement is duplicated. The counter is only a repeat marker
for a source with a running background reader, so a video file — which
decodes a new frame per call — is read once per call as before.
* **The pre-processing frame.** ``Sensor.frame`` is overwritten by the next
``get_img`` call, so a snapshot's raw frame must be copied here, while it is
still this frame's, and handed to the consumer by sequence. The 3D pane and
the pre-roll buffer both read it.
* **When the frame was collected.** ``t0`` is the moment *before* the read, the
same instant the old loop stamped a frame with. The frame rate and
``mono_t`` are measured from it, not from when the pipeline finished.
"""
calibration = getattr(sensor, "calibration", None)
channels = getattr(calibration, "channels", None)
if not channels:
return ()
try:
keys = {str(key).lower() for key in channels}
except TypeError:
return ()
axes: list[str] = []
for axis, aliases in _CALIBRATION_AXIS_ALIASES.items():
if keys & set(aliases):
axes.append(axis)
return tuple(axes)
#: How many frames of raw-frame history to keep. Must exceed the pipeline's
#: queue depth: the consumer can be a few frames behind the producer.
CACHE = 8
def __init__(
self, sensor, *, is_file: bool, repeat_wait_s: float, frame_period: float = 0.0
) -> None:
self.sensor = sensor
self.is_file = bool(is_file)
self.repeat_wait_s = float(repeat_wait_s)
#: What one frame of this device is meant to cost. A *file* is paced by it:
#: a video has no background reader to wait on, so without this the pass runs
#: as fast as the CPU allows and stamps far more video time than real time —
#: the serial loop paced the same way (see its ``last_frame_at``).
self.frame_period = float(frame_period)
self.produced = 0
self.capture_seq = _capture_sequence(sensor)
self._cache: deque = deque(maxlen=self.CACHE)
#: ``(sequence, collection time)`` per produced frame, for the frame rate.
self._times: deque = deque()
self._last_read_at: float | None = None
#: ``(clock, seconds, delivered frames)`` samples behind ``capture_fps``.
self._rate_samples: deque = deque()
self._capture_mark: tuple[float, int] | None = None
#: The wait's own anchor into the counter, consumed by ``capture_fps``.
self._repeat_check: tuple[float, int] | None = None
def __call__(self):
self._wait_for_a_new_frame()
self._pace()
t0 = time.perf_counter()
image = self.sensor.get_img()
if image is None:
return None
self._last_read_at = t0
self.produced += 1
self._times.append((self.produced, t0))
cutoff = t0 - FPS_WINDOW_S
while self._times and self._times[0][1] < cutoff:
self._times.popleft()
raw = getattr(self.sensor, "frame", None)
if raw is not None:
self._cache.append((self.produced, np.array(raw, copy=True)))
return image
def _pace(self) -> None:
"""Wait out the rest of the frame period, for a source with no reader."""
if not self.is_file or self.frame_period <= 0.0:
return
if self._last_read_at is None:
return
remaining = self.frame_period - (time.perf_counter() - self._last_read_at)
if remaining > CAPTURE_REPEAT_POLL_S:
time.sleep(min(remaining, self.frame_period * 0.5))
def _wait_for_a_new_frame(self) -> None:
"""Hold the read until the capture source has published another frame.
Retried with ``time.sleep(0)`` rather than a fixed sleep, because on Windows
even a 1 ms sleep costs a full 15.6 ms timer tick — one tick per frame is
most of a 30 fps frame period, and the camera's own thread is then starved of
the GIL for the rest of it. Yielding hands it back immediately and the next
poll happens as soon as the frame is there.
"""
if self.is_file or self.capture_seq is None:
return
#: When the wait started, and what the counter said then: the two together
#: are the only honest basis for the camera's own rate when this loop is
#: slower than the camera and skips a frame entirely.
self._repeat_check = (time.perf_counter(), int(self.capture_seq))
deadline = time.perf_counter() + self.repeat_wait_s
while True:
current = _capture_sequence(self.sensor)
if current is None or current != self.capture_seq:
self.capture_seq = current
return
if time.perf_counter() >= deadline:
# The camera has stopped publishing but the device is still open.
# Reading on keeps the session alive for the liveness watchdog in
# the consumer to see and report, instead of stalling here with the
# pipeline looking healthy.
return
time.sleep(0)
def raw_frame(self, seq: int) -> np.ndarray | None:
"""The pre-processing frame of produced frame ``seq``, or ``None``."""
for number, frame in reversed(self._cache):
if number == seq:
return frame
return None
def collect_perf(self, seq: int) -> float | None:
"""The collection time of produced frame ``seq``."""
for number, stamp in reversed(self._times):
if number == seq:
return stamp
return None
def capture_window(self, previous: tuple[float, int] | None) -> tuple[float, int]:
"""``(seconds, delivered frames)`` over the interval since ``previous``.
Both anchors have to be read together, and on this thread: the source's
counter advances on the capture thread whether or not a frame is taken, so
pairing a stale time with the current counter would measure the interval
between two *publishes* while crediting every frame delivered since an
earlier read — a rate that comes out at roughly double the camera's.
"""
if previous is None or self.capture_seq is None:
return 0.0, 0
t0, delivered0 = previous
span = time.perf_counter() - t0
delivered = int(self.capture_seq) - int(delivered0)
return (span, delivered) if span > 0.0 else (0.0, 0)
def capture_fps(self) -> float:
"""The capture source's own published rate, as a rate over a window.
Counted from the source's delivered-frame counter between two of this
producer's reads, against the wall-clock between them, and summed over the
last second so one scheduler-jittered interval does not become the reported
rate. Only a source with that counter can answer — a live camera. A video
file decodes a new frame per call, so it has no "the camera had it and we did
not" to measure.
"""
if self.capture_seq is None:
return 0.0
now = time.perf_counter()
span, delivered = self.capture_window(self._capture_mark)
self._capture_mark = (now, int(self.capture_seq))
if span <= 0.0 or delivered <= 0:
# Credit what the repeat wait already observed: when this loop cannot
# keep up, no frame is read at all until the counter moves again, and a
# read-anchored window would then credit a whole interval's frames to a
# fraction of it.
span, delivered = self._repeat_window()
if span > 0.0 and delivered > 0:
self._rate_samples.append((now, span, delivered))
cutoff = now - FPS_WINDOW_S
while len(self._rate_samples) > 1 and self._rate_samples[0][0] < cutoff:
self._rate_samples.popleft()
total_span = sum(sample[1] for sample in self._rate_samples)
total_frames = sum(sample[2] for sample in self._rate_samples)
return max(0.0, total_frames / total_span) if total_span > 0.0 else 0.0
def _repeat_window(self) -> tuple[float, int]:
"""``(seconds, frames)`` the repeat wait saw, and then forget it."""
previous = self._repeat_check
self._repeat_check = None
if previous is None or self._capture_mark is None or self.capture_seq is None:
return 0.0, 0
t0, seq0 = previous
span = self._capture_mark[0] - t0
frames = int(self.capture_seq) - int(seq0)
return (span, frames) if span > 0.0 and frames > 0 else (0.0, 0)
class _CaptureWindow:
"""The capture source's own published-frame rate, for the serial loop.
Measured from the source's delivered-frame counter, sampled once per frame: the
counter advances whether or not the loop takes a frame, so a rate below the
camera's own can be attributed. The concurrent path measures the same thing
inside its producer (``_FrameProducer.capture_fps``), where the counter can be
anchored to the wall clock it belongs to; this window is what the reference path
uses, so both report the rate on the same basis.
"""
def __init__(self, sensor) -> None:
self.sensor = sensor
self._times: list[tuple[float, int]] = []
def update(self, t0: float) -> float:
if self.sensor is None:
return 0.0
delivered = _capture_sequence(self.sensor)
if delivered is not None and (
not self._times or delivered != self._times[-1][1]
):
self._times.append((t0, delivered))
cutoff = t0 - FPS_WINDOW_S
self._times = [item for item in self._times if item[0] >= cutoff]
if len(self._times) < 2:
return 0.0
span = self._times[-1][0] - self._times[0][0]
published = self._times[-1][1] - self._times[0][1]
return max(0.0, published / span) if span > 0.0 else 0.0
def calibration_digest(path: str | Path) -> str:
@@ -493,8 +693,13 @@ class SdkAcquisitionWorker(RecordSinkMixin):
on_status: Callable[[str], None] | None = None,
on_calibration_applied: Callable[[str, str, int, float, int], None] | None = None,
on_reset_applied: Callable[[int, float, int], None] | None = None,
use_stream_pipeline: bool = True,
):
self._init_sinks()
#: The SDK's concurrent ``StreamPipeline`` is the acquisition path; the
#: serial chain is kept only so a change can be checked against it
#: frame-for-frame on the same input (``False`` selects the reference).
self._use_stream_pipeline = bool(use_stream_pipeline)
self._on_error = on_error
self._on_status = on_status
#: Called from the acquisition thread once new coefficients are actually
@@ -510,6 +715,10 @@ class SdkAcquisitionWorker(RecordSinkMixin):
#: Set while the operator has paused: the device stays open and the capture keeps
#: publishing, but nothing is computed and no packet is emitted.
self._pause = threading.Event()
#: The running session's ``StreamPipeline``, when there is one, so
#: ``pause``/``resume`` from the GUI thread reach the pipeline that is doing
#: the computing.
self._pipe = None
self._lock = threading.Lock()
self._latest: SdkPacket | None = None
self._session_id = 0
@@ -687,17 +896,43 @@ class SdkAcquisitionWorker(RecordSinkMixin):
This is what makes pause different from :meth:`stop`: the camera is not
released, so resuming is immediate and the device does not have to be
re-opened (and, on some boards, re-enumerated). While paused the loop keeps
draining the capture source — the disconnect watchdog must not fire on a
healthy camera just because an operator stopped looking at it — but it runs
no algorithm and emits no packet: no flow, no depth, no contact, and the
record channel sees a gap for the paused interval.
re-opened (and, on some boards, re-enumerated). The concurrent pipeline
holds at ``PauseLevel.CAPTURE_ONLY``, which is exactly that statement: the
read continues — so exposure, white balance and frame rate stay converged,
and the disconnect watchdog sees a live device — while the flow and HHD
stages are skipped and no snapshot is published: no flow, no depth, no
contact, and the record channel sees a gap for the paused interval.
"""
self._pause.set()
pipe = self._pipe
if pipe is not None:
try:
pipe.pause(self._pause_level())
except Exception: # noqa: BLE001 - the loop honours ``_pause`` anyway
logger.exception("Could not hold the pipeline for a pause")
def resume(self) -> None:
"""Start computing and publishing again, from the *newest* frame."""
"""Start computing and publishing again, from the *newest* frame.
The tracker is rebased, because a ``CAPTURE_ONLY`` hold read frames that
were never computed: keeping the pre-pause reference frame would report
the hold itself as a large deformation.
"""
self._pause.clear()
pipe = self._pipe
if pipe is not None:
try:
# Only meaningful for a pipeline that was actually held; the SDK
# ignores a resume when it is not paused.
pipe.resume(reset=True)
except Exception: # noqa: BLE001 - the loop honours ``_pause`` anyway
logger.exception("Could not release the pipeline for a resume")
@staticmethod
def _pause_level():
from orisys.pipeline import PauseLevel
return PauseLevel.CAPTURE_ONLY
@property
def paused(self) -> bool:
@@ -818,6 +1053,25 @@ class SdkAcquisitionWorker(RecordSinkMixin):
self._phase = "idle"
def _run_session(self, session: int, previous: threading.Thread | None) -> None:
opened = self._open_session(session, previous)
if opened is None:
return
mode, source, sensor, is_file = opened
if self._use_stream_pipeline:
# The recommended path: the SDK runs produce -> flow -> HHD on three
# threads, so throughput is bounded by the slowest stage rather than by
# their sum. Everything the app adds (session bookkeeping, the record
# sink, calibration reload, the drop counters) stays layered on top.
self._run_stream_pipeline(session, mode, source, sensor, is_file)
return
self._run_serial_loop(session, mode, source, sensor, is_file)
def _open_session(self, session: int, previous: threading.Thread | None):
"""Open the device and configure it; ``None`` when the session cannot run.
Shared by both acquisition paths, so a session's calibration identity,
filter state and status messages cannot drift between them.
"""
import orisys
if previous is not None and previous.is_alive():
@@ -829,7 +1083,7 @@ class SdkAcquisitionWorker(RecordSinkMixin):
mode = self._mode
source = self._video_source
if mode is None or source is None:
return
return None
is_file = _is_file_source(source)
try:
self._status(f"Opening Orisys Sensor ({mode.label()})…")
@@ -843,7 +1097,7 @@ class SdkAcquisitionWorker(RecordSinkMixin):
self._sensors[session] = sensor
if self._stop.is_set() or session != self._session_id:
self._close_sensor(session)
return
return None
# The filter chain lives in the SDK; the app only says what the
# operator asked for and then reads back what the device is doing.
with self._lock:
@@ -866,12 +1120,372 @@ class SdkAcquisitionWorker(RecordSinkMixin):
)
self._status(f"Streaming {mode.label()}")
except Exception as exc:
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
self._set_phase("idle")
return None
return mode, source, sensor, is_file
# ---------------------------------------------------------- concurrent path
def _run_stream_pipeline(
self, session: int, mode: FixedCameraMode, source, sensor, is_file: bool
) -> None:
"""Drive the SDK's concurrent ``StreamPipeline`` and build the packets.
The pipeline publishes one ``FrameSnapshot`` per frame it finishes; this
thread consumes them and owns everything the app layer adds. ``include=``
asks for exactly the arrays the app can display (see
:meth:`_stream_include`), because an array the snapshot does not carry is an
array the SDK did not have to copy.
"""
import queue as queue_module
from orisys.pipeline import PauseLevel, StreamPipeline
frame_period = 1.0 / max(1.0, float(mode.fps or 30.0))
producer = _FrameProducer(
sensor,
is_file=is_file,
# Only a live camera needs the wait; a file decodes the next frame per
# call and has no background reader to ask.
repeat_wait_s=CAMERA_DISCONNECT_TIMEOUT_S,
frame_period=frame_period,
)
snapshots: queue_module.Queue = queue_module.Queue(maxsize=64)
analysis = self._stream_contact(sensor)
#: The monotonic clock at start: every frame's ``mono_t`` is measured from it,
#: and the frame rate is measured between frame *collection* times.
mono_t0 = time.perf_counter()
frame_times: list[float] = []
#: Whether the source can report its own delivered-frame rate at all: a
#: file has no background reader to count published frames.
live_capture = _capture_sequence(sensor) is not None
frame_id = 0
capture_fps = 0.0
capture_dropped = 0
session_started = time.perf_counter()
last_frame_at = session_started
saw_frame = False
errors: list[str] = []
def merged_error() -> str:
return "; ".join(errors)
with self._lock:
self._phase = "running"
try:
pipe = StreamPipeline(
sensor,
producer,
contact=analysis,
include=self._stream_include(sensor),
on_frame=snapshots.put,
)
except Exception as exc: # noqa: BLE001 - reported, not swallowed
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
self._set_phase("idle")
return
# Published before ``start``: ``pause`` may be clicked while the device is
# still opening, and a hold that arrived then must not be lost.
self._pipe = pipe
try:
pipe.start()
if self._pause.is_set():
pipe.pause(self._pause_level())
while not self._stop.is_set() and session == self._session_id:
if pipe.paused:
# ``wait`` returns at once while paused, so a loop that only
# waited would spin a core for the whole hold. Sleep out the
# hold instead and check the stop flag between sleeps.
time.sleep(PAUSE_POLL_S)
continue
if pipe.error is not None:
# A stage raised. ``wait`` would re-raise it; this loop does not
# call ``wait``, so it is reported here instead.
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(pipe.error))
break
# How many frames the producer has finished, and how many this loop
# has taken: a ``wait`` is only worth making while the first is
# ahead. ``producer.produced`` also latches the moment the first
# read has happened, which is what keeps the "no frames" deadline
# below from racing the very first frame.
expect = max(producer.produced, frame_id)
snap = None
if expect > 0:
try:
snap = snapshots.get(timeout=0.05)
except queue_module.Empty:
snap = None
except Exception as exc: # noqa: BLE001 - a stage failure ends the session
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
break
now = time.perf_counter()
if snap is None:
# Nothing published, or nothing newer to hand over. The liveness
# rules below decide whether that is a dead camera or a slow
# start, and whether the pass is simply over.
pass
else:
saw_frame = True
last_frame_at = now
try:
packet = self._packet_from_stream_snapshot(
session=session,
sensor=sensor,
producer=producer,
live_capture=live_capture,
snap=snap,
frame_id=frame_id,
frame_period=frame_period,
is_file=is_file,
analysis=analysis,
frame_times=frame_times,
mono_t0=mono_t0,
capture_fps=capture_fps,
capture_dropped=capture_dropped,
compute_error=merged_error(),
)
except Exception as exc: # noqa: BLE001
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
break
capture_fps = packet.capture_fps
capture_dropped = packet.capture_dropped
if self._pause.is_set():
# The hold arrived after this frame was published but before
# it was built. "Pause the computation and the display"
# includes not handing the operator a packet they paused
# before, so the in-flight frame is dropped here.
continue
# A zeroing and new coefficients are applied by this thread at a
# frame boundary, then reported with the frame and time they took
# effect at — the same confirmation rule as the serial loop.
try:
self._consume_pending_calibration(
sensor, frame_id + 1, float(packet.mono_t or 0.0)
)
except Exception as exc: # noqa: BLE001
errors.append(f"calibration rejected: {type(exc).__name__}: {exc}")
packet.valid = False
packet.stale_forces = True
packet.forces_valid = False
packet.invalid_reason = (
packet.invalid_reason or errors[-1]
)
if self._on_error is not None and session == self._session_id:
self._on_error(errors[-1])
try:
self._consume_pending_reset(
sensor, frame_id + 1, float(packet.mono_t or 0.0)
)
except Exception as exc: # noqa: BLE001
message = f"reset failed: {type(exc).__name__}: {exc}"
if self._on_error is not None and session == self._session_id:
self._on_error(message)
with self._lock:
self._latest = packet
self._phase = "running"
# Record channel: every published packet, exactly as the serial
# loop emitted them.
self._emit(packet)
frame_id += 1
# Liveness. The pipeline reports "input ended" and "no newer frame"
# with the same ``None``, so these facts are what tell a finished
# pass, a dead camera and a slow start apart.
producer_done = not pipe.running and producer.produced <= frame_id
if not saw_frame:
if is_file and (now - session_started) >= FILE_FIRST_FRAME_TIMEOUT_S:
message = (
f"this video produced no frames in "
f"{FILE_FIRST_FRAME_TIMEOUT_S:.0f} s: {source} may be "
"unreadable, truncated, or in a codec this build cannot decode"
)
if self._on_error is not None and session == self._session_id:
self._on_error(message)
self._set_end_reason("error")
break
if producer_done:
# The read itself ended and nothing was ever measured: an
# unreadable file, or a device that delivered nothing. Say
# so now instead of waiting out the deadline above.
if self._on_error is not None and session == self._session_id:
self._on_error(
f"this source produced no frames: {source} may be "
"unreadable, truncated, or in a codec this build cannot "
"decode"
)
self._set_end_reason("error" if is_file else "disconnected")
break
elif producer_done:
# Every frame the producer made has become a packet and there is
# no newer one: the clean end-of-input the serial loop reported.
self._set_end_reason("ended")
break
elif not is_file and (now - last_frame_at) >= CAMERA_DISCONNECT_TIMEOUT_S:
# The producer is still reading, but a live camera has published
# nothing for a whole timeout: the device is gone.
self._set_end_reason("disconnected")
break
finally:
self._pipe = None
try:
pipe.pause(PauseLevel.BOTH)
except Exception: # noqa: BLE001 - a failed pause must not block teardown
logger.debug("Could not hold the pipeline while stopping", exc_info=True)
try:
pipe.stop(timeout=STOP_JOIN_TIMEOUT_S)
except Exception as exc: # noqa: BLE001 - a stage error is reported here
if self._stop.is_set():
logger.debug("pipeline stopped with %r", exc, exc_info=True)
else:
self._set_end_reason("error")
if self._on_error is not None and session == self._session_id:
self._on_error(str(exc))
if self._stop.is_set():
self._set_end_reason("stopped")
elif saw_frame and self._end_reason is None:
# The producer returned end-of-input and every published frame was
# consumed: that is the same clean end the serial loop reported.
self._set_end_reason("ended")
self._set_phase("idle")
self._status("Acquisition stopped")
def _stream_include(self, sensor) -> tuple[str, ...]:
"""The arrays the app can actually display, so the SDK copies only those.
``image`` is the tactile frame the 2D canvas paints, ``flow`` and ``depth``
are the two quantities the 3D pane offers, and the contact arrays are what
its overlay and the 2D contact arrow draw. Every one of them was read
unconditionally before this migration; a sensor with no contact analysis
does not pay for the contact channels.
"""
fields = ["image", "flow", "depth"]
if self._stream_contact(sensor):
fields += ["centroid", "contour"]
return tuple(fields)
def _stream_contact(self, sensor) -> bool:
"""Whether this device's snapshot can carry a contact result at all."""
return callable(getattr(sensor, "compute_contact", None))
def _packet_from_stream_snapshot(
self,
*,
session: int,
sensor,
producer: _FrameProducer,
live_capture: bool,
snap,
frame_id: int,
frame_period: float,
is_file: bool,
analysis: bool,
frame_times: list[float],
mono_t0: float,
capture_fps: float,
capture_dropped: int,
compute_error: str,
):
"""One :class:`SdkPacket` from one finished snapshot, plus app facts.
Everything measured comes from the snapshot through
:func:`~orisys_app_shared.acquisition.models.from_snapshot`; everything
app-owned is computed here, from the same evidence the serial loop used.
"""
# ``produced`` counts the reads the producer has made, and the pipeline
# numbers its snapshots from zero, so produced frame N carries snapshot
# sequence N - 1.
produced_seq = frame_id + 1
t_collect = producer.collect_perf(produced_seq) or time.perf_counter()
mono = (
# A file has the timeline of the *video*, not of how long the pass took:
# recomputing a 5 s recording at 60 fps would otherwise land on a
# 0-2 s axis and could not be compared with the forces.csv it is
# being checked against.
frame_id * frame_period
if is_file
else max(0.0, t_collect - mono_t0)
)
# The rate is measured between *collections*, so it is the rate the frames
# arrived at rather than the loop's own cost dressed up as a frame rate.
# The SDK measures its own rate on the stage that publishes the snapshot;
# this window is what lets the packet's ``fps`` be stated on the same basis
# as the serial loop's, which measured it from the capture time.
frame_times.append(t_collect)
cutoff = t_collect - FPS_WINDOW_S
del frame_times[
: next(
(
index
for index, stamp in enumerate(frame_times)
if stamp >= cutoff
),
len(frame_times),
)
]
span = frame_times[-1] - frame_times[0] if len(frame_times) > 1 else 0.0
fps = (len(frame_times) - 1) / span if span > 0.0 else 0.0
if live_capture:
measured = producer.capture_fps()
if measured > 0.0:
capture_fps = measured
# What the capture source published that nobody took. The source counts
# these itself, which is the camera's own evidence; a file source has no
# such counter, so the pipeline's own backlog is the only honest answer.
taken = _capture_dropped(sensor)
if taken is not None:
capture_dropped = int(taken)
elif producer.produced > produced_seq:
capture_dropped = int(producer.produced - produced_seq)
with self._lock:
self._measured_capture_fps = capture_fps
raw_frame = producer.raw_frame(produced_seq)
if raw_frame is None:
# The pipeline had already moved on: the frame that produced these
# forces is unavailable, so the packet carries the newest frame the
# sensor has rather than an image it never measured.
raw_frame = getattr(sensor, "frame", None)
return _packet_from_snapshot(
snap,
sensor,
session_id=session,
frame_id=frame_id,
mono_t=mono,
fps=fps,
capture_fps=capture_fps,
capture_dropped=capture_dropped,
raw_frame=raw_frame,
valid_mask=getattr(sensor, "valid_mask", None),
contact_analysed=analysis,
contact_found=bool(getattr(sensor, "centroid_found", False)),
compute_error=compute_error,
calibration_version=self._calibration_version,
valid=not compute_error,
)
# ------------------------------------------------------------- serial path
def _run_serial_loop(
self, session: int, mode: FixedCameraMode, source, sensor, is_file: bool
) -> None:
"""The single-threaded chain, kept whole for the packet-diff check.
``use_stream_pipeline=False`` selects it: produce, decompose and read the
forces back to back on this one thread, which bounds throughput by the sum
of the three stages. It is the reference the concurrent path is measured
against, and it is not the path a session uses.
"""
frame_times: list[float] = []
saw_frame = False
last_frame_mono = 0.0
shared/tests/test_stream_pipeline_acquisition.py
+418
View File
@@ -0,0 +1,418 @@
"""The concurrent acquisition path: no left-behind threads, and a stop/start cycle.
The worker drives the SDK's ``StreamPipeline``, which runs three threads of its own
(``orisys-pipeline-produce``, ``-flow``, ``-hhd``). Those threads own the sensor's
buffers and tracker state, so a session that leaves one running would corrupt the
next session's reference frame — the failure this test exists to catch.
No camera and no sensor bundle are needed: the fake ``orisys`` module below serves
``Sensor`` and ``finger.FingerRuntime`` the way the real package does, including the
compute methods ``StreamPipeline`` calls.
"""
from __future__ import annotations
import sys
import threading
import time
import types
from pathlib import Path
import numpy as np
import pytest
APP_ROOT = Path(__file__).resolve().parents[2]
BOX_CONFIG = APP_ROOT / "apps" / "studio" / "config" / "sensors" / "box" / "sensor.json"
FINGER_CONFIG = APP_ROOT / "apps" / "studio" / "config" / "sensors" / "finger" / "sensor.json"
PIPELINE_THREADS = ("orisys-pipeline-produce", "orisys-pipeline-flow", "orisys-pipeline-hhd")
#: Bound by :func:`_install` from the real SDK, before the fake device replaces the
#: ``orisys`` name.
StreamPipeline = None
FrameSnapshot = None
def _pipeline_threads() -> list[str]:
return [t.name for t in threading.enumerate() if t.name in PIPELINE_THREADS]
class _Info:
FNORMAL = "FNORMAL"
FSHEARX = "FSHEARX"
FSHEARY = "FSHEARY"
FNORMAL_RAW = "FNORMAL_RAW"
FSHEARX_RAW = "FSHEARX_RAW"
FSHEARY_RAW = "FSHEARY_RAW"
VRAW = "VRAW"
VNORMAL = "VNORMAL"
VSHEAR = "VSHEAR"
DEPTH = "DEPTH"
CENTROID = "CENTROID"
CONTOUR = "CONTOUR"
IMG = "IMG"
class _Source:
"""A capture source with a running background reader, so repeats are counted."""
class _Status:
def __init__(self) -> None:
self.last_seq = 0
self.dropped_frames = 0
self.width = 1920
self.height = 1080
self.fps = 30.0
self.fourcc = "MJPG"
class _Config:
def __init__(self) -> None:
self.width = 1920
self.height = 1080
self.fps = 30.0
self.fourcc = "MJPG"
self.source = 0
def __init__(self) -> None:
self.status = self._Status()
self.config = self._Config()
self.running = True
def read_latest(self, **kwargs):
return True, np.zeros((8, 8, 3), dtype=np.uint8)
def is_open(self) -> bool:
return True
def start_background(self) -> None:
self.running = True
def stop_background(self) -> None:
self.running = False
def open(self) -> None:
return None
def close(self) -> None:
self.running = False
class _Sensor:
"""A device ``StreamPipeline`` can drive."""
def __init__(self, *args, **kwargs) -> None:
self.info = _Info()
self.configured: dict = {}
self.connected = True
self.capture_source = _Source()
self._capture_source = self.capture_source
self.img = np.zeros((8, 8), dtype=np.uint8)
self.frame = np.zeros((8, 8, 3), dtype=np.uint8)
self.flow = None
self.vnormal = None
self.vshear = None
self.depth_map = None
self.centroid = None
self.centroid_found = False
self.contour = None
self.compute_contact_valid = False
self.valid_mask = None
self.img_size_raw = (1920, 1080)
self.force_calibrated = True
self.calibration = None
self.fnormal = 1.0
self.fshearx = 0.0
self.fsheary = 0.0
self.ftor = 0.0
self.force_unit = "N"
self._force_raw = (9.0, 0.5, -0.5)
self._frames = 0
def configure(self, **kwargs):
self.configured = kwargs
def read_info(self, *names):
values = {
_Info.FNORMAL: 1.0,
_Info.FSHEARX: 0.0,
_Info.FSHEARY: 0.0,
_Info.FNORMAL_RAW: 9.0,
_Info.FSHEARX_RAW: 0.5,
_Info.FSHEARY_RAW: -0.5,
}
return tuple(values.get(str(name), None) for name in names)
def compute_flow(self, img=None):
self.flow = np.zeros((8, 8, 2), dtype=np.float32)
def compute_hhd(self, flow=None):
self._force_raw = (9.0, 0.5, -0.5)
def compute_deformation(self, decompose=True, check_motion=False, **kwargs):
self.compute_flow()
if decompose:
self.compute_hhd()
return None
def compute_contact(self, vnormal=None):
self.compute_contact_valid = True
return {}
def get_img(self, **kwargs):
self._frames += 1
return self.img.copy()
def warmup_deformation(self):
return True
def reset(self):
return None
def disconnect(self):
self.connected = False
class _Runtime:
"""A stand-in for ``FingerRuntime`` with the API the worker uses."""
def __init__(self, *args, **kwargs) -> None:
self.tactile = types.SimpleNamespace(
raw_sensor=_Sensor(),
reset=lambda: None,
)
@classmethod
def open(cls, **kwargs):
return cls(**kwargs)
@property
def valid_mask(self):
return np.ones((8, 8), dtype=bool)
def read_source(self):
return True, np.zeros((8, 8, 3), dtype=np.uint8)
def process_frame(self, frame, *, color=False):
return np.zeros((8, 8), dtype=np.uint8)
def warmup(self, *, color=False):
return True, np.zeros((8, 8), dtype=np.uint8)
def close(self):
return None
def _install() -> dict:
"""Replace ``orisys`` with the fakes, keeping the real ``StreamPipeline``.
``StreamPipeline`` and ``FrameSnapshot`` are the SDK under test, so they are
bound before the swap: the fake stands in for the device, not for the pipeline.
The submodules the workers import lazily are bound too, because a module object
placed in ``sys.modules`` is not a package and cannot resolve them itself.
"""
import orisys.calibration # noqa: F401 - bound so the workers can import it
import orisys.finger # noqa: F401
import orisys.pipeline # noqa: F401
global StreamPipeline, FrameSnapshot
from orisys.pipeline import FrameSnapshot as _Snapshot
from orisys.pipeline import StreamPipeline as _Pipeline
StreamPipeline, FrameSnapshot = _Pipeline, _Snapshot
saved = {name: sys.modules.get(name) for name in ("orisys", "orisys.finger")}
module = types.ModuleType("orisys")
module.__version__ = "fake"
module.Sensor = _Sensor # type: ignore[attr-defined]
finger_module = types.ModuleType("orisys.finger")
finger_module.FingerRuntime = _Runtime # type: ignore[attr-defined]
module.finger = finger_module # type: ignore[attr-defined]
sys.modules["orisys"] = module
sys.modules["orisys.finger"] = finger_module
return saved
def _restore(saved: dict) -> None:
for name, original in saved.items():
if original is None:
sys.modules.pop(name, None)
else:
sys.modules[name] = original
def _worker(kind: str):
from orisys_app_shared.acquisition.finger_worker import FingerAcquisitionWorker
from orisys_app_shared.acquisition.sdk_worker import SdkAcquisitionWorker
return FingerAcquisitionWorker() if kind == "finger" else SdkAcquisitionWorker()
def _config(kind: str) -> Path:
return FINGER_CONFIG if kind == "finger" else BOX_CONFIG
@pytest.mark.parametrize("kind", ["box", "finger"])
def test_a_stopped_session_leaves_no_pipeline_threads(kind):
"""``stop`` must join the three stages: they own the sensor's tracker state."""
packets: list = []
saved = _install()
worker = _worker(kind)
try:
worker.add_sink(packets.append)
worker.start("0", config_ref=_config(kind))
deadline = time.perf_counter() + 5.0
while time.perf_counter() < deadline and not packets:
time.sleep(0.02)
assert packets, "the session published nothing to stop"
assert _pipeline_threads(), "the fake session did not start the pipeline"
worker.stop(wait=True)
deadline = time.perf_counter() + 5.0
while time.perf_counter() < deadline and _pipeline_threads():
time.sleep(0.02)
assert _pipeline_threads() == [], (
f"{kind}: pipeline threads outlived stop(): {_pipeline_threads()}"
)
finally:
worker.stop(wait=True)
_restore(saved)
@pytest.mark.parametrize("kind", ["box", "finger"])
def test_a_stop_start_cycle_works_and_leaves_nothing_behind(kind):
"""One worker, two sessions: the second must run and the first must be gone."""
saved = _install()
worker = _worker(kind)
try:
for session in (1, 2):
packets: list = []
worker.add_sink(packets.append)
worker.start("0", config_ref=_config(kind))
deadline = time.perf_counter() + 5.0
while time.perf_counter() < deadline and not packets:
time.sleep(0.02)
assert packets, f"session {session} published nothing"
assert worker.latest() is packets[-1]
worker.stop(wait=True)
assert not worker.is_running
assert worker.phase == "idle"
worker.remove_sink(packets.append)
deadline = time.perf_counter() + 5.0
while time.perf_counter() < deadline and _pipeline_threads():
time.sleep(0.02)
assert _pipeline_threads() == [], _pipeline_threads()
finally:
worker.stop(wait=True)
_restore(saved)
def test_the_packet_carries_every_field_the_ui_reads():
"""The mapping from a snapshot: forces, raws, arrays, ids and provenance."""
import orisys_app_shared.acquisition.models as models
saved = _install()
try:
snapshot = FrameSnapshot(
seq=7,
timestamp=1000.5,
age_ms=12.0,
fps=30.0,
unit="N",
normal_force=1.25,
shear_x=0.5,
shear_y=-0.5,
normal_force_raw=9.0,
shear_x_raw=0.4,
shear_y_raw=-0.4,
torque=0.0,
image=np.zeros((4, 4), dtype=np.uint8),
flow=np.zeros((4, 4, 2), dtype=np.float32),
depth=np.ones((4, 4), dtype=np.float32),
centroid=np.array([2.0, 3.0]),
contour=None,
produce_ms=1.0,
flow_ms=2.0,
hhd_ms=3.0,
)
packet = models.from_snapshot(
snapshot,
_Sensor(),
session_id=3,
frame_id=7,
mono_t=0.25,
capture_fps=30.0,
capture_dropped=2,
raw_frame=np.zeros((8, 8, 3), dtype=np.uint8),
valid_mask=np.ones((4, 4), dtype=bool),
contact_analysed=True,
contact_found=True,
)
assert packet.frame_id == 7
assert packet.session_id == 3
assert packet.mono_t == pytest.approx(0.25)
assert packet.fps == 30.0
# The stage costs add up to the frame's own cost; the SDK measures them.
assert packet.process_ms == pytest.approx(6.0)
assert (packet.normal_force, packet.shear_x, packet.shear_y) == (1.25, 0.5, -0.5)
assert (
packet.normal_force_raw,
packet.shear_x_raw,
packet.shear_y_raw,
) == (9.0, 0.4, -0.4)
assert packet.force_unit == "N"
assert packet.capture_fps == 30.0
assert packet.capture_dropped == 2
assert packet.processed_frame.shape == (4, 4, 3)
assert packet.raw_frame.shape == (8, 8, 3)
assert packet.flow is not None and packet.depth is not None
assert packet.valid_mask is not None
assert packet.flow_valid is True and packet.flow_frame_id == 7
assert packet.depth_valid is True and packet.depth_frame_id == 7
assert packet.forces_valid is True and packet.valid is True
assert packet.contact is not None
assert packet.contact.centroid_uv_xy == (2.0, 3.0)
assert packet.contact.found is True and packet.contact.frame_id == 7
finally:
_restore(saved)
def test_a_failed_chain_marks_the_frame_rather_than_claiming_its_forces():
"""A compute error must leave the frame's forces unclaimable, not silently trusted."""
import orisys_app_shared.acquisition.models as models
saved = _install()
try:
snapshot = FrameSnapshot(
seq=0,
timestamp=1000.0,
age_ms=1.0,
fps=0.0,
unit="raw",
normal_force=1.0,
shear_x=0.0,
shear_y=0.0,
normal_force_raw=1.0,
shear_x_raw=0.0,
shear_y_raw=0.0,
torque=0.0,
image=np.zeros((4, 4), dtype=np.uint8),
flow=None,
depth=None,
centroid=np.array(0),
contour=None,
)
packet = models.from_snapshot(
snapshot,
_Sensor(),
frame_id=0,
contact_analysed=True,
contact_found=False,
compute_error="compute: ValueError: boom",
)
assert packet.valid is False
assert packet.stale_forces is True
assert packet.forces_valid is False
assert packet.flow_valid is False and packet.depth_valid is False
assert packet.contact is not None
assert packet.contact.found is False and packet.contact.valid is False
assert packet.contact.centroid_uv_xy is None
finally:
_restore(saved)
shared/tests/test_worker_frame_rate.py
+58 -3
View File
@@ -109,15 +109,42 @@ CREATED: list = []
class _Sensor:
"""A device the ``StreamPipeline`` can drive as well as the serial loop.
``Sensor`` is what the SDK pipeline calls: ``compute_flow`` then
``compute_hhd`` then, with ``contact=True``, ``compute_contact``. The fake
device has no optics, so the compute methods only have to leave the arrays the
snapshot is built from in a shape the SDK's contract describes.
"""
def __init__(self, *args, **kwargs) -> None:
import numpy as np
self.configured: dict = {}
self.connected = True
self.info = _Info()
self.capture_source = _CaptureSource()
self.frame = np.zeros((8, 8, 3), dtype=np.uint8)
self.info = _Info()
self.img = np.zeros((8, 8), dtype=np.uint8)
self.flow = None
self.vnormal = None
self.vshear = None
self.depth_map = None
self.centroid = None
self.centroid_found = False
self.contour = None
self.compute_contact_valid = False
self.valid_mask = None
self.img_size_raw = (1920, 1080)
self.force_calibrated = True
self.calibration = None
self.fnormal = 1.5
self.fshearx = 0.25
self.fsheary = -0.25
self.ftor = 0.0
self.force_unit = "N"
#: The unfiltered integrator pair a real sensor keeps after ``compute_hhd``.
self._force_raw = (15.0, 2.5, -2.5)
CREATED.append(self)
def configure(self, **kwargs):
@@ -135,14 +162,42 @@ class _Sensor:
}
return tuple(values.get(str(name), 0.0) for name in names)
def get_img(self):
def compute_flow(self, img=None):
import numpy as np
self.flow = np.zeros((8, 8, 2), dtype=np.float32)
def compute_hhd(self, flow=None):
# A real ``compute_hhd`` refreshes the unfiltered integrator pair; keeping
# the values here is what makes a snapshot carry a force reading at all.
self._force_raw = (15.0, 2.5, -2.5)
def compute_deformation(self, decompose=True, check_motion=False, **kwargs):
self.compute_flow()
if decompose:
self.compute_hhd()
return None
def compute_contact(self, vnormal=None):
self.compute_contact_valid = True
return {}
def read_latest(self, **kwargs):
"""``CaptureSource.read_latest``: the newest frame, or nothing yet."""
self.capture_source.take()
return True, self.frame
def get_img(self, **kwargs):
# ``read_latest``: hand back the newest frame immediately, the same one
# again when nothing new has arrived. A test can make this cost time, so
# the loop can be put under a pipeline slower than the camera.
if PROCESS_COST_S > 0:
time.sleep(PROCESS_COST_S)
self.capture_source.take()
return self.frame
return self.img.copy()
def reset(self):
return None
def disconnect(self):
self.connected = False