DCCONSTRUCTIONS
e515ab1b8c
Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
142 lines
5.5 KiB
Python
142 lines
5.5 KiB
Python
"""Bounded 1x raw archive through the production derived queue and K1 decoder."""
|
|
|
|
import threading
|
|
import time
|
|
|
|
from k1link.compute.live_perception import LivePerceptionIngress
|
|
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
|
|
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
|
|
|
|
|
|
class ReceiptQueueArchiveSource:
|
|
"""Private instance only: no live singleton, MQTT, capture or hardware access."""
|
|
|
|
def __init__(
|
|
self,
|
|
raw,
|
|
session,
|
|
maximum_seconds=65,
|
|
*,
|
|
after_monotonic_ns=None,
|
|
spatial_stop_monotonic_ns=None,
|
|
drop_interval_s=None,
|
|
):
|
|
if not 0 < maximum_seconds <= 600:
|
|
raise ValueError("This offline probe supports up to ten minutes of recorded receipts.")
|
|
self.maximum_seconds = maximum_seconds
|
|
if drop_interval_s is not None and not (
|
|
len(drop_interval_s) == 2
|
|
and 0 < drop_interval_s[0] < drop_interval_s[1] < maximum_seconds
|
|
):
|
|
raise ValueError("Invalid explicit archive fault interval.")
|
|
self.drop_interval_s = drop_interval_s
|
|
self.dropped_receipts = []
|
|
self.raw, self.session = raw, session
|
|
self.after_monotonic_ns = after_monotonic_ns
|
|
self.spatial_stop_monotonic_ns = spatial_stop_monotonic_ns
|
|
self.stopping_snapshot = None
|
|
self.ingress = LivePerceptionIngress()
|
|
self.adapter = K1PlanningLiveSource(self.ingress)
|
|
self.started = None
|
|
self.owner = None
|
|
self.deliveries = []
|
|
self.stop = threading.Event()
|
|
self.thread = None
|
|
self.error = None
|
|
|
|
def snapshot(self):
|
|
return self.ingress.snapshot()
|
|
|
|
def open(self, owner):
|
|
self.adapter.open(owner)
|
|
self.owner = owner
|
|
|
|
def close(self, owner):
|
|
assert self.owner == owner
|
|
self.stop.set()
|
|
if self.thread:
|
|
self.thread.join(3)
|
|
assert not self.thread.is_alive(), "Archive publisher did not stop."
|
|
self.adapter.close(owner)
|
|
self.owner = None
|
|
|
|
def activate(self):
|
|
self.ingress.begin_session(self.session)
|
|
self.started = time.monotonic_ns()
|
|
self.thread = threading.Thread(target=self.publish, name="bounded-archive-publisher")
|
|
self.thread.start()
|
|
|
|
def take(self, owner):
|
|
return self.adapter.take(owner)
|
|
|
|
def publish(self):
|
|
previous, origin = -1, None
|
|
try:
|
|
for message in iter_replay_messages(self.raw):
|
|
stamp = message.received_monotonic_ns
|
|
if stamp is None or stamp < previous:
|
|
raise ValueError("Archive has missing or regressing receipt clocks.")
|
|
previous = stamp
|
|
if self.after_monotonic_ns is not None and stamp < self.after_monotonic_ns:
|
|
continue
|
|
modality = (
|
|
"pose"
|
|
if message.topic.endswith("/lio_pose")
|
|
else "lidar"
|
|
if message.topic.endswith("/lio_pcl")
|
|
else None
|
|
)
|
|
if modality is None:
|
|
continue
|
|
if origin is None:
|
|
origin = stamp
|
|
delay = stamp - origin
|
|
if delay > self.maximum_seconds * 1e9:
|
|
break
|
|
if (
|
|
self.drop_interval_s is not None
|
|
and self.drop_interval_s[0] <= delay / 1e9 < self.drop_interval_s[1]
|
|
):
|
|
self.dropped_receipts.append(
|
|
dict(sequence=message.sequence, kind=modality, original_monotonic_ns=stamp)
|
|
)
|
|
continue # Surviving receipts retain original cadence and identity.
|
|
mapped = self.started + delay
|
|
if self.stop.wait(max(0, (mapped - time.monotonic_ns()) / 1e9)):
|
|
break
|
|
delivered = time.monotonic_ns()
|
|
admitted = self.ingress.publish(
|
|
modality=modality,
|
|
source_id=message.topic,
|
|
source_sequence=message.sequence,
|
|
captured_at_epoch_ns=message.received_at_epoch_ns,
|
|
received_monotonic_ns=mapped,
|
|
payload=message.payload,
|
|
)
|
|
self.deliveries.append(
|
|
dict(
|
|
sequence=message.sequence,
|
|
kind=modality,
|
|
original_monotonic_ns=stamp,
|
|
mapped_monotonic_ns=mapped,
|
|
delivered_monotonic_ns=delivered,
|
|
lag_s=(delivered - mapped) / 1e9,
|
|
admitted=admitted,
|
|
)
|
|
)
|
|
if (
|
|
self.spatial_stop_monotonic_ns is not None
|
|
and stamp >= self.spatial_stop_monotonic_ns
|
|
):
|
|
self.ingress.request_spatial_stop(self.session, 1)
|
|
self.stopping_snapshot = self.ingress.snapshot()
|
|
# Model retained recorder ownership. The planner must end
|
|
# first and close this private publisher, not wait for EOF.
|
|
if not self.stop.wait(10):
|
|
raise RuntimeError("Planning did not finish after the STOP boundary.")
|
|
break
|
|
except Exception as exc:
|
|
self.error = f"{type(exc).__name__}: {exc}"
|
|
finally:
|
|
self.ingress.end_session(self.session)
|