Add RFG UDP source to rph_radar
This commit is contained in:
Ayzen
@@ -0,0 +1,285 @@
|
||||
"""Background reader publishing the latest completed RadioPhotonic radar (RPH) sweep.
|
||||
|
||||
The source (E-502 collector TTY or RFG board UDP socket) emits sweeps continuously,
|
||||
faster than callers invoke :meth:`RphRadarService.acquire`. A daemon thread drains
|
||||
the descriptor non-stop, feeds it to :class:`RphRadarStreamParser` or
|
||||
:class:`RfgSweepAssembler`, and stores the
|
||||
most recent :class:`RawSweep` in a single-slot mailbox. :meth:`read_sweep`
|
||||
returns the freshest sweep; if a newer one arrives before the consumer reads, it
|
||||
overwrites the previous unread value — by design, since consumers always want the
|
||||
latest data. Parser/stream errors are captured and re-raised on the consumer
|
||||
thread (fail-fast; the supervisor relaunches a clean collector).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from contextlib import suppress
|
||||
from dataclasses import dataclass, field
|
||||
import errno
|
||||
import logging
|
||||
import os
|
||||
import select
|
||||
import socket
|
||||
import struct
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
from python_app.hardware_full.rph_radar.protocol import RawSweep, RfgSweepAssembler, RphRadarStreamParser
|
||||
from python_app.models.run_config_validation import parse_udp_bind
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Large reads keep up with bursty CDC-ACM/PTY writers without raising the syscall
|
||||
# rate; 64 KiB matches the typical Linux PTY buffer size.
|
||||
_READ_CHUNK_BYTES = 65536
|
||||
# select() poll interval — short enough to react to close() promptly, long enough
|
||||
# that idle CPU stays near zero.
|
||||
_READ_POLL_INTERVAL_S = 0.1
|
||||
# Kernel receive stamps (Linux SO_TIMESTAMPNS == SCM_TIMESTAMPNS on x86_64 and aarch64,
|
||||
# a 64-bit timespec) are immune to the reader thread's scheduling stalls.
|
||||
_SO_TIMESTAMPNS = 35
|
||||
_TIMESPEC = struct.Struct("qq")
|
||||
# A REALTIME step between the kernel stamp and the clock read would mistranslate it;
|
||||
# NTP slewing moves the REALTIME-monotonic offset by microseconds only.
|
||||
_CLOCK_JUMP_NS = 1_000_000
|
||||
|
||||
|
||||
def raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None:
|
||||
"""Raise if the external collector process has exited."""
|
||||
if process is None:
|
||||
return
|
||||
return_code = process.poll()
|
||||
if return_code is not None:
|
||||
raise RuntimeError(f"RadioPhotonic radar (RPH) collector exited with code {return_code}")
|
||||
|
||||
|
||||
def _clock_offset_ns() -> int:
|
||||
return time.clock_gettime_ns(time.CLOCK_REALTIME) - time.monotonic_ns()
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class RphRadarStreamReader:
|
||||
"""Daemon-thread reader of the collector TTY, or of the RFG UDP stream when ``rfg`` is set."""
|
||||
|
||||
endpoint: str # TTY path, or HOST:PORT bind address for the RFG stream
|
||||
assembler: RfgSweepAssembler | None = None
|
||||
_socket: socket.socket | None = field(init=False, default=None, repr=False)
|
||||
_clock_offset_ns: int | None = field(init=False, default=None, repr=False)
|
||||
_fd: int | None = field(init=False, default=None, repr=False)
|
||||
_thread: threading.Thread | None = field(init=False, default=None, repr=False)
|
||||
_stop_event: threading.Event = field(init=False, default_factory=threading.Event, repr=False)
|
||||
_mailbox_cv: threading.Condition = field(init=False, default_factory=threading.Condition, repr=False)
|
||||
_latest_sweep: RawSweep | None = field(init=False, default=None, repr=False)
|
||||
# Latest clean sweep per switch combination, for the switch-aware collector.
|
||||
# read_sweep() ignores this and serves the single newest sweep (calibration /
|
||||
# non-switch mode); an optional combo selects its own slot.
|
||||
_combo_slots: dict[tuple[int, int], RawSweep] = field(init=False, default_factory=dict, repr=False)
|
||||
_reader_error: Exception | None = field(init=False, default=None, repr=False)
|
||||
_published_count: int = field(init=False, default=0, repr=False)
|
||||
last_dirty_size: int | None = field(init=False, default=None)
|
||||
|
||||
def open(self) -> None:
|
||||
"""Open the TTY or bind the UDP socket and start the background reader thread."""
|
||||
if self._fd is not None:
|
||||
return
|
||||
if self.assembler is None:
|
||||
self._fd = os.open(self.endpoint, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
|
||||
else:
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
# Taken before bind, so a REALTIME step while the first datagrams queue is still seen.
|
||||
offset_ns = _clock_offset_ns()
|
||||
try:
|
||||
# ~10k datagrams/s: a deep buffer rides out receiver stalls; the kernel caps it at rmem_max.
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 4 << 20)
|
||||
sock.setsockopt(socket.SOL_SOCKET, _SO_TIMESTAMPNS, 1)
|
||||
sock.bind(parse_udp_bind(self.endpoint))
|
||||
sock.setblocking(False)
|
||||
except OSError:
|
||||
sock.close()
|
||||
raise
|
||||
self._socket, self._fd, self._clock_offset_ns = sock, sock.fileno(), offset_ns
|
||||
logger.info("RFG socket receive buffer: %d bytes", sock.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF))
|
||||
self._stop_event.clear()
|
||||
self._latest_sweep = None
|
||||
self._combo_slots = {}
|
||||
self._reader_error = None
|
||||
self._published_count = 0
|
||||
self._thread = threading.Thread(
|
||||
target=self._reader_loop,
|
||||
name=f"rph-radar-reader[{self.endpoint}]",
|
||||
daemon=True,
|
||||
)
|
||||
try:
|
||||
self._thread.start()
|
||||
except Exception:
|
||||
self._thread = None
|
||||
self._close_descriptor()
|
||||
raise
|
||||
logger.info("RadioPhotonic radar (RPH) reader started on %s", self.endpoint)
|
||||
|
||||
def close(self) -> None:
|
||||
"""Stop the reader thread and close the descriptor."""
|
||||
logger.debug("Stopping RadioPhotonic radar (RPH) reader on %s", self.endpoint)
|
||||
self._stop_event.set()
|
||||
with self._mailbox_cv:
|
||||
self._mailbox_cv.notify_all()
|
||||
if self._thread is not None:
|
||||
self._thread.join(timeout=1.0)
|
||||
if self._thread.is_alive():
|
||||
logger.warning("RadioPhotonic radar (RPH) reader thread did not stop within 1.0s")
|
||||
self._thread = None
|
||||
self._close_descriptor()
|
||||
self._latest_sweep = None
|
||||
self._combo_slots = {}
|
||||
self._reader_error = None
|
||||
|
||||
def _close_descriptor(self) -> None:
|
||||
fd, sock, self._fd, self._socket = self._fd, self._socket, None, None
|
||||
if sock is not None:
|
||||
sock.close()
|
||||
elif fd is not None:
|
||||
os.close(fd)
|
||||
|
||||
@property
|
||||
def published_count(self) -> int:
|
||||
"""Total number of sweeps the reader thread has produced."""
|
||||
with self._mailbox_cv:
|
||||
return self._published_count
|
||||
|
||||
def read_sweep(
|
||||
self,
|
||||
*,
|
||||
timeout_s: float,
|
||||
combo: tuple[int, int] | None = None,
|
||||
process: subprocess.Popen[bytes] | None = None,
|
||||
) -> RawSweep:
|
||||
"""Wait for and return the next published sweep.
|
||||
|
||||
Raises :class:`TimeoutError` if none arrives within ``timeout_s``,
|
||||
:class:`RuntimeError` if the collector process exited, and re-raises any
|
||||
error caught by the reader thread.
|
||||
"""
|
||||
if self._thread is None:
|
||||
raise RuntimeError("RadioPhotonic radar (RPH) reader is not open")
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
with self._mailbox_cv:
|
||||
while True:
|
||||
if self._stop_event.is_set():
|
||||
raise RuntimeError("RadioPhotonic radar (RPH) reader is closed")
|
||||
# Deliver a pending sweep first: if the reader both published a
|
||||
# sweep and then died, the consumer still sees the good data and
|
||||
# only meets the error on the next call.
|
||||
sweep = self._latest_sweep if combo is None else self._combo_slots.pop(combo, None)
|
||||
if sweep is not None:
|
||||
if combo is None:
|
||||
self._latest_sweep = None
|
||||
return sweep
|
||||
if self._reader_error is not None:
|
||||
raise self._reader_error
|
||||
raise_if_process_exited(process)
|
||||
remaining_s = deadline - time.monotonic()
|
||||
if remaining_s <= 0.0:
|
||||
rejected = "" if self.last_dirty_size is None else f"; last rejected sweep had {self.last_dirty_size} points"
|
||||
raise TimeoutError(
|
||||
f"Timed out waiting for RadioPhotonic radar (RPH) sweep after {float(timeout_s):.3f}s{rejected}"
|
||||
)
|
||||
self._mailbox_cv.wait(timeout=min(_READ_POLL_INTERVAL_S, remaining_s))
|
||||
|
||||
def _reader_loop(self) -> None:
|
||||
"""Drain the descriptor, parse sweeps, and publish each completed one until stop."""
|
||||
parser = RphRadarStreamParser()
|
||||
try:
|
||||
while not self._stop_event.is_set():
|
||||
if self.assembler is None:
|
||||
chunk = self._read_available()
|
||||
sweeps = parser.feed(chunk) if chunk else []
|
||||
else:
|
||||
datagram = self._receive_datagram()
|
||||
sweeps = self.assembler.feed(*datagram) if datagram is not None else []
|
||||
for sweep in sweeps:
|
||||
self._publish_sweep(sweep)
|
||||
except Exception as exc: # noqa: BLE001 — surfaced to the consumer via read_sweep
|
||||
logger.exception("RadioPhotonic radar (RPH) reader thread failed on %s", self.endpoint)
|
||||
self._publish_error(exc)
|
||||
|
||||
def _read_available(self) -> bytes:
|
||||
"""Block on ``select`` up to the poll interval; return new bytes (maybe empty).
|
||||
|
||||
Returns ``b""`` when no data is ready yet or a stop was requested; raises
|
||||
:class:`RuntimeError` on stream close or an unrecoverable read error.
|
||||
"""
|
||||
fd = self._fd
|
||||
if fd is None or self._stop_event.is_set():
|
||||
return b""
|
||||
try:
|
||||
readable, _, _ = select.select([fd], [], [], _READ_POLL_INTERVAL_S)
|
||||
except InterruptedError:
|
||||
return b""
|
||||
if not readable:
|
||||
return b""
|
||||
try:
|
||||
chunk = os.read(fd, _READ_CHUNK_BYTES)
|
||||
except BlockingIOError:
|
||||
return b""
|
||||
except OSError as exc:
|
||||
if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}:
|
||||
return b""
|
||||
raise RuntimeError(f"Failed to read RadioPhotonic radar (RPH) source `{self.endpoint}`: {exc}") from exc
|
||||
if not chunk:
|
||||
raise RuntimeError(f"RadioPhotonic radar (RPH) source `{self.endpoint}` closed while reading")
|
||||
return chunk
|
||||
|
||||
def _receive_datagram(self) -> tuple[bytes, int] | None:
|
||||
"""Return one RFG datagram with its kernel receive time on the monotonic clock."""
|
||||
sock = self._socket
|
||||
if sock is None or self._stop_event.is_set():
|
||||
return None
|
||||
try:
|
||||
readable, _, _ = select.select([sock], [], [], _READ_POLL_INTERVAL_S)
|
||||
if not readable:
|
||||
return None
|
||||
datagram, ancillary, _, _ = sock.recvmsg(_READ_CHUNK_BYTES, socket.CMSG_SPACE(_TIMESPEC.size))
|
||||
except (BlockingIOError, InterruptedError):
|
||||
return None
|
||||
offset_ns, now_ns = _clock_offset_ns(), time.monotonic_ns()
|
||||
stamps = [data for level, kind, data in ancillary if level == socket.SOL_SOCKET and kind == _SO_TIMESTAMPNS]
|
||||
if not stamps:
|
||||
raise RuntimeError("RFG datagram arrived without a kernel receive timestamp")
|
||||
seconds, nanoseconds = _TIMESPEC.unpack(stamps[0])
|
||||
received_ns = seconds * 1_000_000_000 + nanoseconds - offset_ns
|
||||
previous, self._clock_offset_ns = self._clock_offset_ns, offset_ns
|
||||
if previous is None or abs(offset_ns - previous) > _CLOCK_JUMP_NS or received_ns > now_ns:
|
||||
# Queued stamps may predate the step: drop them and the sweep in progress.
|
||||
with suppress(BlockingIOError):
|
||||
while True:
|
||||
sock.recv(_READ_CHUNK_BYTES)
|
||||
assert self.assembler is not None
|
||||
self.assembler.reset()
|
||||
return None
|
||||
return datagram, received_ns
|
||||
|
||||
def _publish_sweep(self, sweep: RawSweep) -> None:
|
||||
"""Publish a completed sweep to the mailbox(es), waking any waiter.
|
||||
|
||||
Dirty sweeps (straddling a switch, or incomplete RFG sweeps) are counted but not
|
||||
delivered: the collector re-takes that combination on the next sweep. Clean
|
||||
tagged sweeps go to their per-combo slot; untagged sweeps (non-switch mode)
|
||||
only update the single newest-sweep mailbox that read_sweep() serves.
|
||||
"""
|
||||
with self._mailbox_cv:
|
||||
self._published_count += 1
|
||||
if sweep.dirty:
|
||||
self.last_dirty_size = sweep.size
|
||||
self._mailbox_cv.notify_all()
|
||||
return
|
||||
self._latest_sweep = sweep
|
||||
if sweep.combo is not None:
|
||||
self._combo_slots[sweep.combo] = sweep
|
||||
self._mailbox_cv.notify_all()
|
||||
|
||||
def _publish_error(self, exc: Exception) -> None:
|
||||
"""Record ``exc`` as the reader fault and wake any waiter."""
|
||||
with self._mailbox_cv:
|
||||
self._reader_error = exc
|
||||
self._mailbox_cv.notify_all()
|
||||
Reference in New Issue
Block a user