feat: add live K1 Foxglove console
This commit is contained in:
DCCONSTRUCTIONS
+28
-2
@@ -10,6 +10,7 @@ from pathlib import Path
|
||||
from typing import Annotated, TypedDict
|
||||
|
||||
import typer
|
||||
import uvicorn
|
||||
from bleak.exc import BleakError
|
||||
from rich.console import Console
|
||||
from rich.table import Table
|
||||
@@ -200,6 +201,32 @@ def doctor(
|
||||
console.print(f"- {note}")
|
||||
|
||||
|
||||
@app.command("serve")
|
||||
def serve_console(
|
||||
port: Annotated[
|
||||
int,
|
||||
typer.Option(min=1024, max=65535, help="Loopback HTTP port for the local console."),
|
||||
] = 8000,
|
||||
) -> None:
|
||||
"""Serve the built K1 console and local-only control API on loopback."""
|
||||
frontend = Path(__file__).resolve().parents[2] / "apps" / "k1-viewer" / "dist"
|
||||
if not frontend.is_dir():
|
||||
console.print(
|
||||
"[red]Frontend build is missing.[/red] Run npm install && npm run build "
|
||||
"inside apps/k1-viewer."
|
||||
)
|
||||
raise typer.Exit(code=2)
|
||||
console.print(f"K1 Live Console: http://127.0.0.1:{port}")
|
||||
console.print("The credential endpoint is bound to this Mac only.")
|
||||
uvicorn.run(
|
||||
"k1link.web.app:app",
|
||||
host="127.0.0.1",
|
||||
port=port,
|
||||
log_level="info",
|
||||
access_log=True,
|
||||
)
|
||||
|
||||
|
||||
@ble_app.command("scan")
|
||||
def ble_scan(
|
||||
out: Annotated[
|
||||
@@ -300,8 +327,7 @@ def ble_wifi_configure(
|
||||
raise typer.Exit(code=2)
|
||||
if not confirm_write:
|
||||
console.print(
|
||||
"[red]Write not confirmed.[/red] "
|
||||
"Add --confirm-write after reviewing the profile."
|
||||
"[red]Write not confirmed.[/red] Add --confirm-write after reviewing the profile."
|
||||
)
|
||||
raise typer.Exit(code=2)
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@ from k1link.mqtt.capture import (
|
||||
DEFAULT_MAX_MESSAGE_BYTES,
|
||||
MAX_CONFIGURABLE_MESSAGE_BYTES,
|
||||
REPORT_TOPICS,
|
||||
CapturedMqttMessage,
|
||||
CaptureError,
|
||||
CaptureFormatError,
|
||||
CaptureFrame,
|
||||
@@ -21,6 +22,7 @@ __all__ = [
|
||||
"CaptureFormatError",
|
||||
"CaptureFrame",
|
||||
"CaptureSummary",
|
||||
"CapturedMqttMessage",
|
||||
"capture_mqtt",
|
||||
"iter_capture_frames",
|
||||
"validate_private_ipv4",
|
||||
|
||||
+46
-11
@@ -44,6 +44,7 @@ _PRIVATE_NETWORKS = tuple(
|
||||
|
||||
StopReason = Literal[
|
||||
"duration_elapsed",
|
||||
"external_stop",
|
||||
"keyboard_interrupt",
|
||||
"message_too_large",
|
||||
"connection_failed",
|
||||
@@ -127,6 +128,21 @@ class CaptureFrame:
|
||||
raw_frame_bytes: int
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CapturedMqttMessage:
|
||||
"""A message made durable by the raw writer and ready for live preview."""
|
||||
|
||||
sequence: int
|
||||
topic: str
|
||||
payload: bytes
|
||||
qos: int
|
||||
retain: bool
|
||||
dup: bool
|
||||
received_at_utc: str
|
||||
received_at_epoch_ns: int
|
||||
received_monotonic_ns: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class _CaptureState:
|
||||
connected: bool = False
|
||||
@@ -168,19 +184,19 @@ class _CaptureWriter:
|
||||
self.close()
|
||||
raise
|
||||
|
||||
def record(self, message: mqtt.MQTTMessage) -> None:
|
||||
def record(self, message: mqtt.MQTTMessage) -> CapturedMqttMessage:
|
||||
raw = self._require_raw()
|
||||
metadata = self._require_metadata()
|
||||
topic = message.topic
|
||||
topic_bytes = topic.encode("utf-8")
|
||||
payload = message.payload
|
||||
received_at_utc = utc_now_iso()
|
||||
received_at_epoch_ns = time.time_ns()
|
||||
received_monotonic_ns = time.monotonic_ns()
|
||||
|
||||
if not 1 <= len(topic_bytes) <= MAX_TOPIC_BYTES:
|
||||
raise ValueError(
|
||||
f"incoming MQTT topic is {len(topic_bytes)} bytes; "
|
||||
f"expected 1..{MAX_TOPIC_BYTES}"
|
||||
f"incoming MQTT topic is {len(topic_bytes)} bytes; expected 1..{MAX_TOPIC_BYTES}"
|
||||
)
|
||||
|
||||
if len(payload) > self.max_message_bytes:
|
||||
@@ -218,6 +234,7 @@ class _CaptureWriter:
|
||||
"record_type": "message",
|
||||
"sequence": self.message_count,
|
||||
"received_at_utc": received_at_utc,
|
||||
"received_at_epoch_ns": received_at_epoch_ns,
|
||||
"received_monotonic_ns": received_monotonic_ns,
|
||||
"topic": topic,
|
||||
"qos": message.qos,
|
||||
@@ -230,6 +247,17 @@ class _CaptureWriter:
|
||||
"raw_frame_bytes": frame_bytes,
|
||||
}
|
||||
self._write_metadata(metadata, record)
|
||||
return CapturedMqttMessage(
|
||||
sequence=self.message_count,
|
||||
topic=topic,
|
||||
payload=payload,
|
||||
qos=message.qos,
|
||||
retain=message.retain,
|
||||
dup=message.dup,
|
||||
received_at_utc=received_at_utc,
|
||||
received_at_epoch_ns=received_at_epoch_ns,
|
||||
received_monotonic_ns=received_monotonic_ns,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
first_error: OSError | None = None
|
||||
@@ -296,8 +324,7 @@ def iter_capture_frames(
|
||||
"""Yield validated frames from a K1 MQTT raw capture without decoding payloads."""
|
||||
if not 1 <= max_payload_bytes <= MAX_CONFIGURABLE_MESSAGE_BYTES:
|
||||
raise ValueError(
|
||||
"max_payload_bytes must be between 1 and "
|
||||
f"{MAX_CONFIGURABLE_MESSAGE_BYTES}"
|
||||
f"max_payload_bytes must be between 1 and {MAX_CONFIGURABLE_MESSAGE_BYTES}"
|
||||
)
|
||||
if not 1 <= max_topic_bytes <= MAX_TOPIC_BYTES:
|
||||
raise ValueError(f"max_topic_bytes must be between 1 and {MAX_TOPIC_BYTES}")
|
||||
@@ -367,6 +394,8 @@ def capture_mqtt(
|
||||
duration_seconds: float = 60.0,
|
||||
max_message_bytes: int = DEFAULT_MAX_MESSAGE_BYTES,
|
||||
on_ready: Callable[[], None] | None = None,
|
||||
on_message_recorded: Callable[[CapturedMqttMessage], None] | None = None,
|
||||
should_stop: Callable[[], bool] | None = None,
|
||||
_client_factory: Callable[[], mqtt.Client] | None = None,
|
||||
) -> CaptureSummary:
|
||||
"""Capture the fixed K1 report subscriptions once, without publishing or reconnecting."""
|
||||
@@ -377,8 +406,7 @@ def capture_mqtt(
|
||||
raise ValueError("duration_seconds must be finite and greater than zero")
|
||||
if not 1 <= max_message_bytes <= MAX_CONFIGURABLE_MESSAGE_BYTES:
|
||||
raise ValueError(
|
||||
"max_message_bytes must be between 1 and "
|
||||
f"{MAX_CONFIGURABLE_MESSAGE_BYTES}"
|
||||
f"max_message_bytes must be between 1 and {MAX_CONFIGURABLE_MESSAGE_BYTES}"
|
||||
)
|
||||
|
||||
client = (
|
||||
@@ -453,11 +481,17 @@ def capture_mqtt(
|
||||
if state.error is not None:
|
||||
return
|
||||
try:
|
||||
writer.record(message)
|
||||
recorded = writer.record(message)
|
||||
except MessageTooLargeError as exc:
|
||||
fail("message_too_large", str(exc))
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
fail("capture_error", f"artifact write failed: {type(exc).__name__}: {exc}")
|
||||
return
|
||||
if on_message_recorded is not None:
|
||||
try:
|
||||
on_message_recorded(recorded)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
fail("capture_error", f"preview callback failed: {type(exc).__name__}: {exc}")
|
||||
|
||||
def on_disconnect(
|
||||
_callback_client: mqtt.Client,
|
||||
@@ -487,6 +521,9 @@ def capture_mqtt(
|
||||
|
||||
while state.error is None:
|
||||
now = time.monotonic()
|
||||
if should_stop is not None and should_stop():
|
||||
state.stop_reason = "external_stop"
|
||||
break
|
||||
if state.subscribed and capture_started is None:
|
||||
capture_started = now
|
||||
if on_ready is not None:
|
||||
@@ -523,9 +560,7 @@ def capture_mqtt(
|
||||
fail("capture_error", f"artifact close failed: {type(exc).__name__}: {exc}")
|
||||
|
||||
operation_completed = time.monotonic()
|
||||
capture_elapsed = (
|
||||
0.0 if capture_started is None else operation_completed - capture_started
|
||||
)
|
||||
capture_elapsed = 0.0 if capture_started is None else operation_completed - capture_started
|
||||
|
||||
summary = _build_summary(
|
||||
writer=writer,
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1 @@
|
||||
"""Local-only control API for the K1 live console."""
|
||||
@@ -0,0 +1,316 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import threading
|
||||
from collections.abc import Mapping
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from bleak.exc import BleakError
|
||||
from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect
|
||||
from fastapi.staticfiles import StaticFiles
|
||||
from pydantic import BaseModel, Field
|
||||
|
||||
from k1link import __version__
|
||||
from k1link.artifacts import write_json_atomic
|
||||
from k1link.ble.scanner import scan
|
||||
from k1link.ble.wifi_provisioning import AP_FALLBACK_IPV4, provision_wifi_once
|
||||
from k1link.mqtt import validate_private_ipv4
|
||||
from k1link.viewer.runtime import VisualizationRuntime, new_live_session_dir
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
class BleScanRequest(BaseModel):
|
||||
duration_seconds: float = Field(default=6.0, ge=1.0, le=60.0)
|
||||
|
||||
|
||||
class ConnectRequest(BaseModel):
|
||||
device_id: str = Field(min_length=1, max_length=128)
|
||||
ssid: str = Field(min_length=1, max_length=128)
|
||||
password: str = Field(min_length=1, max_length=256)
|
||||
|
||||
|
||||
class LiveRequest(BaseModel):
|
||||
host: str | None = Field(default=None, max_length=15)
|
||||
duration_seconds: float = Field(default=3600.0, ge=1.0, le=86_400.0)
|
||||
|
||||
|
||||
class ReplayRequest(BaseModel):
|
||||
path: str = Field(min_length=1, max_length=4096)
|
||||
speed: float = Field(default=1.0, ge=0.0, le=100.0)
|
||||
loop: bool = False
|
||||
|
||||
|
||||
class ConsoleService:
|
||||
def __init__(self, repository_root: Path) -> None:
|
||||
self.repository_root = repository_root.resolve()
|
||||
self._lock = threading.Lock()
|
||||
self._devices: list[dict[str, Any]] = []
|
||||
self._selected_device_id: str | None = None
|
||||
self._k1_ip: str | None = None
|
||||
self._operation_phase: str | None = None
|
||||
self._operation_message: str | None = None
|
||||
self.runtime = VisualizationRuntime()
|
||||
|
||||
def state(self) -> dict[str, Any]:
|
||||
runtime = self.runtime.snapshot()
|
||||
metrics = runtime["metrics"]
|
||||
with self._lock:
|
||||
operation_phase = self._operation_phase
|
||||
operation_message = self._operation_message
|
||||
devices = list(self._devices)
|
||||
selected_device_id = self._selected_device_id
|
||||
k1_ip = self._k1_ip
|
||||
|
||||
runtime_active = runtime["source_mode"] != "idle" or runtime["phase"] in {
|
||||
"starting_live",
|
||||
"stopping",
|
||||
"error",
|
||||
}
|
||||
if operation_phase is not None:
|
||||
phase = operation_phase
|
||||
message = operation_message
|
||||
elif runtime_active:
|
||||
phase = runtime["phase"]
|
||||
message = runtime["message"]
|
||||
elif k1_ip is not None:
|
||||
phase = "connected"
|
||||
message = runtime["message"]
|
||||
elif selected_device_id is not None:
|
||||
phase = "device_selected"
|
||||
message = "K1 selected; Wi-Fi credentials have not been written."
|
||||
else:
|
||||
phase = "idle"
|
||||
message = runtime["message"]
|
||||
|
||||
return {
|
||||
"phase": phase,
|
||||
"message": message,
|
||||
"devices": devices,
|
||||
"selected_device_id": selected_device_id,
|
||||
"k1_ip": k1_ip,
|
||||
"foxglove_ws_url": runtime["foxglove_ws_url"],
|
||||
"foxglove_viewer_url": runtime["foxglove_viewer_url"],
|
||||
"source_mode": runtime["source_mode"],
|
||||
"metrics": {
|
||||
"pipeline_ms": metrics["mqtt_to_publish_ms"],
|
||||
"end_to_end_ms": metrics["mqtt_to_publish_ms"],
|
||||
"decode_ms": metrics["decode_publish_ms"],
|
||||
"frame_rate": metrics["pcl_fps"],
|
||||
"frame_rate_hz": metrics["pcl_fps"],
|
||||
"point_count": metrics["last_point_count"],
|
||||
"dropped_preview_frames": metrics["preview_dropped"],
|
||||
**metrics,
|
||||
},
|
||||
}
|
||||
|
||||
async def scan_ble(self, duration_seconds: float) -> dict[str, Any]:
|
||||
self._set_operation("scanning", "Scanning for nearby K1 BLE advertisements.")
|
||||
try:
|
||||
result = await scan(duration_seconds)
|
||||
devices = [
|
||||
{
|
||||
"device_id": item["macos_uuid"],
|
||||
"name": item["local_name"] or item["name"],
|
||||
"rssi": item["rssi"],
|
||||
"address": None,
|
||||
"connectable": True,
|
||||
}
|
||||
for item in result["devices"]
|
||||
if item["k1_name_candidate"]
|
||||
]
|
||||
with self._lock:
|
||||
self._devices = devices
|
||||
if len(devices) == 1:
|
||||
self._selected_device_id = str(devices[0]["device_id"])
|
||||
self._operation_message = f"BLE scan complete: {len(devices)} K1 candidate(s)."
|
||||
finally:
|
||||
with self._lock:
|
||||
self._operation_phase = None
|
||||
return self.state()
|
||||
|
||||
async def connect(self, request: ConnectRequest) -> dict[str, Any]:
|
||||
known_ids = {str(item["device_id"]) for item in self.state()["devices"]}
|
||||
if request.device_id not in known_ids:
|
||||
raise ValueError("device_id must come from the latest K1 BLE scan")
|
||||
self._set_operation(
|
||||
"provisioning",
|
||||
"Sending one explicitly requested Wi-Fi provisioning write.",
|
||||
)
|
||||
session_dir = _new_operation_session_dir(
|
||||
self.repository_root,
|
||||
"viewer_wifi_provisioning",
|
||||
)
|
||||
session_dir.mkdir(parents=True, exist_ok=False)
|
||||
password = request.password
|
||||
try:
|
||||
result = await provision_wifi_once(
|
||||
request.device_id,
|
||||
request.ssid,
|
||||
password,
|
||||
timeout_seconds=45.0,
|
||||
write_mode="auto",
|
||||
)
|
||||
write_json_atomic(session_dir / "provisioning.sensitive.json", result)
|
||||
ipv4 = _provisioned_ipv4(result)
|
||||
write_json_atomic(
|
||||
session_dir / "manifest.redacted.json",
|
||||
{
|
||||
"schema_version": 1,
|
||||
"started_at_utc": result["started_at_utc"],
|
||||
"completed_at_utc": result["completed_at_utc"],
|
||||
"operation": "single_reviewed_wifi_provisioning_write",
|
||||
"profile_id": result["profile_id"],
|
||||
"outcome": result["outcome"],
|
||||
"k1_lan_address_observed": ipv4 is not None,
|
||||
"credentials_persisted_by_connector": False,
|
||||
},
|
||||
)
|
||||
if ipv4 is None:
|
||||
raise RuntimeError(
|
||||
"K1 did not report a non-AP LAN address; no automatic retry was made"
|
||||
)
|
||||
with self._lock:
|
||||
self._selected_device_id = request.device_id
|
||||
self._k1_ip = ipv4
|
||||
self._operation_message = "K1 joined the LAN and reported its private address."
|
||||
finally:
|
||||
password = ""
|
||||
with self._lock:
|
||||
self._operation_phase = None
|
||||
return self.state()
|
||||
|
||||
def start_live(self, host: str | None, duration_seconds: float) -> dict[str, Any]:
|
||||
target = host or self.state()["k1_ip"]
|
||||
if not isinstance(target, str) or not target:
|
||||
raise ValueError("live host is required until BLE provisioning reports a K1 address")
|
||||
target = validate_private_ipv4(target)
|
||||
out_dir = new_live_session_dir(self.repository_root)
|
||||
self.runtime.start_live(target, out_dir, duration_seconds=duration_seconds)
|
||||
return self.state()
|
||||
|
||||
def start_replay(self, path: str, speed: float, loop: bool) -> dict[str, Any]:
|
||||
replay_path = Path(path).expanduser().resolve()
|
||||
if not replay_path.is_relative_to(self.repository_root):
|
||||
raise ValueError("replay path must remain inside this repository")
|
||||
self.runtime.start_replay(replay_path, speed=speed, loop=loop)
|
||||
return self.state()
|
||||
|
||||
def stop(self) -> dict[str, Any]:
|
||||
self.runtime.stop()
|
||||
return self.state()
|
||||
|
||||
def _set_operation(self, phase: str, message: str) -> None:
|
||||
with self._lock:
|
||||
self._operation_phase = phase
|
||||
self._operation_message = message
|
||||
|
||||
|
||||
service = ConsoleService(REPOSITORY_ROOT)
|
||||
app = FastAPI(
|
||||
title="NODE.DC K1 Live Console API",
|
||||
version=__version__,
|
||||
docs_url="/api/docs",
|
||||
redoc_url=None,
|
||||
openapi_url="/api/openapi.json",
|
||||
)
|
||||
|
||||
|
||||
@app.get("/api/health")
|
||||
def health() -> dict[str, Any]:
|
||||
return {
|
||||
"ok": True,
|
||||
"status": "ok",
|
||||
"service": "k1-live-console",
|
||||
"version": __version__,
|
||||
}
|
||||
|
||||
|
||||
@app.get("/api/state")
|
||||
def get_state() -> dict[str, Any]:
|
||||
return service.state()
|
||||
|
||||
|
||||
@app.post("/api/ble/scan")
|
||||
async def scan_ble(request: BleScanRequest) -> dict[str, Any]:
|
||||
try:
|
||||
return await service.scan_ble(request.duration_seconds)
|
||||
except (BleakError, OSError, RuntimeError, ValueError) as exc:
|
||||
raise HTTPException(status_code=502, detail=f"BLE scan failed: {exc}") from exc
|
||||
|
||||
|
||||
@app.post("/api/connect")
|
||||
async def connect(request: ConnectRequest) -> dict[str, Any]:
|
||||
try:
|
||||
return await service.connect(request)
|
||||
except (BleakError, OSError, TimeoutError, RuntimeError, ValueError) as exc:
|
||||
raise HTTPException(status_code=502, detail=f"Wi-Fi provisioning failed: {exc}") from exc
|
||||
|
||||
|
||||
@app.post("/api/session/live")
|
||||
def start_live(request: LiveRequest) -> dict[str, Any]:
|
||||
try:
|
||||
return service.start_live(request.host, request.duration_seconds)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
raise HTTPException(status_code=400, detail=str(exc)) from exc
|
||||
|
||||
|
||||
@app.post("/api/session/replay")
|
||||
def start_replay(request: ReplayRequest) -> dict[str, Any]:
|
||||
try:
|
||||
return service.start_replay(request.path, request.speed, request.loop)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
raise HTTPException(status_code=400, detail=str(exc)) from exc
|
||||
|
||||
|
||||
@app.post("/api/session/stop")
|
||||
def stop_session() -> dict[str, Any]:
|
||||
return service.stop()
|
||||
|
||||
|
||||
@app.websocket("/api/events")
|
||||
async def events(websocket: WebSocket) -> None:
|
||||
await websocket.accept()
|
||||
try:
|
||||
while True:
|
||||
await websocket.send_json({"state": service.state()})
|
||||
await asyncio.sleep(0.5)
|
||||
except WebSocketDisconnect:
|
||||
return
|
||||
|
||||
|
||||
frontend_dist = REPOSITORY_ROOT / "apps" / "k1-viewer" / "dist"
|
||||
if frontend_dist.is_dir():
|
||||
app.mount("/", StaticFiles(directory=frontend_dist, html=True), name="frontend")
|
||||
|
||||
|
||||
def _new_operation_session_dir(repository_root: Path, suffix: str) -> Path:
|
||||
stamp = datetime.now(UTC).strftime("%Y%m%dT%H%M%SZ")
|
||||
base = repository_root / "sessions" / f"{stamp}_{suffix}"
|
||||
candidate = base
|
||||
serial = 1
|
||||
while candidate.exists():
|
||||
serial += 1
|
||||
candidate = base.with_name(f"{base.name}_{serial:02d}")
|
||||
return candidate
|
||||
|
||||
|
||||
def _provisioned_ipv4(result: Mapping[str, Any]) -> str | None:
|
||||
observations = result.get("observations")
|
||||
if not isinstance(observations, list):
|
||||
return None
|
||||
for observation in reversed(observations):
|
||||
if not isinstance(observation, dict):
|
||||
continue
|
||||
status = observation.get("status")
|
||||
if not isinstance(status, dict):
|
||||
continue
|
||||
address = status.get("ipv4")
|
||||
if isinstance(address, str) and address != AP_FALLBACK_IPV4:
|
||||
try:
|
||||
return validate_private_ipv4(address)
|
||||
except ValueError:
|
||||
continue
|
||||
return None
|
||||
Reference in New Issue
Block a user