feat: add live K1 Foxglove console
This commit is contained in:
@@ -0,0 +1,15 @@
|
||||
"""Live/replay visualization bridge for verified K1 MQTT streams."""
|
||||
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
from k1link.viewer.replay import (
|
||||
ReplayFormatError,
|
||||
detect_replay_format,
|
||||
iter_replay_messages,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"ReplayFormatError",
|
||||
"StreamMessage",
|
||||
"detect_replay_format",
|
||||
"iter_replay_messages",
|
||||
]
|
||||
@@ -0,0 +1,417 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import statistics
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import TypedDict
|
||||
|
||||
import foxglove
|
||||
from foxglove import Channel
|
||||
from foxglove.channels import (
|
||||
PointCloudChannel,
|
||||
PoseInFrameChannel,
|
||||
SceneUpdateChannel,
|
||||
)
|
||||
from foxglove.messages import (
|
||||
Color,
|
||||
LinePrimitive,
|
||||
LinePrimitiveLineType,
|
||||
PackedElementField,
|
||||
PackedElementFieldNumericType,
|
||||
Point3,
|
||||
PointCloud,
|
||||
Pose,
|
||||
PoseInFrame,
|
||||
Quaternion,
|
||||
SceneEntity,
|
||||
SceneUpdate,
|
||||
Timestamp,
|
||||
Vector3,
|
||||
)
|
||||
|
||||
from k1link.protocol.streams import (
|
||||
LegacyPointCloudFrame,
|
||||
LegacyPoseFrame,
|
||||
LioPointCloudFrame,
|
||||
LioPoseFrame,
|
||||
StreamDecodeError,
|
||||
decode_legacy_pointcloud,
|
||||
decode_legacy_pose,
|
||||
decode_lio_pcl,
|
||||
decode_lio_pose,
|
||||
)
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
|
||||
POINT_STRUCT = struct.Struct("<fffB3x")
|
||||
POINT_STRIDE = POINT_STRUCT.size
|
||||
FRAME_ID = "map"
|
||||
MAX_TRAJECTORY_POSES = 20_000
|
||||
|
||||
|
||||
class MetricsSnapshot(TypedDict):
|
||||
messages_received: int
|
||||
payload_bytes: int
|
||||
pcl_frames: int
|
||||
pose_frames: int
|
||||
points_published: int
|
||||
last_point_count: int
|
||||
decode_errors: int
|
||||
preview_dropped: int
|
||||
pcl_fps: float
|
||||
pose_fps: float
|
||||
mqtt_to_publish_ms: float | None
|
||||
mqtt_to_publish_p50_ms: float | None
|
||||
mqtt_to_publish_p95_ms: float | None
|
||||
decode_publish_ms: float | None
|
||||
trajectory_poses: int
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class PackedPointCloud:
|
||||
data: bytes
|
||||
point_count: int
|
||||
|
||||
|
||||
class BridgeMetrics:
|
||||
"""Thread-safe counters shared with the local control API."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._messages_received = 0
|
||||
self._payload_bytes = 0
|
||||
self._pcl_frames = 0
|
||||
self._pose_frames = 0
|
||||
self._points_published = 0
|
||||
self._last_point_count = 0
|
||||
self._decode_errors = 0
|
||||
self._preview_dropped = 0
|
||||
self._trajectory_poses = 0
|
||||
self._pcl_times: deque[int] = deque()
|
||||
self._pose_times: deque[int] = deque()
|
||||
self._latencies_ms: deque[float] = deque(maxlen=512)
|
||||
self._decode_publish_ms: float | None = None
|
||||
|
||||
def received(self, payload_bytes: int) -> None:
|
||||
with self._lock:
|
||||
self._messages_received += 1
|
||||
self._payload_bytes += payload_bytes
|
||||
|
||||
def published_pcl(self, point_count: int, now_ns: int, decode_publish_ms: float) -> None:
|
||||
with self._lock:
|
||||
self._pcl_frames += 1
|
||||
self._points_published += point_count
|
||||
self._last_point_count = point_count
|
||||
self._decode_publish_ms = decode_publish_ms
|
||||
self._pcl_times.append(now_ns)
|
||||
_trim_rate_window(self._pcl_times, now_ns)
|
||||
|
||||
def published_pose(self, now_ns: int, decode_publish_ms: float, path_size: int) -> None:
|
||||
with self._lock:
|
||||
self._pose_frames += 1
|
||||
self._decode_publish_ms = decode_publish_ms
|
||||
self._trajectory_poses = path_size
|
||||
self._pose_times.append(now_ns)
|
||||
_trim_rate_window(self._pose_times, now_ns)
|
||||
|
||||
def record_latency(self, milliseconds: float) -> None:
|
||||
if not math.isfinite(milliseconds) or milliseconds < 0:
|
||||
return
|
||||
with self._lock:
|
||||
self._latencies_ms.append(milliseconds)
|
||||
|
||||
def decode_error(self) -> None:
|
||||
with self._lock:
|
||||
self._decode_errors += 1
|
||||
|
||||
def preview_dropped(self) -> None:
|
||||
with self._lock:
|
||||
self._preview_dropped += 1
|
||||
|
||||
def snapshot(self) -> MetricsSnapshot:
|
||||
now_ns = time.monotonic_ns()
|
||||
with self._lock:
|
||||
_trim_rate_window(self._pcl_times, now_ns)
|
||||
_trim_rate_window(self._pose_times, now_ns)
|
||||
latencies = list(self._latencies_ms)
|
||||
last_latency = latencies[-1] if latencies else None
|
||||
p50 = statistics.median(latencies) if latencies else None
|
||||
p95 = _percentile(latencies, 0.95) if latencies else None
|
||||
return {
|
||||
"messages_received": self._messages_received,
|
||||
"payload_bytes": self._payload_bytes,
|
||||
"pcl_frames": self._pcl_frames,
|
||||
"pose_frames": self._pose_frames,
|
||||
"points_published": self._points_published,
|
||||
"last_point_count": self._last_point_count,
|
||||
"decode_errors": self._decode_errors,
|
||||
"preview_dropped": self._preview_dropped,
|
||||
"pcl_fps": _window_rate(self._pcl_times),
|
||||
"pose_fps": _window_rate(self._pose_times),
|
||||
"mqtt_to_publish_ms": _rounded(last_latency),
|
||||
"mqtt_to_publish_p50_ms": _rounded(p50),
|
||||
"mqtt_to_publish_p95_ms": _rounded(p95),
|
||||
"decode_publish_ms": _rounded(self._decode_publish_ms),
|
||||
"trajectory_poses": self._trajectory_poses,
|
||||
}
|
||||
|
||||
|
||||
class FoxgloveBridge:
|
||||
"""Decode verified K1 topics and publish Foxglove-native visualization messages."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
host: str = "127.0.0.1",
|
||||
port: int = 8765,
|
||||
metrics: BridgeMetrics | None = None,
|
||||
) -> None:
|
||||
self.metrics = metrics or BridgeMetrics()
|
||||
self._server = foxglove.start_server(
|
||||
name="NODE.DC K1 live bridge",
|
||||
host=host,
|
||||
port=port,
|
||||
message_backlog_size=32,
|
||||
)
|
||||
self._points = PointCloudChannel("/k1/points")
|
||||
self._pose = PoseInFrameChannel("/k1/pose")
|
||||
self._trajectory = SceneUpdateChannel("/k1/trajectory")
|
||||
self._metrics = Channel(
|
||||
"/k1/metrics",
|
||||
schema={
|
||||
"type": "object",
|
||||
"properties": {
|
||||
"mqtt_to_publish_ms": {"type": ["number", "null"]},
|
||||
"mqtt_to_publish_p50_ms": {"type": ["number", "null"]},
|
||||
"mqtt_to_publish_p95_ms": {"type": ["number", "null"]},
|
||||
"decode_publish_ms": {"type": ["number", "null"]},
|
||||
"point_count": {"type": "integer"},
|
||||
"pcl_fps": {"type": "number"},
|
||||
"pose_fps": {"type": "number"},
|
||||
"preview_dropped": {"type": "integer"},
|
||||
},
|
||||
},
|
||||
)
|
||||
self._path: deque[tuple[float, float, float]] = deque(maxlen=MAX_TRAJECTORY_POSES)
|
||||
self._last_trajectory_publish_ns = 0
|
||||
self._last_point_count = 0
|
||||
self._closed = False
|
||||
|
||||
@property
|
||||
def port(self) -> int:
|
||||
return int(self._server.port)
|
||||
|
||||
@property
|
||||
def websocket_url(self) -> str:
|
||||
return f"ws://127.0.0.1:{self.port}"
|
||||
|
||||
@property
|
||||
def viewer_url(self) -> str:
|
||||
return self._server.app_url() or "https://app.foxglove.dev/"
|
||||
|
||||
def process(self, message: StreamMessage) -> None:
|
||||
started_ns = time.monotonic_ns()
|
||||
self.metrics.received(len(message.payload))
|
||||
try:
|
||||
if message.topic.endswith("/lio_pcl"):
|
||||
self._publish_lio_pcl(decode_lio_pcl(message.payload), message)
|
||||
elif message.topic == "RealtimePointcloud":
|
||||
self._publish_legacy_pcl(decode_legacy_pointcloud(message.payload), message)
|
||||
elif message.topic.endswith("/lio_pose"):
|
||||
self._publish_lio_pose(decode_lio_pose(message.payload), message)
|
||||
elif message.topic == "RealtimePath":
|
||||
self._publish_legacy_pose(decode_legacy_pose(message.payload), message)
|
||||
else:
|
||||
return
|
||||
except StreamDecodeError:
|
||||
self.metrics.decode_error()
|
||||
return
|
||||
|
||||
published_ns = time.monotonic_ns()
|
||||
decode_publish_ms = (published_ns - started_ns) / 1_000_000
|
||||
if message.topic.endswith("/lio_pcl") or message.topic == "RealtimePointcloud":
|
||||
self.metrics.published_pcl(self._last_point_count, published_ns, decode_publish_ms)
|
||||
else:
|
||||
self.metrics.published_pose(published_ns, decode_publish_ms, len(self._path))
|
||||
if message.received_monotonic_ns is not None:
|
||||
self.metrics.record_latency((published_ns - message.received_monotonic_ns) / 1_000_000)
|
||||
snapshot = self.metrics.snapshot()
|
||||
self._metrics.log(
|
||||
{
|
||||
"mqtt_to_publish_ms": snapshot["mqtt_to_publish_ms"],
|
||||
"mqtt_to_publish_p50_ms": snapshot["mqtt_to_publish_p50_ms"],
|
||||
"mqtt_to_publish_p95_ms": snapshot["mqtt_to_publish_p95_ms"],
|
||||
"decode_publish_ms": snapshot["decode_publish_ms"],
|
||||
"point_count": snapshot["last_point_count"],
|
||||
"pcl_fps": snapshot["pcl_fps"],
|
||||
"pose_fps": snapshot["pose_fps"],
|
||||
"preview_dropped": snapshot["preview_dropped"],
|
||||
},
|
||||
log_time=message.received_at_epoch_ns,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
for channel in (self._points, self._pose, self._trajectory, self._metrics):
|
||||
channel.close()
|
||||
self._server.stop()
|
||||
|
||||
def _publish_lio_pcl(self, frame: LioPointCloudFrame, message: StreamMessage) -> None:
|
||||
packed = pack_lio_point_cloud(frame)
|
||||
self._publish_point_cloud(packed, message)
|
||||
|
||||
def _publish_legacy_pcl(
|
||||
self,
|
||||
frame: LegacyPointCloudFrame,
|
||||
message: StreamMessage,
|
||||
) -> None:
|
||||
packed = pack_legacy_point_cloud(frame)
|
||||
self._publish_point_cloud(packed, message)
|
||||
|
||||
def _publish_point_cloud(self, packed: PackedPointCloud, message: StreamMessage) -> None:
|
||||
timestamp = _timestamp(message.received_at_epoch_ns)
|
||||
self._points.log(
|
||||
PointCloud(
|
||||
timestamp=timestamp,
|
||||
frame_id=FRAME_ID,
|
||||
pose=_identity_pose(),
|
||||
point_stride=POINT_STRIDE,
|
||||
fields=_point_fields(),
|
||||
data=packed.data,
|
||||
),
|
||||
log_time=message.received_at_epoch_ns,
|
||||
)
|
||||
self._last_point_count = packed.point_count
|
||||
|
||||
def _publish_lio_pose(self, frame: LioPoseFrame, message: StreamMessage) -> None:
|
||||
self._publish_pose(frame.position_xyz, frame.orientation_xyzw, message)
|
||||
|
||||
def _publish_legacy_pose(self, frame: LegacyPoseFrame, message: StreamMessage) -> None:
|
||||
self._publish_pose(frame.position_xyz, frame.orientation_xyzw, message)
|
||||
|
||||
def _publish_pose(
|
||||
self,
|
||||
position_xyz: tuple[float, float, float],
|
||||
orientation_xyzw: tuple[float, float, float, float],
|
||||
message: StreamMessage,
|
||||
) -> None:
|
||||
pose = Pose(
|
||||
position=Vector3(x=position_xyz[0], y=position_xyz[1], z=position_xyz[2]),
|
||||
orientation=Quaternion(
|
||||
x=orientation_xyzw[0],
|
||||
y=orientation_xyzw[1],
|
||||
z=orientation_xyzw[2],
|
||||
w=orientation_xyzw[3],
|
||||
),
|
||||
)
|
||||
timestamp = _timestamp(message.received_at_epoch_ns)
|
||||
self._pose.log(
|
||||
PoseInFrame(timestamp=timestamp, frame_id=FRAME_ID, pose=pose),
|
||||
log_time=message.received_at_epoch_ns,
|
||||
)
|
||||
self._path.append(position_xyz)
|
||||
now_ns = time.monotonic_ns()
|
||||
if (
|
||||
len(self._path) > 2
|
||||
and len(self._path) % 20
|
||||
and now_ns - self._last_trajectory_publish_ns < 200_000_000
|
||||
):
|
||||
return
|
||||
self._last_trajectory_publish_ns = now_ns
|
||||
line_points = [Point3(x=item[0], y=item[1], z=item[2]) for item in self._path]
|
||||
self._trajectory.log(
|
||||
SceneUpdate(
|
||||
entities=[
|
||||
SceneEntity(
|
||||
timestamp=timestamp,
|
||||
frame_id=FRAME_ID,
|
||||
id="k1-trajectory",
|
||||
frame_locked=True,
|
||||
lines=[
|
||||
LinePrimitive(
|
||||
type=LinePrimitiveLineType.LineStrip,
|
||||
thickness=3.0,
|
||||
scale_invariant=True,
|
||||
points=line_points,
|
||||
color=Color(r=0.08, g=0.82, b=1.0, a=1.0),
|
||||
)
|
||||
],
|
||||
)
|
||||
]
|
||||
),
|
||||
log_time=message.received_at_epoch_ns,
|
||||
)
|
||||
|
||||
|
||||
def pack_lio_point_cloud(frame: LioPointCloudFrame) -> PackedPointCloud:
|
||||
data = bytearray(len(frame.points) * POINT_STRIDE)
|
||||
scaler = frame.header.scaler
|
||||
for index, point in enumerate(frame.points):
|
||||
x, y, z = point.scaled_xyz(scaler)
|
||||
POINT_STRUCT.pack_into(data, index * POINT_STRIDE, x, y, z, point.intensity)
|
||||
return PackedPointCloud(data=bytes(data), point_count=len(frame.points))
|
||||
|
||||
|
||||
def pack_legacy_point_cloud(frame: LegacyPointCloudFrame) -> PackedPointCloud:
|
||||
data = bytearray(len(frame.points) * POINT_STRIDE)
|
||||
for index, point in enumerate(frame.points):
|
||||
POINT_STRUCT.pack_into(
|
||||
data,
|
||||
index * POINT_STRIDE,
|
||||
point.x,
|
||||
point.y,
|
||||
point.z,
|
||||
point.intensity,
|
||||
)
|
||||
return PackedPointCloud(data=bytes(data), point_count=len(frame.points))
|
||||
|
||||
|
||||
def _point_fields() -> list[PackedElementField]:
|
||||
return [
|
||||
PackedElementField(name="x", offset=0, type=PackedElementFieldNumericType.Float32),
|
||||
PackedElementField(name="y", offset=4, type=PackedElementFieldNumericType.Float32),
|
||||
PackedElementField(name="z", offset=8, type=PackedElementFieldNumericType.Float32),
|
||||
PackedElementField(
|
||||
name="intensity",
|
||||
offset=12,
|
||||
type=PackedElementFieldNumericType.Uint8,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def _identity_pose() -> Pose:
|
||||
return Pose(position=Vector3(), orientation=Quaternion(w=1.0))
|
||||
|
||||
|
||||
def _timestamp(epoch_ns: int) -> Timestamp:
|
||||
return Timestamp(epoch_ns // 1_000_000_000, epoch_ns % 1_000_000_000)
|
||||
|
||||
|
||||
def _trim_rate_window(values: deque[int], now_ns: int) -> None:
|
||||
cutoff = now_ns - 1_000_000_000
|
||||
while values and values[0] < cutoff:
|
||||
values.popleft()
|
||||
|
||||
|
||||
def _window_rate(values: deque[int]) -> float:
|
||||
if len(values) < 2:
|
||||
return float(len(values))
|
||||
elapsed = (values[-1] - values[0]) / 1_000_000_000
|
||||
return len(values) / max(elapsed, 1.0)
|
||||
|
||||
|
||||
def _percentile(values: list[float], fraction: float) -> float:
|
||||
if not values:
|
||||
raise ValueError("cannot calculate a percentile of an empty sample")
|
||||
ordered = sorted(values)
|
||||
index = min(len(ordered) - 1, math.ceil(len(ordered) * fraction) - 1)
|
||||
return ordered[index]
|
||||
|
||||
|
||||
def _rounded(value: float | None) -> float | None:
|
||||
return None if value is None else round(value, 3)
|
||||
@@ -0,0 +1,15 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class StreamMessage:
|
||||
"""One MQTT message entering the derived visualization pipeline."""
|
||||
|
||||
sequence: int
|
||||
topic: str
|
||||
payload: bytes
|
||||
received_at_epoch_ns: int
|
||||
received_monotonic_ns: int | None = None
|
||||
source: str = "replay"
|
||||
@@ -0,0 +1,160 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import time
|
||||
from collections.abc import Generator, Iterator
|
||||
from decimal import Decimal, InvalidOperation
|
||||
from pathlib import Path
|
||||
from typing import IO, Literal
|
||||
|
||||
from k1link.mqtt import CaptureFormatError, iter_capture_frames
|
||||
from k1link.mqtt.capture import RAW_MAGIC
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
|
||||
MAX_REPLAY_PAYLOAD_BYTES = 2 * 1024 * 1024
|
||||
MAX_LEGACY_LINE_BYTES = MAX_REPLAY_PAYLOAD_BYTES * 2 + 64 * 1024
|
||||
MAX_METADATA_LINE_CHARS = 1024 * 1024
|
||||
ReplayFormat = Literal["k1mqtt", "legacy_tsv"]
|
||||
|
||||
|
||||
class ReplayFormatError(ValueError):
|
||||
"""A replay input is corrupt or outside the reviewed bounds."""
|
||||
|
||||
|
||||
def detect_replay_format(path: Path) -> ReplayFormat:
|
||||
resolved = path.expanduser()
|
||||
with resolved.open("rb") as stream:
|
||||
prefix = stream.read(len(RAW_MAGIC))
|
||||
if prefix == RAW_MAGIC:
|
||||
return "k1mqtt"
|
||||
if resolved.suffix.casefold() == ".tsv":
|
||||
return "legacy_tsv"
|
||||
raise ReplayFormatError("input is neither a K1MQTT capture nor the reviewed legacy TSV")
|
||||
|
||||
|
||||
def iter_replay_messages(path: Path) -> Generator[StreamMessage, None, None]:
|
||||
"""Yield bounded messages from native evidence or the one reviewed TSV export."""
|
||||
resolved = path.expanduser().resolve()
|
||||
replay_format = detect_replay_format(resolved)
|
||||
if replay_format == "legacy_tsv":
|
||||
yield from _iter_legacy_tsv(resolved)
|
||||
return
|
||||
yield from _iter_native_capture(resolved)
|
||||
|
||||
|
||||
def _iter_native_capture(path: Path) -> Iterator[StreamMessage]:
|
||||
metadata_path = path.with_name("mqtt.metadata.jsonl")
|
||||
metadata_stream: IO[str] | None = None
|
||||
if metadata_path.is_file():
|
||||
metadata_stream = metadata_path.open("r", encoding="utf-8")
|
||||
fallback_epoch_ns = time.time_ns()
|
||||
try:
|
||||
for frame in iter_capture_frames(path, max_payload_bytes=MAX_REPLAY_PAYLOAD_BYTES):
|
||||
epoch_ns = fallback_epoch_ns + frame.sequence - 1
|
||||
monotonic_ns: int | None = None
|
||||
if metadata_stream is not None:
|
||||
epoch_ns, monotonic_ns = _read_native_timing(
|
||||
metadata_stream,
|
||||
expected_sequence=frame.sequence,
|
||||
fallback_epoch_ns=epoch_ns,
|
||||
)
|
||||
yield StreamMessage(
|
||||
sequence=frame.sequence,
|
||||
topic=frame.topic,
|
||||
payload=frame.payload,
|
||||
received_at_epoch_ns=epoch_ns,
|
||||
received_monotonic_ns=monotonic_ns,
|
||||
source="k1mqtt",
|
||||
)
|
||||
except CaptureFormatError as exc:
|
||||
raise ReplayFormatError(str(exc)) from exc
|
||||
finally:
|
||||
if metadata_stream is not None:
|
||||
metadata_stream.close()
|
||||
|
||||
|
||||
def _read_native_timing(
|
||||
stream: IO[str],
|
||||
*,
|
||||
expected_sequence: int,
|
||||
fallback_epoch_ns: int,
|
||||
) -> tuple[int, int | None]:
|
||||
line = stream.readline(MAX_METADATA_LINE_CHARS + 1)
|
||||
if not line:
|
||||
return fallback_epoch_ns, None
|
||||
if len(line) > MAX_METADATA_LINE_CHARS:
|
||||
raise ReplayFormatError("native metadata line exceeds the reviewed bound")
|
||||
try:
|
||||
record = json.loads(line)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise ReplayFormatError(
|
||||
f"native metadata line {expected_sequence} is not valid JSON"
|
||||
) from exc
|
||||
if record.get("record_type") != "message" or record.get("sequence") != expected_sequence:
|
||||
raise ReplayFormatError(f"native metadata is not aligned at message {expected_sequence}")
|
||||
epoch_ns = record.get("received_at_epoch_ns", fallback_epoch_ns)
|
||||
monotonic_ns = record.get("received_monotonic_ns")
|
||||
if not isinstance(epoch_ns, int) or epoch_ns < 0:
|
||||
raise ReplayFormatError("native metadata received_at_epoch_ns is invalid")
|
||||
if monotonic_ns is not None and (not isinstance(monotonic_ns, int) or monotonic_ns < 0):
|
||||
raise ReplayFormatError("native metadata received_monotonic_ns is invalid")
|
||||
return epoch_ns, monotonic_ns
|
||||
|
||||
|
||||
def _iter_legacy_tsv(path: Path) -> Iterator[StreamMessage]:
|
||||
with path.open("rb") as stream:
|
||||
line_number = 0
|
||||
while True:
|
||||
raw_line = stream.readline(MAX_LEGACY_LINE_BYTES + 1)
|
||||
if not raw_line:
|
||||
return
|
||||
line_number += 1
|
||||
if len(raw_line) > MAX_LEGACY_LINE_BYTES:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} exceeds {MAX_LEGACY_LINE_BYTES} bytes"
|
||||
)
|
||||
stripped = raw_line.rstrip(b"\r\n")
|
||||
if not stripped:
|
||||
continue
|
||||
columns = stripped.split(b"\t")
|
||||
if len(columns) != 4:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} must contain exactly four columns"
|
||||
)
|
||||
timestamp_raw, topic_raw, length_raw, payload_hex = columns
|
||||
try:
|
||||
timestamp = Decimal(timestamp_raw.decode("ascii"))
|
||||
declared_length = int(length_raw.decode("ascii"), 10)
|
||||
topic = topic_raw.decode("utf-8")
|
||||
except (InvalidOperation, UnicodeDecodeError, ValueError) as exc:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} has invalid timestamp/topic/length"
|
||||
) from exc
|
||||
if not timestamp.is_finite() or timestamp < 0:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} timestamp is outside bounds"
|
||||
)
|
||||
if not topic:
|
||||
raise ReplayFormatError(f"legacy TSV line {line_number} has an empty topic")
|
||||
if declared_length < 0 or declared_length > MAX_REPLAY_PAYLOAD_BYTES:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} payload length is outside bounds"
|
||||
)
|
||||
if len(payload_hex) != declared_length * 2:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} declared payload length does not match hex"
|
||||
)
|
||||
try:
|
||||
payload = bytes.fromhex(payload_hex.decode("ascii"))
|
||||
except (UnicodeDecodeError, ValueError) as exc:
|
||||
raise ReplayFormatError(
|
||||
f"legacy TSV line {line_number} payload is not valid hex"
|
||||
) from exc
|
||||
epoch_ns = int(timestamp * Decimal(1_000_000_000))
|
||||
yield StreamMessage(
|
||||
sequence=line_number,
|
||||
topic=topic,
|
||||
payload=payload,
|
||||
received_at_epoch_ns=epoch_ns,
|
||||
source="legacy_tsv",
|
||||
)
|
||||
@@ -0,0 +1,394 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Callable
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Literal, TypedDict
|
||||
|
||||
from k1link.artifacts import utc_now_iso, write_json_atomic
|
||||
from k1link.mqtt import CapturedMqttMessage, CaptureError, capture_mqtt
|
||||
from k1link.viewer.foxglove_bridge import BridgeMetrics, FoxgloveBridge, MetricsSnapshot
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
from k1link.viewer.replay import iter_replay_messages
|
||||
|
||||
RuntimePhase = Literal[
|
||||
"idle",
|
||||
"starting_live",
|
||||
"live",
|
||||
"replay",
|
||||
"stopping",
|
||||
"error",
|
||||
]
|
||||
SourceMode = Literal["idle", "live", "replay"]
|
||||
StateCallback = Callable[[], None]
|
||||
|
||||
|
||||
class RuntimeSnapshot(TypedDict):
|
||||
phase: RuntimePhase
|
||||
message: str
|
||||
source_mode: SourceMode
|
||||
foxglove_ws_url: str | None
|
||||
foxglove_viewer_url: str | None
|
||||
metrics: MetricsSnapshot
|
||||
|
||||
|
||||
class VisualizationRuntime:
|
||||
"""Own one bounded live/replay source and one deterministic publisher thread."""
|
||||
|
||||
def __init__(self, *, on_state_change: StateCallback | None = None) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._on_state_change = on_state_change
|
||||
self._thread: threading.Thread | None = None
|
||||
self._stop_event = threading.Event()
|
||||
self._phase: RuntimePhase = "idle"
|
||||
self._message = "Ready for a live scanner or replay capture."
|
||||
self._source_mode: SourceMode = "idle"
|
||||
self._foxglove_ws_url: str | None = None
|
||||
self._foxglove_viewer_url: str | None = None
|
||||
self._metrics = BridgeMetrics()
|
||||
|
||||
def snapshot(self) -> RuntimeSnapshot:
|
||||
with self._lock:
|
||||
return {
|
||||
"phase": self._phase,
|
||||
"message": self._message,
|
||||
"source_mode": self._source_mode,
|
||||
"foxglove_ws_url": self._foxglove_ws_url,
|
||||
"foxglove_viewer_url": self._foxglove_viewer_url,
|
||||
"metrics": self._metrics.snapshot(),
|
||||
}
|
||||
|
||||
def start_replay(self, path: Path, *, speed: float = 1.0, loop: bool = False) -> None:
|
||||
resolved = path.expanduser().resolve()
|
||||
if not resolved.is_file():
|
||||
raise ValueError("replay path must be an existing backend-local file")
|
||||
if not math.isfinite(speed) or speed < 0:
|
||||
raise ValueError("replay speed must be finite and non-negative")
|
||||
# Validate the reviewed shape before changing runtime state.
|
||||
iterator = iter_replay_messages(resolved)
|
||||
try:
|
||||
next(iterator)
|
||||
except StopIteration as exc:
|
||||
raise ValueError("replay capture contains no messages") from exc
|
||||
finally:
|
||||
iterator.close()
|
||||
|
||||
self._start(
|
||||
source_mode="replay",
|
||||
phase="replay",
|
||||
message=f"Starting replay: {resolved.name}",
|
||||
target=lambda: self._run_replay(resolved, speed=speed, loop=loop),
|
||||
)
|
||||
|
||||
def start_live(
|
||||
self,
|
||||
host: str,
|
||||
out_dir: Path,
|
||||
*,
|
||||
duration_seconds: float = 3600.0,
|
||||
) -> None:
|
||||
if not math.isfinite(duration_seconds) or duration_seconds <= 0:
|
||||
raise ValueError("live duration must be finite and greater than zero")
|
||||
self._start(
|
||||
source_mode="live",
|
||||
phase="starting_live",
|
||||
message="Starting read-only MQTT capture and Foxglove bridge.",
|
||||
target=lambda: self._run_live(
|
||||
host,
|
||||
out_dir.expanduser().resolve(),
|
||||
duration_seconds=duration_seconds,
|
||||
),
|
||||
)
|
||||
|
||||
def stop(self, *, wait_seconds: float = 5.0) -> None:
|
||||
notify_only = False
|
||||
with self._lock:
|
||||
thread = self._thread
|
||||
if thread is None or not thread.is_alive():
|
||||
self._phase = "idle"
|
||||
self._source_mode = "idle"
|
||||
self._message = "No active visualization session."
|
||||
notify_only = True
|
||||
else:
|
||||
self._phase = "stopping"
|
||||
self._message = "Stopping source after preserving queued evidence."
|
||||
self._stop_event.set()
|
||||
self._notify()
|
||||
if notify_only:
|
||||
return
|
||||
assert thread is not None
|
||||
thread.join(timeout=wait_seconds)
|
||||
|
||||
def _start(
|
||||
self,
|
||||
*,
|
||||
source_mode: SourceMode,
|
||||
phase: RuntimePhase,
|
||||
message: str,
|
||||
target: Callable[[], None],
|
||||
) -> None:
|
||||
with self._lock:
|
||||
if self._thread is not None and self._thread.is_alive():
|
||||
raise RuntimeError("a visualization session is already active")
|
||||
self._stop_event = threading.Event()
|
||||
self._metrics = BridgeMetrics()
|
||||
self._phase = phase
|
||||
self._source_mode = source_mode
|
||||
self._message = message
|
||||
self._foxglove_ws_url = None
|
||||
self._foxglove_viewer_url = None
|
||||
self._thread = threading.Thread(
|
||||
target=target,
|
||||
name=f"k1-{source_mode}-session",
|
||||
daemon=True,
|
||||
)
|
||||
self._thread.start()
|
||||
self._notify()
|
||||
|
||||
def _run_replay(self, path: Path, *, speed: float, loop: bool) -> None:
|
||||
def produce(put: Callable[[StreamMessage], None]) -> str:
|
||||
while not self._stop_event.is_set():
|
||||
first_source_ns: int | None = None
|
||||
replay_started_ns = time.monotonic_ns()
|
||||
count = 0
|
||||
for message in iter_replay_messages(path):
|
||||
if self._stop_event.is_set():
|
||||
return "Replay stopped by operator."
|
||||
source_ns = (
|
||||
message.received_monotonic_ns
|
||||
if message.received_monotonic_ns is not None
|
||||
else message.received_at_epoch_ns
|
||||
)
|
||||
if first_source_ns is None:
|
||||
first_source_ns = source_ns
|
||||
if speed > 0:
|
||||
target_ns = replay_started_ns + int((source_ns - first_source_ns) / speed)
|
||||
remaining = (target_ns - time.monotonic_ns()) / 1_000_000_000
|
||||
if remaining > 0 and self._stop_event.wait(remaining):
|
||||
return "Replay stopped by operator."
|
||||
put(message)
|
||||
count += 1
|
||||
if not loop:
|
||||
return f"Replay completed: {count} MQTT messages."
|
||||
return "Replay stopped by operator."
|
||||
|
||||
self._run_pipeline(produce, running_phase="replay")
|
||||
|
||||
def _run_live(self, host: str, out_dir: Path, *, duration_seconds: float) -> None:
|
||||
_write_live_session_preamble(out_dir, host, duration_seconds)
|
||||
|
||||
def produce(put: Callable[[StreamMessage], None]) -> str:
|
||||
def on_message(message: CapturedMqttMessage) -> None:
|
||||
put(
|
||||
StreamMessage(
|
||||
sequence=message.sequence,
|
||||
topic=message.topic,
|
||||
payload=message.payload,
|
||||
received_at_epoch_ns=message.received_at_epoch_ns,
|
||||
received_monotonic_ns=message.received_monotonic_ns,
|
||||
source="live_mqtt",
|
||||
)
|
||||
)
|
||||
|
||||
summary = capture_mqtt(
|
||||
host,
|
||||
out_dir / "captures" / "mqtt_live",
|
||||
duration_seconds=duration_seconds,
|
||||
on_ready=lambda: self._set_running(
|
||||
"live", "MQTT subscribed; waiting for K1 scan frames."
|
||||
),
|
||||
on_message_recorded=on_message,
|
||||
should_stop=self._stop_event.is_set,
|
||||
)
|
||||
return (
|
||||
f"Live capture stopped: {summary['stop_reason']}; "
|
||||
f"{summary['message_count']} messages preserved."
|
||||
)
|
||||
|
||||
try:
|
||||
self._run_pipeline(produce, running_phase="live")
|
||||
finally:
|
||||
_finalize_live_session(out_dir, self.snapshot())
|
||||
|
||||
def _run_pipeline(
|
||||
self,
|
||||
producer: Callable[[Callable[[StreamMessage], None]], str],
|
||||
*,
|
||||
running_phase: RuntimePhase,
|
||||
) -> None:
|
||||
messages: queue.Queue[StreamMessage] = queue.Queue(maxsize=32)
|
||||
source_done = threading.Event()
|
||||
publisher_ready = threading.Event()
|
||||
publisher_error: list[BaseException] = []
|
||||
|
||||
def enqueue(message: StreamMessage) -> None:
|
||||
try:
|
||||
messages.put_nowait(message)
|
||||
return
|
||||
except queue.Full:
|
||||
pass
|
||||
try:
|
||||
messages.get_nowait()
|
||||
messages.task_done()
|
||||
except queue.Empty:
|
||||
pass
|
||||
self._metrics.preview_dropped()
|
||||
try:
|
||||
messages.put_nowait(message)
|
||||
except queue.Full:
|
||||
self._metrics.preview_dropped()
|
||||
|
||||
def publish() -> None:
|
||||
bridge: FoxgloveBridge | None = None
|
||||
try:
|
||||
bridge = FoxgloveBridge(metrics=self._metrics)
|
||||
with self._lock:
|
||||
self._foxglove_ws_url = bridge.websocket_url
|
||||
self._foxglove_viewer_url = bridge.viewer_url
|
||||
if self._phase != "stopping":
|
||||
self._phase = running_phase
|
||||
self._message = "Foxglove bridge ready; source is active."
|
||||
publisher_ready.set()
|
||||
self._notify()
|
||||
while not source_done.is_set() or not messages.empty():
|
||||
if self._stop_event.is_set() and source_done.is_set() and messages.empty():
|
||||
break
|
||||
try:
|
||||
message = messages.get(timeout=0.1)
|
||||
except queue.Empty:
|
||||
continue
|
||||
try:
|
||||
bridge.process(message)
|
||||
finally:
|
||||
messages.task_done()
|
||||
if self._metrics.snapshot()["messages_received"] % 10 == 0:
|
||||
self._notify()
|
||||
except BaseException as exc:
|
||||
publisher_error.append(exc)
|
||||
publisher_ready.set()
|
||||
self._stop_event.set()
|
||||
finally:
|
||||
if bridge is not None:
|
||||
bridge.close()
|
||||
|
||||
publisher = threading.Thread(target=publish, name="k1-foxglove-publisher", daemon=True)
|
||||
publisher.start()
|
||||
if not publisher_ready.wait(timeout=15.0):
|
||||
self._finish_error("Foxglove bridge did not start within 15 seconds.")
|
||||
source_done.set()
|
||||
self._stop_event.set()
|
||||
return
|
||||
if publisher_error:
|
||||
self._finish_error(
|
||||
f"Foxglove bridge failed: {type(publisher_error[0]).__name__}: {publisher_error[0]}"
|
||||
)
|
||||
source_done.set()
|
||||
return
|
||||
|
||||
final_message = "Session stopped."
|
||||
try:
|
||||
final_message = producer(enqueue)
|
||||
except CaptureError as exc:
|
||||
self._finish_error(f"Live MQTT capture failed: {exc}")
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self._finish_error(f"Source failed: {type(exc).__name__}: {exc}")
|
||||
finally:
|
||||
source_done.set()
|
||||
publisher.join(timeout=15.0)
|
||||
if publisher.is_alive():
|
||||
self._stop_event.set()
|
||||
self._finish_error("Publisher did not drain within 15 seconds.")
|
||||
elif publisher_error:
|
||||
self._finish_error(
|
||||
f"Publisher failed: {type(publisher_error[0]).__name__}: {publisher_error[0]}"
|
||||
)
|
||||
elif self.snapshot()["phase"] != "error":
|
||||
self._finish_idle(final_message)
|
||||
|
||||
def _set_running(self, phase: RuntimePhase, message: str) -> None:
|
||||
with self._lock:
|
||||
if self._phase != "stopping":
|
||||
self._phase = phase
|
||||
self._message = message
|
||||
self._notify()
|
||||
|
||||
def _finish_idle(self, message: str) -> None:
|
||||
with self._lock:
|
||||
self._phase = "idle"
|
||||
self._source_mode = "idle"
|
||||
self._message = message
|
||||
self._foxglove_ws_url = None
|
||||
self._foxglove_viewer_url = None
|
||||
self._notify()
|
||||
|
||||
def _finish_error(self, message: str) -> None:
|
||||
with self._lock:
|
||||
self._phase = "error"
|
||||
self._message = message
|
||||
self._foxglove_ws_url = None
|
||||
self._foxglove_viewer_url = None
|
||||
self._notify()
|
||||
|
||||
def _notify(self) -> None:
|
||||
callback = self._on_state_change
|
||||
if callback is not None:
|
||||
callback()
|
||||
|
||||
|
||||
def new_live_session_dir(repository_root: Path) -> Path:
|
||||
stamp = datetime.now(UTC).strftime("%Y%m%dT%H%M%SZ")
|
||||
base = repository_root / "sessions" / f"{stamp}_viewer_live"
|
||||
candidate = base
|
||||
suffix = 1
|
||||
while candidate.exists():
|
||||
suffix += 1
|
||||
candidate = base.with_name(f"{base.name}_{suffix:02d}")
|
||||
return candidate
|
||||
|
||||
|
||||
def _write_live_session_preamble(out_dir: Path, host: str, duration_seconds: float) -> None:
|
||||
out_dir.mkdir(parents=True, exist_ok=False)
|
||||
started_at_utc = utc_now_iso()
|
||||
write_json_atomic(
|
||||
out_dir / "manifest.redacted.json",
|
||||
{
|
||||
"schema_version": 1,
|
||||
"started_at_utc": started_at_utc,
|
||||
"started_monotonic_ns": time.monotonic_ns(),
|
||||
"operation": "k1_live_mqtt_to_foxglove",
|
||||
"target": "owner-controlled K1 at redacted RFC1918 address",
|
||||
"requested_duration_seconds": duration_seconds,
|
||||
"raw_capture": "captures/mqtt_live/mqtt.raw.k1mqtt",
|
||||
"credential_storage": "none",
|
||||
},
|
||||
)
|
||||
notes = (
|
||||
"# K1 live visualization session\n\n"
|
||||
f"Started UTC: {started_at_utc}\n\n"
|
||||
"The local control API started a read-only MQTT subscription and Foxglove "
|
||||
"preview. Raw MQTT evidence is written before preview decoding. The target "
|
||||
f"was a validated private IPv4 address ({host.rsplit('.', 1)[0]}.x).\n"
|
||||
)
|
||||
notes_path = out_dir / "operator-notes.md"
|
||||
notes_path.write_text(notes, encoding="utf-8")
|
||||
notes_path.chmod(0o600)
|
||||
|
||||
|
||||
def _finalize_live_session(out_dir: Path, snapshot: RuntimeSnapshot) -> None:
|
||||
manifest_path = out_dir / "manifest.redacted.json"
|
||||
try:
|
||||
import json
|
||||
|
||||
payload = json.loads(manifest_path.read_text(encoding="utf-8"))
|
||||
payload["completed_at_utc"] = utc_now_iso()
|
||||
payload["completed_monotonic_ns"] = time.monotonic_ns()
|
||||
payload["final_phase"] = snapshot["phase"]
|
||||
payload["aggregate_metrics"] = snapshot["metrics"]
|
||||
write_json_atomic(manifest_path, payload)
|
||||
except (OSError, ValueError):
|
||||
# The MQTT capture summary remains authoritative if final annotation fails.
|
||||
return
|
||||
Reference in New Issue
Block a user