feat(perception): integrate calibrated operator pipeline
Add calibrated K1 projection, recorded and near-live perception qualification, unified Rerun operator layers, bounded replay admission, audited viewer controls, worker experiments, and lab evidence.
This commit is contained in:
DCCONSTRUCTIONS
@@ -1,15 +1,51 @@
|
||||
"""Bounded, offline analysis of sensitive K1 evidence artifacts."""
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_overlay import (
|
||||
CalibratedOverlayExperiment,
|
||||
CalibratedOverlayExperimentError,
|
||||
run_calibrated_overlay_experiment,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
CalibratedProjectionError,
|
||||
Kb4ProjectionProfile,
|
||||
ProjectedPointCloud,
|
||||
depth_colors,
|
||||
map_points_to_lidar,
|
||||
project_map_points_kb4,
|
||||
quaternion_xyzw_to_rotation_matrix,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.analyze.stream_summary import (
|
||||
DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES,
|
||||
MAX_STREAM_SUMMARY_PAYLOAD_BYTES,
|
||||
StreamSummary,
|
||||
summarize_mqtt_streams,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.analyze.valid_fov import (
|
||||
DEFAULT_EDGE_MARGIN_PIXELS,
|
||||
K1ValidFovMask,
|
||||
K1ValidFovMaskError,
|
||||
prepare_k1_valid_fov_mask,
|
||||
validate_k1_valid_fov_mask,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"CalibratedOverlayExperiment",
|
||||
"CalibratedOverlayExperimentError",
|
||||
"CalibratedProjectionError",
|
||||
"DEFAULT_EDGE_MARGIN_PIXELS",
|
||||
"DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES",
|
||||
"Kb4ProjectionProfile",
|
||||
"K1ValidFovMask",
|
||||
"K1ValidFovMaskError",
|
||||
"MAX_STREAM_SUMMARY_PAYLOAD_BYTES",
|
||||
"ProjectedPointCloud",
|
||||
"StreamSummary",
|
||||
"depth_colors",
|
||||
"map_points_to_lidar",
|
||||
"project_map_points_kb4",
|
||||
"prepare_k1_valid_fov_mask",
|
||||
"quaternion_xyzw_to_rotation_matrix",
|
||||
"run_calibrated_overlay_experiment",
|
||||
"summarize_mqtt_streams",
|
||||
"validate_k1_valid_fov_mask",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,950 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import shutil
|
||||
import stat
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from contextlib import suppress
|
||||
from dataclasses import dataclass
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from uuid import uuid4
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
import rerun as rr
|
||||
from PIL import Image, ImageDraw
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
Kb4ProjectionProfile,
|
||||
ProjectedPointCloud,
|
||||
depth_colors,
|
||||
project_map_points_kb4,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
||||
K1FactoryCalibration,
|
||||
parse_k1_factory_calibration,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.calibration_snapshot import (
|
||||
CALIBRATION_SNAPSHOT_MANIFEST_VERSION,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.mqtt.capture import (
|
||||
read_capture_clock_envelope,
|
||||
read_capture_clock_origin,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.streams import (
|
||||
LioPointCloudFrame,
|
||||
LioPoseFrame,
|
||||
decode_lio_pcl,
|
||||
decode_lio_pose,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
|
||||
from k1link.sessions import inspect_recorded_media_epoch
|
||||
|
||||
CALIBRATED_OVERLAY_SCHEMA = "missioncore.k1-calibrated-overlay-experiment/v1"
|
||||
CALIBRATED_OVERLAY_SUFFIX = "k1_calibrated_overlay"
|
||||
MAX_JSON_BYTES = 2 * 1024 * 1024
|
||||
MAX_INDEX_LINE_BYTES = 64 * 1024
|
||||
MAX_OPERATOR_NOTES_BYTES = 64 * 1024
|
||||
FFMPEG_TIMEOUT_SECONDS = 180.0
|
||||
MAX_SYNC_DELTA_SECONDS = 0.25
|
||||
_READ_CHUNK_BYTES = 1024 * 1024
|
||||
|
||||
RgbImage = npt.NDArray[np.uint8]
|
||||
|
||||
|
||||
class CalibratedOverlayExperimentError(RuntimeError):
|
||||
"""Raised when a recorded calibrated-overlay experiment cannot be sealed."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CalibratedOverlayExperiment:
|
||||
experiment_id: str
|
||||
experiment_root: Path
|
||||
manifest_path: Path
|
||||
rerun_path: Path
|
||||
mosaic_path: Path
|
||||
frame_count: int
|
||||
source_id: str
|
||||
calibration_content_identity: str
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class _CameraFrameAnchor:
|
||||
requested_video_offset_seconds: float
|
||||
sequence: int
|
||||
session_time_seconds: float
|
||||
host_epoch_ns: int
|
||||
host_monotonic_ns: int
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class _LidarSample:
|
||||
camera: _CameraFrameAnchor
|
||||
point_session_time_seconds: float
|
||||
pose_session_time_seconds: float
|
||||
point_frame: LioPointCloudFrame
|
||||
pose_frame: LioPoseFrame
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class _RenderedFrame:
|
||||
camera: _CameraFrameAnchor
|
||||
lidar: _LidarSample
|
||||
image_rgb: RgbImage
|
||||
overlay_rgb: RgbImage
|
||||
projection: ProjectedPointCloud
|
||||
colors_rgb: RgbImage
|
||||
camera_artifact_name: str
|
||||
artifact_name: str
|
||||
|
||||
|
||||
def run_calibrated_overlay_experiment(
|
||||
*,
|
||||
session_root: Path,
|
||||
calibration_snapshot_root: Path,
|
||||
source_id: str,
|
||||
video_offsets_seconds: tuple[float, ...],
|
||||
output_root: Path,
|
||||
ffmpeg_path: Path,
|
||||
temporal_offset_seconds: float = 0.0,
|
||||
) -> CalibratedOverlayExperiment:
|
||||
"""Seal a derived LiDAR→KB4→camera diagnostic without changing native evidence."""
|
||||
|
||||
started_monotonic_ns = time.monotonic_ns()
|
||||
session = session_root.expanduser().resolve(strict=True)
|
||||
if not session.is_dir() or session.name in {"", ".", ".."}:
|
||||
raise CalibratedOverlayExperimentError("session root is invalid")
|
||||
if not video_offsets_seconds:
|
||||
raise CalibratedOverlayExperimentError("at least one video offset is required")
|
||||
if len(video_offsets_seconds) > 16:
|
||||
raise CalibratedOverlayExperimentError("at most sixteen diagnostic frames are allowed")
|
||||
offsets = tuple(sorted(video_offsets_seconds))
|
||||
if len(set(offsets)) != len(offsets) or any(
|
||||
not math.isfinite(value) or value < 0.0 for value in offsets
|
||||
):
|
||||
raise CalibratedOverlayExperimentError(
|
||||
"video offsets must be unique finite non-negative seconds"
|
||||
)
|
||||
if not math.isfinite(temporal_offset_seconds) or abs(temporal_offset_seconds) > 5.0:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
"temporal offset must be finite and inside the reviewed ±5 second window"
|
||||
)
|
||||
|
||||
calibration, calibration_identity = _load_calibration_snapshot(
|
||||
calibration_snapshot_root
|
||||
)
|
||||
profile = Kb4ProjectionProfile.from_factory_calibration(calibration, source_id)
|
||||
capture_root = session / "captures" / "mqtt_live"
|
||||
raw_path = capture_root / "mqtt.raw.k1mqtt"
|
||||
metadata_path = capture_root / "mqtt.metadata.jsonl"
|
||||
summary_path = capture_root / "mqtt.summary.json"
|
||||
clock_origin_path = capture_root / "mqtt.timeline.origin.json"
|
||||
clock_summary = _read_json(summary_path, MAX_JSON_BYTES)
|
||||
clock_name = _required_text(
|
||||
_required_mapping(clock_summary.get("artifacts"), "artifacts").get(
|
||||
"capture_clock"
|
||||
),
|
||||
"capture clock filename",
|
||||
)
|
||||
clock_path = capture_root / clock_name
|
||||
capture_clock = read_capture_clock_envelope(clock_path)
|
||||
capture_origin = read_capture_clock_origin(clock_origin_path)
|
||||
if (
|
||||
capture_clock.started_at_epoch_ns != capture_origin.started_at_epoch_ns
|
||||
or capture_clock.started_monotonic_ns != capture_origin.started_monotonic_ns
|
||||
):
|
||||
raise CalibratedOverlayExperimentError("MQTT capture clock does not match its origin")
|
||||
if not raw_path.is_file() or not metadata_path.is_file():
|
||||
raise CalibratedOverlayExperimentError("sealed MQTT evidence is incomplete")
|
||||
|
||||
camera_epoch_root = session / "media" / source_id / "epoch-1"
|
||||
inspected_epoch = inspect_recorded_media_epoch(
|
||||
camera_epoch_root,
|
||||
expected_source_name=source_id,
|
||||
origin_epoch_ns=capture_origin.started_at_epoch_ns,
|
||||
origin_monotonic_ns=capture_origin.started_monotonic_ns,
|
||||
)
|
||||
camera_duration_seconds = (
|
||||
inspected_epoch.timeline_end_seconds - inspected_epoch.timeline_start_seconds
|
||||
)
|
||||
if offsets[-1] >= camera_duration_seconds:
|
||||
raise CalibratedOverlayExperimentError("a video offset is outside the camera epoch")
|
||||
camera_anchors = _select_camera_anchors(
|
||||
camera_epoch_root / "index.jsonl",
|
||||
expected_count=len(inspected_epoch.segments),
|
||||
offsets_seconds=offsets,
|
||||
timeline_start_seconds=inspected_epoch.timeline_start_seconds,
|
||||
origin_monotonic_ns=capture_origin.started_monotonic_ns,
|
||||
)
|
||||
camera_images = _extract_camera_frames(
|
||||
ffmpeg_path.expanduser().resolve(strict=True),
|
||||
init_path=inspected_epoch.init_path,
|
||||
segment_paths=tuple(segment.path for segment in inspected_epoch.segments),
|
||||
frame_sequences=tuple(anchor.sequence for anchor in camera_anchors),
|
||||
width=profile.width,
|
||||
height=profile.height,
|
||||
)
|
||||
lidar_samples = _select_lidar_samples(
|
||||
raw_path,
|
||||
camera_anchors,
|
||||
origin_monotonic_ns=capture_origin.started_monotonic_ns,
|
||||
temporal_offset_seconds=temporal_offset_seconds,
|
||||
)
|
||||
|
||||
created_at = datetime.now(UTC)
|
||||
experiment_uuid = uuid4().hex
|
||||
experiment_id = (
|
||||
f"{created_at.strftime('%Y%m%dT%H%M%SZ')}_{CALIBRATED_OVERLAY_SUFFIX}_"
|
||||
f"{experiment_uuid[:12]}"
|
||||
)
|
||||
private_root = output_root.expanduser().resolve() / "private" / "perception-experiments"
|
||||
private_root.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
_chmod_private(private_root)
|
||||
final_root = private_root / experiment_id
|
||||
staging_root = private_root / f".{experiment_id}.incomplete"
|
||||
staging_root.mkdir(mode=0o700, exist_ok=False)
|
||||
published = False
|
||||
try:
|
||||
rendered: list[_RenderedFrame] = []
|
||||
for image_rgb, camera_anchor, lidar_sample in zip(
|
||||
camera_images,
|
||||
camera_anchors,
|
||||
lidar_samples,
|
||||
strict=True,
|
||||
):
|
||||
positions = np.asarray(
|
||||
[
|
||||
point.scaled_xyz(lidar_sample.point_frame.header.scaler)
|
||||
for point in lidar_sample.point_frame.points
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
projection = project_map_points_kb4(
|
||||
positions,
|
||||
position_map_xyz=lidar_sample.pose_frame.position_xyz,
|
||||
orientation_map_from_lidar_xyzw=(
|
||||
lidar_sample.pose_frame.orientation_xyzw
|
||||
),
|
||||
profile=profile,
|
||||
)
|
||||
if projection.projected_point_count == 0:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
f"camera frame {camera_anchor.sequence} has no projected LiDAR points"
|
||||
)
|
||||
colors = depth_colors(projection.depths_m)
|
||||
overlay = _render_overlay(
|
||||
image_rgb,
|
||||
projection,
|
||||
colors,
|
||||
camera_anchor=camera_anchor,
|
||||
lidar_sample=lidar_sample,
|
||||
)
|
||||
artifact_name = (
|
||||
f"frame-{camera_anchor.sequence:06d}-"
|
||||
f"session-{round(camera_anchor.session_time_seconds * 1000):09d}ms.png"
|
||||
)
|
||||
camera_artifact_name = (
|
||||
f"camera-{camera_anchor.sequence:06d}-"
|
||||
f"session-{round(camera_anchor.session_time_seconds * 1000):09d}ms.png"
|
||||
)
|
||||
_write_image_exclusive(staging_root / camera_artifact_name, image_rgb)
|
||||
_write_image_exclusive(staging_root / artifact_name, overlay)
|
||||
rendered.append(
|
||||
_RenderedFrame(
|
||||
camera=camera_anchor,
|
||||
lidar=lidar_sample,
|
||||
image_rgb=image_rgb,
|
||||
overlay_rgb=overlay,
|
||||
projection=projection,
|
||||
colors_rgb=colors,
|
||||
camera_artifact_name=camera_artifact_name,
|
||||
artifact_name=artifact_name,
|
||||
)
|
||||
)
|
||||
|
||||
mosaic_path = staging_root / "overlay-mosaic.png"
|
||||
_write_image_exclusive(mosaic_path, _mosaic(tuple(rendered)))
|
||||
rerun_path = staging_root / "diagnostic.rrd"
|
||||
_write_rerun_diagnostic(
|
||||
rerun_path,
|
||||
experiment_id=experiment_id,
|
||||
rendered=tuple(rendered),
|
||||
calibration_identity=calibration_identity,
|
||||
profile=profile,
|
||||
)
|
||||
operator_notes = _operator_notes(
|
||||
experiment_id=experiment_id,
|
||||
session_id=session.name,
|
||||
source_id=source_id,
|
||||
calibration_identity=calibration_identity,
|
||||
temporal_offset_seconds=temporal_offset_seconds,
|
||||
)
|
||||
if len(operator_notes) > MAX_OPERATOR_NOTES_BYTES:
|
||||
raise CalibratedOverlayExperimentError("operator notes exceed their byte bound")
|
||||
_write_exclusive(staging_root / "operator-notes.md", operator_notes)
|
||||
|
||||
input_identity = {
|
||||
"session_id": session.name,
|
||||
"source_id": source_id,
|
||||
"raw_mqtt_sha256": _verified_raw_sha256(raw_path, clock_summary),
|
||||
"camera_stream_sha256": _required_text(
|
||||
_read_json(camera_epoch_root / "summary.json", MAX_JSON_BYTES).get(
|
||||
"stream_sha256"
|
||||
),
|
||||
"camera stream sha256",
|
||||
),
|
||||
"calibration_content_identity_sha256": calibration_identity,
|
||||
"video_offsets_seconds": list(offsets),
|
||||
"temporal_offset_seconds": temporal_offset_seconds,
|
||||
"projection_contract": "map→inverse(T_map_from_lidar)→T_camera_1_from_lidar→KB4",
|
||||
}
|
||||
generation_sha256 = hashlib.sha256(_canonical_json(input_identity)).hexdigest()
|
||||
frame_documents = [_frame_document(item) for item in rendered]
|
||||
output_names = [
|
||||
*(item.camera_artifact_name for item in rendered),
|
||||
*(item.artifact_name for item in rendered),
|
||||
mosaic_path.name,
|
||||
rerun_path.name,
|
||||
"operator-notes.md",
|
||||
]
|
||||
manifest = {
|
||||
"schema_version": CALIBRATED_OVERLAY_SCHEMA,
|
||||
"experiment_id": experiment_id,
|
||||
"created_at_utc": created_at.isoformat(timespec="milliseconds").replace(
|
||||
"+00:00", "Z"
|
||||
),
|
||||
"completed_monotonic_ns": time.monotonic_ns(),
|
||||
"elapsed_seconds": round(
|
||||
(time.monotonic_ns() - started_monotonic_ns) / 1_000_000_000,
|
||||
6,
|
||||
),
|
||||
"classification": "private-derived-calibration-diagnostic",
|
||||
"generation_sha256": generation_sha256,
|
||||
"input": input_identity,
|
||||
"camera_epoch": {
|
||||
"codec_epoch": 1,
|
||||
"resolution": [profile.width, profile.height],
|
||||
"timeline_start_seconds": inspected_epoch.timeline_start_seconds,
|
||||
"timeline_end_seconds": inspected_epoch.timeline_end_seconds,
|
||||
"synchronization": "host-arrival-best-effort",
|
||||
},
|
||||
"geometry": {
|
||||
"calibration_slot": profile.calibration_slot,
|
||||
"camera_model": "kb4",
|
||||
"transform_notation": "T_destination_from_source",
|
||||
"point_frame": "map",
|
||||
"pose_interpretation": "T_map_from_lidar",
|
||||
"behind_camera_policy": "reject-z-less-than-or-equal-to-zero",
|
||||
"occlusion_policy": "diagnostic-draw-far-to-near",
|
||||
},
|
||||
"frames": frame_documents,
|
||||
"outputs": [_artifact_document(staging_root / name) for name in output_names],
|
||||
"acceptance": {
|
||||
"geometry_input_subgate": "accepted",
|
||||
"diagnostic_layer": "generated",
|
||||
"measured_reprojection": "pending-static-landmark-correspondences",
|
||||
"temporal_calibration": "unverified-host-arrival-only",
|
||||
"p0": "not-yet-accepted",
|
||||
},
|
||||
}
|
||||
_write_exclusive(
|
||||
staging_root / "manifest.redacted.json",
|
||||
json.dumps(
|
||||
manifest,
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
sort_keys=True,
|
||||
allow_nan=False,
|
||||
).encode("utf-8")
|
||||
+ b"\n",
|
||||
)
|
||||
_fsync_directory(staging_root)
|
||||
os.rename(staging_root, final_root)
|
||||
_fsync_directory(private_root)
|
||||
published = True
|
||||
finally:
|
||||
if not published and staging_root.exists():
|
||||
shutil.rmtree(staging_root)
|
||||
|
||||
return CalibratedOverlayExperiment(
|
||||
experiment_id=experiment_id,
|
||||
experiment_root=final_root,
|
||||
manifest_path=final_root / "manifest.redacted.json",
|
||||
rerun_path=final_root / "diagnostic.rrd",
|
||||
mosaic_path=final_root / "overlay-mosaic.png",
|
||||
frame_count=len(camera_anchors),
|
||||
source_id=source_id,
|
||||
calibration_content_identity=calibration_identity,
|
||||
)
|
||||
|
||||
|
||||
def _load_calibration_snapshot(
|
||||
snapshot_root: Path,
|
||||
) -> tuple[K1FactoryCalibration, str]:
|
||||
root = snapshot_root.expanduser().resolve(strict=True)
|
||||
if not root.is_dir():
|
||||
raise CalibratedOverlayExperimentError("calibration snapshot is not a directory")
|
||||
manifest = _read_json(root / "manifest.json", MAX_JSON_BYTES)
|
||||
if manifest.get("schema_version") != CALIBRATION_SNAPSHOT_MANIFEST_VERSION:
|
||||
raise CalibratedOverlayExperimentError("calibration snapshot schema is incompatible")
|
||||
camera_payload = _read_regular(root / "camera.yaml", 64 * 1024)
|
||||
extrinsic_payload = _read_regular(root / "extrinsic_camera_lidar.yaml", 64 * 1024)
|
||||
artifact_digests = {
|
||||
item.get("artifact_name"): item.get("sha256")
|
||||
for item in _required_list(manifest.get("artifacts"), "calibration artifacts")
|
||||
if isinstance(item, dict)
|
||||
}
|
||||
expected = {
|
||||
"camera.yaml": hashlib.sha256(camera_payload).hexdigest(),
|
||||
"extrinsic_camera_lidar.yaml": hashlib.sha256(extrinsic_payload).hexdigest(),
|
||||
}
|
||||
if artifact_digests != expected:
|
||||
raise CalibratedOverlayExperimentError("calibration artifact identity changed")
|
||||
calibration = parse_k1_factory_calibration(camera_payload, extrinsic_payload)
|
||||
if manifest.get("normalized_calibration") != calibration.normalized_profile():
|
||||
raise CalibratedOverlayExperimentError("normalized calibration changed")
|
||||
identity = _required_sha256(
|
||||
manifest.get("content_identity_sha256"),
|
||||
"calibration content identity",
|
||||
)
|
||||
return calibration, identity
|
||||
|
||||
|
||||
def _select_camera_anchors(
|
||||
index_path: Path,
|
||||
*,
|
||||
expected_count: int,
|
||||
offsets_seconds: tuple[float, ...],
|
||||
timeline_start_seconds: float,
|
||||
origin_monotonic_ns: int,
|
||||
) -> tuple[_CameraFrameAnchor, ...]:
|
||||
targets = tuple(timeline_start_seconds + offset for offset in offsets_seconds)
|
||||
best: list[tuple[float, _CameraFrameAnchor] | None] = [None] * len(targets)
|
||||
with index_path.open("rb") as stream:
|
||||
for expected_sequence in range(1, expected_count + 1):
|
||||
line = stream.readline(MAX_INDEX_LINE_BYTES + 1)
|
||||
if not line or len(line) > MAX_INDEX_LINE_BYTES or not line.endswith(b"\n"):
|
||||
raise CalibratedOverlayExperimentError("camera index is incomplete or unbounded")
|
||||
try:
|
||||
entry = json.loads(line)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
"camera index contains invalid JSON"
|
||||
) from exc
|
||||
if (
|
||||
not isinstance(entry, dict)
|
||||
or entry.get("sequence") != expected_sequence
|
||||
or entry.get("kind") != "media"
|
||||
or entry.get("path") != f"segments/{expected_sequence}.m4s"
|
||||
):
|
||||
raise CalibratedOverlayExperimentError("camera index sequence is inconsistent")
|
||||
host_monotonic_ns = entry.get("host_monotonic_ns")
|
||||
host_epoch_ns = entry.get("host_epoch_ns")
|
||||
if (
|
||||
not isinstance(host_monotonic_ns, int)
|
||||
or host_monotonic_ns < origin_monotonic_ns
|
||||
or not isinstance(host_epoch_ns, int)
|
||||
or host_epoch_ns < 0
|
||||
):
|
||||
raise CalibratedOverlayExperimentError("camera index time is invalid")
|
||||
session_time = (host_monotonic_ns - origin_monotonic_ns) / 1_000_000_000
|
||||
for index, (offset, target) in enumerate(zip(offsets_seconds, targets, strict=True)):
|
||||
delta = abs(session_time - target)
|
||||
current = best[index]
|
||||
if current is None or delta < current[0]:
|
||||
best[index] = (
|
||||
delta,
|
||||
_CameraFrameAnchor(
|
||||
requested_video_offset_seconds=offset,
|
||||
sequence=expected_sequence,
|
||||
session_time_seconds=session_time,
|
||||
host_epoch_ns=host_epoch_ns,
|
||||
host_monotonic_ns=host_monotonic_ns,
|
||||
),
|
||||
)
|
||||
if stream.read(1):
|
||||
raise CalibratedOverlayExperimentError("camera index has undeclared rows")
|
||||
anchors_list: list[_CameraFrameAnchor] = []
|
||||
for item in best:
|
||||
if item is None:
|
||||
raise CalibratedOverlayExperimentError("camera offset has no frame")
|
||||
anchors_list.append(item[1])
|
||||
anchors = tuple(anchors_list)
|
||||
if len({item.sequence for item in anchors}) != len(anchors):
|
||||
raise CalibratedOverlayExperimentError("camera offsets do not select unique frames")
|
||||
return anchors
|
||||
|
||||
|
||||
def _extract_camera_frames(
|
||||
ffmpeg_path: Path,
|
||||
*,
|
||||
init_path: Path,
|
||||
segment_paths: tuple[Path, ...],
|
||||
frame_sequences: tuple[int, ...],
|
||||
width: int,
|
||||
height: int,
|
||||
) -> tuple[RgbImage, ...]:
|
||||
if not ffmpeg_path.is_file():
|
||||
raise CalibratedOverlayExperimentError("ffmpeg is unavailable")
|
||||
frame_indices = tuple(sequence - 1 for sequence in frame_sequences)
|
||||
select = "+".join(f"eq(n\\,{index})" for index in frame_indices)
|
||||
read_fd, write_fd = os.pipe()
|
||||
process = subprocess.Popen(
|
||||
[
|
||||
str(ffmpeg_path),
|
||||
"-hide_banner",
|
||||
"-loglevel",
|
||||
"error",
|
||||
"-i",
|
||||
"pipe:0",
|
||||
"-vf",
|
||||
f"select={select}",
|
||||
"-fps_mode",
|
||||
"passthrough",
|
||||
"-frames:v",
|
||||
str(len(frame_indices)),
|
||||
"-f",
|
||||
"rawvideo",
|
||||
"-pix_fmt",
|
||||
"rgb24",
|
||||
"pipe:1",
|
||||
],
|
||||
stdin=read_fd,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE,
|
||||
shell=False,
|
||||
)
|
||||
os.close(read_fd)
|
||||
feeder_errors: list[BaseException] = []
|
||||
|
||||
def feed() -> None:
|
||||
try:
|
||||
with os.fdopen(write_fd, "wb", buffering=0) as sink:
|
||||
for path in (init_path, *segment_paths):
|
||||
with path.open("rb") as source:
|
||||
shutil.copyfileobj(source, sink, length=_READ_CHUNK_BYTES)
|
||||
except BrokenPipeError:
|
||||
return
|
||||
except BaseException as exc: # pragma: no cover - defensive child-pipe boundary
|
||||
feeder_errors.append(exc)
|
||||
|
||||
feeder = threading.Thread(target=feed, name="k1-calibrated-overlay-ffmpeg", daemon=True)
|
||||
feeder.start()
|
||||
try:
|
||||
stdout, stderr = process.communicate(timeout=FFMPEG_TIMEOUT_SECONDS)
|
||||
except subprocess.TimeoutExpired as exc:
|
||||
process.kill()
|
||||
process.communicate()
|
||||
raise CalibratedOverlayExperimentError("ffmpeg frame extraction timed out") from exc
|
||||
feeder.join(timeout=5.0)
|
||||
if feeder.is_alive():
|
||||
raise CalibratedOverlayExperimentError("camera archive feeder did not stop")
|
||||
if feeder_errors:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
"camera archive streaming failed"
|
||||
) from feeder_errors[0]
|
||||
if process.returncode != 0:
|
||||
message = stderr.decode("utf-8", errors="replace").strip()[-1000:]
|
||||
raise CalibratedOverlayExperimentError(
|
||||
f"ffmpeg rejected the sealed camera epoch: {message}"
|
||||
)
|
||||
frame_bytes = width * height * 3
|
||||
if len(stdout) != frame_bytes * len(frame_indices):
|
||||
raise CalibratedOverlayExperimentError("ffmpeg returned an unexpected frame count")
|
||||
return tuple(
|
||||
np.frombuffer(stdout, dtype=np.uint8, count=frame_bytes, offset=index * frame_bytes)
|
||||
.reshape((height, width, 3))
|
||||
.copy()
|
||||
for index in range(len(frame_indices))
|
||||
)
|
||||
|
||||
|
||||
def _select_lidar_samples(
|
||||
raw_path: Path,
|
||||
camera_anchors: tuple[_CameraFrameAnchor, ...],
|
||||
*,
|
||||
origin_monotonic_ns: int,
|
||||
temporal_offset_seconds: float,
|
||||
) -> tuple[_LidarSample, ...]:
|
||||
targets = tuple(
|
||||
anchor.session_time_seconds + temporal_offset_seconds for anchor in camera_anchors
|
||||
)
|
||||
best_points: list[tuple[float, float, LioPointCloudFrame] | None] = [None] * len(targets)
|
||||
nearby_poses: list[list[tuple[float, LioPoseFrame]]] = [[] for _ in targets]
|
||||
last_target = max(targets)
|
||||
first_target = min(targets)
|
||||
for message in iter_replay_messages(raw_path):
|
||||
monotonic_ns = message.received_monotonic_ns
|
||||
if monotonic_ns is None:
|
||||
raise CalibratedOverlayExperimentError("MQTT metadata has no monotonic time")
|
||||
session_time = (monotonic_ns - origin_monotonic_ns) / 1_000_000_000
|
||||
if session_time < first_target - 1.0:
|
||||
continue
|
||||
if session_time > last_target + 1.0:
|
||||
break
|
||||
if message.topic.endswith("/lio_pcl"):
|
||||
for index, target in enumerate(targets):
|
||||
delta = abs(session_time - target)
|
||||
current = best_points[index]
|
||||
if delta <= MAX_SYNC_DELTA_SECONDS and (current is None or delta < current[0]):
|
||||
best_points[index] = (delta, session_time, decode_lio_pcl(message.payload))
|
||||
elif message.topic.endswith("/lio_pose"):
|
||||
for index, target in enumerate(targets):
|
||||
if abs(session_time - target) <= 0.5:
|
||||
nearby_poses[index].append((session_time, decode_lio_pose(message.payload)))
|
||||
|
||||
samples: list[_LidarSample] = []
|
||||
for index, anchor in enumerate(camera_anchors):
|
||||
point = best_points[index]
|
||||
if point is None or not nearby_poses[index]:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
f"camera frame {anchor.sequence} has no temporally compatible LiDAR sample"
|
||||
)
|
||||
_delta, point_time, point_frame = point
|
||||
pose_time, pose_frame = min(
|
||||
nearby_poses[index],
|
||||
key=lambda item: abs(item[0] - point_time),
|
||||
)
|
||||
if abs(pose_time - point_time) > MAX_SYNC_DELTA_SECONDS:
|
||||
raise CalibratedOverlayExperimentError(
|
||||
f"camera frame {anchor.sequence} has no compatible pose"
|
||||
)
|
||||
samples.append(
|
||||
_LidarSample(
|
||||
camera=anchor,
|
||||
point_session_time_seconds=point_time,
|
||||
pose_session_time_seconds=pose_time,
|
||||
point_frame=point_frame,
|
||||
pose_frame=pose_frame,
|
||||
)
|
||||
)
|
||||
return tuple(samples)
|
||||
|
||||
|
||||
def _render_overlay(
|
||||
image_rgb: RgbImage,
|
||||
projection: ProjectedPointCloud,
|
||||
colors_rgb: RgbImage,
|
||||
*,
|
||||
camera_anchor: _CameraFrameAnchor,
|
||||
lidar_sample: _LidarSample,
|
||||
) -> RgbImage:
|
||||
image = Image.fromarray(image_rgb, mode="RGB").convert("RGBA")
|
||||
draw = ImageDraw.Draw(image, "RGBA")
|
||||
order = np.argsort(projection.depths_m)[::-1]
|
||||
for index in order:
|
||||
u, v = projection.pixels_xy[index]
|
||||
red, green, blue = (int(value) for value in colors_rgb[index])
|
||||
draw.ellipse(
|
||||
(u - 2.0, v - 2.0, u + 2.0, v + 2.0),
|
||||
fill=(red, green, blue, 205),
|
||||
)
|
||||
point_delta_ms = (
|
||||
lidar_sample.point_session_time_seconds - camera_anchor.session_time_seconds
|
||||
) * 1000.0
|
||||
draw.rectangle((0, 0, image.width, 34), fill=(0, 0, 0, 190))
|
||||
draw.text(
|
||||
(8, 9),
|
||||
(
|
||||
f"session={camera_anchor.session_time_seconds:.3f}s "
|
||||
f"LiDAR-camera={point_delta_ms:+.1f}ms "
|
||||
f"projected={projection.projected_point_count}/"
|
||||
f"{projection.source_point_count}"
|
||||
),
|
||||
fill=(255, 255, 255, 255),
|
||||
)
|
||||
return np.asarray(image.convert("RGB"), dtype=np.uint8)
|
||||
|
||||
|
||||
def _mosaic(rendered: tuple[_RenderedFrame, ...]) -> RgbImage:
|
||||
columns = 2 if len(rendered) > 1 else 1
|
||||
rows = math.ceil(len(rendered) / columns)
|
||||
width = rendered[0].overlay_rgb.shape[1]
|
||||
height = rendered[0].overlay_rgb.shape[0]
|
||||
canvas = Image.new("RGB", (columns * width, rows * height), "black")
|
||||
for index, item in enumerate(rendered):
|
||||
canvas.paste(
|
||||
Image.fromarray(item.overlay_rgb, mode="RGB"),
|
||||
((index % columns) * width, (index // columns) * height),
|
||||
)
|
||||
return np.asarray(canvas, dtype=np.uint8)
|
||||
|
||||
|
||||
def _write_rerun_diagnostic(
|
||||
path: Path,
|
||||
*,
|
||||
experiment_id: str,
|
||||
rendered: tuple[_RenderedFrame, ...],
|
||||
calibration_identity: str,
|
||||
profile: Kb4ProjectionProfile,
|
||||
) -> None:
|
||||
recording = rr.RecordingStream(
|
||||
"nodedc_mission_core_k1_calibration",
|
||||
recording_id=experiment_id,
|
||||
)
|
||||
recording.set_sinks(rr.FileSink(path, write_footer=True))
|
||||
try:
|
||||
recording.log(
|
||||
"k1/calibrated/contract",
|
||||
rr.TextDocument(
|
||||
"\n".join(
|
||||
(
|
||||
"K1 factory-calibrated diagnostic",
|
||||
f"source={profile.source_id}",
|
||||
f"slot={profile.calibration_slot}",
|
||||
f"calibration={calibration_identity}",
|
||||
"projection=map→inverse(T_map_from_lidar)→T_camera_from_lidar→KB4",
|
||||
"timing=host-arrival-best-effort; P0 temporal acceptance pending",
|
||||
)
|
||||
)
|
||||
),
|
||||
static=True,
|
||||
)
|
||||
for item in rendered:
|
||||
session_time_ns = round(item.camera.session_time_seconds * 1_000_000_000)
|
||||
recording.set_time(
|
||||
"session_time",
|
||||
duration=np.timedelta64(session_time_ns, "ns"),
|
||||
)
|
||||
recording.log("k1/calibrated/camera", rr.Image(item.image_rgb))
|
||||
recording.log(
|
||||
"k1/calibrated/camera/projected_lidar",
|
||||
rr.Points2D(
|
||||
item.projection.pixels_xy.astype(np.float32),
|
||||
colors=item.colors_rgb,
|
||||
radii=rr.Radius.ui_points(2.0),
|
||||
),
|
||||
)
|
||||
recording.flush(timeout_sec=30.0)
|
||||
recording.disconnect()
|
||||
finally:
|
||||
path.chmod(0o600)
|
||||
_fsync_file(path)
|
||||
|
||||
|
||||
def _frame_document(item: _RenderedFrame) -> dict[str, object]:
|
||||
depth = item.projection.depths_m
|
||||
return {
|
||||
"camera_sequence": item.camera.sequence,
|
||||
"requested_video_offset_seconds": item.camera.requested_video_offset_seconds,
|
||||
"camera_session_time_seconds": item.camera.session_time_seconds,
|
||||
"point_session_time_seconds": item.lidar.point_session_time_seconds,
|
||||
"pose_session_time_seconds": item.lidar.pose_session_time_seconds,
|
||||
"point_minus_camera_ms": round(
|
||||
(
|
||||
item.lidar.point_session_time_seconds
|
||||
- item.camera.session_time_seconds
|
||||
)
|
||||
* 1000.0,
|
||||
6,
|
||||
),
|
||||
"pose_minus_point_ms": round(
|
||||
(
|
||||
item.lidar.pose_session_time_seconds
|
||||
- item.lidar.point_session_time_seconds
|
||||
)
|
||||
* 1000.0,
|
||||
6,
|
||||
),
|
||||
"source_points": item.projection.source_point_count,
|
||||
"camera_front_points": item.projection.camera_front_point_count,
|
||||
"projected_points": item.projection.projected_point_count,
|
||||
"projected_fraction": round(
|
||||
item.projection.projected_point_count / item.projection.source_point_count,
|
||||
9,
|
||||
),
|
||||
"depth_m": {
|
||||
"p05": float(np.percentile(depth, 5.0)),
|
||||
"median": float(np.median(depth)),
|
||||
"p95": float(np.percentile(depth, 95.0)),
|
||||
},
|
||||
"artifact": item.artifact_name,
|
||||
"camera_artifact": item.camera_artifact_name,
|
||||
}
|
||||
|
||||
|
||||
def _operator_notes(
|
||||
*,
|
||||
experiment_id: str,
|
||||
session_id: str,
|
||||
source_id: str,
|
||||
calibration_identity: str,
|
||||
temporal_offset_seconds: float,
|
||||
) -> bytes:
|
||||
return (
|
||||
f"# {experiment_id}\n\n"
|
||||
"Offline read-only diagnostic. Native MQTT/camera evidence was not modified.\n\n"
|
||||
f"- session: {session_id}\n"
|
||||
f"- source: {source_id}\n"
|
||||
f"- calibration content identity: {calibration_identity}\n"
|
||||
f"- requested host-arrival temporal offset: {temporal_offset_seconds:+.6f} s\n"
|
||||
"- point frame: K1 map\n"
|
||||
"- pose interpretation: T_map_from_lidar; inverted before factory extrinsic\n"
|
||||
"- camera model: KB4 at admitted 800x600\n\n"
|
||||
"This run proves a reproducible diagnostic layer only. Static landmark pixel "
|
||||
"correspondences and a temporal-offset error budget are still required before "
|
||||
"P0 acceptance.\n"
|
||||
).encode()
|
||||
|
||||
|
||||
def _verified_raw_sha256(raw_path: Path, summary: dict[str, object]) -> str:
|
||||
expected = _required_sha256(
|
||||
_required_mapping(summary.get("artifact_hashes"), "artifact hashes").get(
|
||||
"raw_sha256"
|
||||
),
|
||||
"raw MQTT sha256",
|
||||
)
|
||||
actual = _sha256_file(raw_path)
|
||||
if actual != expected:
|
||||
raise CalibratedOverlayExperimentError("raw MQTT identity changed")
|
||||
return actual
|
||||
|
||||
|
||||
def _artifact_document(path: Path) -> dict[str, object]:
|
||||
metadata = path.stat()
|
||||
if not stat.S_ISREG(metadata.st_mode) or metadata.st_size <= 0:
|
||||
raise CalibratedOverlayExperimentError("derived artifact is unavailable")
|
||||
return {
|
||||
"name": path.name,
|
||||
"bytes": metadata.st_size,
|
||||
"sha256": _sha256_file(path),
|
||||
}
|
||||
|
||||
|
||||
def _read_json(path: Path, max_bytes: int) -> dict[str, object]:
|
||||
payload = _read_regular(path, max_bytes)
|
||||
try:
|
||||
value = json.loads(payload)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise CalibratedOverlayExperimentError(f"{path.name} is not valid JSON") from exc
|
||||
if not isinstance(value, dict):
|
||||
raise CalibratedOverlayExperimentError(f"{path.name} is not a JSON object")
|
||||
return value
|
||||
|
||||
|
||||
def _read_regular(path: Path, max_bytes: int) -> bytes:
|
||||
flags = os.O_RDONLY | getattr(os, "O_CLOEXEC", 0) | getattr(os, "O_NOFOLLOW", 0)
|
||||
descriptor = os.open(path, flags)
|
||||
try:
|
||||
before = os.fstat(descriptor)
|
||||
if not stat.S_ISREG(before.st_mode) or not 1 <= before.st_size <= max_bytes:
|
||||
raise CalibratedOverlayExperimentError(f"{path.name} is outside bounds")
|
||||
chunks: list[bytes] = []
|
||||
remaining = before.st_size
|
||||
while remaining:
|
||||
chunk = os.read(descriptor, min(_READ_CHUNK_BYTES, remaining))
|
||||
if not chunk:
|
||||
break
|
||||
chunks.append(chunk)
|
||||
remaining -= len(chunk)
|
||||
after = os.fstat(descriptor)
|
||||
if (
|
||||
remaining
|
||||
or (before.st_dev, before.st_ino, before.st_size, before.st_mtime_ns)
|
||||
!= (after.st_dev, after.st_ino, after.st_size, after.st_mtime_ns)
|
||||
):
|
||||
raise CalibratedOverlayExperimentError(f"{path.name} changed during read")
|
||||
return b"".join(chunks)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
|
||||
|
||||
def _write_image_exclusive(path: Path, image_rgb: RgbImage) -> None:
|
||||
descriptor = os.open(
|
||||
path,
|
||||
os.O_WRONLY | os.O_CREAT | os.O_EXCL | getattr(os, "O_NOFOLLOW", 0),
|
||||
0o600,
|
||||
)
|
||||
try:
|
||||
with os.fdopen(descriptor, "wb", closefd=False) as stream:
|
||||
Image.fromarray(image_rgb, mode="RGB").save(stream, format="PNG")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
|
||||
|
||||
def _write_exclusive(path: Path, payload: bytes) -> None:
|
||||
descriptor = os.open(
|
||||
path,
|
||||
os.O_WRONLY | os.O_CREAT | os.O_EXCL | getattr(os, "O_NOFOLLOW", 0),
|
||||
0o600,
|
||||
)
|
||||
try:
|
||||
view = memoryview(payload)
|
||||
while view:
|
||||
written = os.write(descriptor, view)
|
||||
view = view[written:]
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
|
||||
|
||||
def _sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(_READ_CHUNK_BYTES), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _canonical_json(value: object) -> bytes:
|
||||
return json.dumps(
|
||||
value,
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
allow_nan=False,
|
||||
).encode("utf-8")
|
||||
|
||||
|
||||
def _required_mapping(value: object, label: str) -> dict[str, object]:
|
||||
if not isinstance(value, dict):
|
||||
raise CalibratedOverlayExperimentError(f"{label} is missing")
|
||||
return value
|
||||
|
||||
|
||||
def _required_list(value: object, label: str) -> list[object]:
|
||||
if not isinstance(value, list):
|
||||
raise CalibratedOverlayExperimentError(f"{label} is missing")
|
||||
return value
|
||||
|
||||
|
||||
def _required_text(value: object, label: str) -> str:
|
||||
if not isinstance(value, str) or not value:
|
||||
raise CalibratedOverlayExperimentError(f"{label} is missing")
|
||||
return value
|
||||
|
||||
|
||||
def _required_sha256(value: object, label: str) -> str:
|
||||
text = _required_text(value, label)
|
||||
if len(text) != 64 or any(character not in "0123456789abcdef" for character in text):
|
||||
raise CalibratedOverlayExperimentError(f"{label} is invalid")
|
||||
return text
|
||||
|
||||
|
||||
def _chmod_private(path: Path) -> None:
|
||||
current = path
|
||||
while current.name in {"private", "perception-experiments"}:
|
||||
with suppress(OSError):
|
||||
current.chmod(0o700)
|
||||
current = current.parent
|
||||
|
||||
|
||||
def _fsync_file(path: Path) -> None:
|
||||
with path.open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def _fsync_directory(path: Path) -> None:
|
||||
descriptor = os.open(path, os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
@@ -0,0 +1,235 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
import numpy.typing as npt
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
||||
ADMITTED_MAIN_STREAM_RESOLUTION,
|
||||
MAIN_CAMERA_SLOT_BY_SOURCE,
|
||||
K1FactoryCalibration,
|
||||
)
|
||||
|
||||
FloatArray = npt.NDArray[np.float64]
|
||||
IntArray = npt.NDArray[np.int64]
|
||||
|
||||
|
||||
class CalibratedProjectionError(ValueError):
|
||||
"""Raised when a calibrated projection input violates the reviewed contract."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class Kb4ProjectionProfile:
|
||||
source_id: str
|
||||
calibration_slot: str
|
||||
width: int
|
||||
height: int
|
||||
intrinsic_fx_fy_cx_cy: tuple[float, float, float, float]
|
||||
distortion_kb4: tuple[float, float, float, float]
|
||||
t_camera_from_lidar: FloatArray
|
||||
|
||||
@classmethod
|
||||
def from_factory_calibration(
|
||||
cls,
|
||||
calibration: K1FactoryCalibration,
|
||||
source_id: str,
|
||||
) -> Kb4ProjectionProfile:
|
||||
try:
|
||||
slot = MAIN_CAMERA_SLOT_BY_SOURCE[source_id]
|
||||
except KeyError as exc:
|
||||
raise CalibratedProjectionError("source is not an admitted K1 main camera") from exc
|
||||
camera = calibration.camera(slot)
|
||||
width, height = ADMITTED_MAIN_STREAM_RESOLUTION
|
||||
scale_x = width / camera.image_width
|
||||
scale_y = height / camera.image_height
|
||||
intrinsic = (
|
||||
camera.intrinsic[0] * scale_x,
|
||||
camera.intrinsic[1] * scale_y,
|
||||
camera.intrinsic[2] * scale_x,
|
||||
camera.intrinsic[3] * scale_y,
|
||||
)
|
||||
transform = np.asarray(calibration.t_camera_from_lidar(slot), dtype=np.float64)
|
||||
transform.setflags(write=False)
|
||||
return cls(
|
||||
source_id=source_id,
|
||||
calibration_slot=slot,
|
||||
width=width,
|
||||
height=height,
|
||||
intrinsic_fx_fy_cx_cy=intrinsic,
|
||||
distortion_kb4=camera.distortion,
|
||||
t_camera_from_lidar=transform,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ProjectedPointCloud:
|
||||
pixels_xy: FloatArray
|
||||
depths_m: FloatArray
|
||||
source_indices: IntArray
|
||||
source_point_count: int
|
||||
camera_front_point_count: int
|
||||
|
||||
@property
|
||||
def projected_point_count(self) -> int:
|
||||
return int(self.pixels_xy.shape[0])
|
||||
|
||||
|
||||
def quaternion_xyzw_to_rotation_matrix(
|
||||
orientation_xyzw: tuple[float, float, float, float],
|
||||
) -> FloatArray:
|
||||
"""Return R_map_from_lidar for a finite, non-zero xyzw quaternion."""
|
||||
|
||||
quaternion = np.asarray(orientation_xyzw, dtype=np.float64)
|
||||
if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
|
||||
raise CalibratedProjectionError("pose quaternion must contain four finite values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if not math.isfinite(norm) or norm < 1e-9:
|
||||
raise CalibratedProjectionError("pose quaternion has no usable norm")
|
||||
x, y, z, w = quaternion / norm
|
||||
rotation = np.asarray(
|
||||
[
|
||||
[
|
||||
1.0 - 2.0 * (y * y + z * z),
|
||||
2.0 * (x * y - z * w),
|
||||
2.0 * (x * z + y * w),
|
||||
],
|
||||
[
|
||||
2.0 * (x * y + z * w),
|
||||
1.0 - 2.0 * (x * x + z * z),
|
||||
2.0 * (y * z - x * w),
|
||||
],
|
||||
[
|
||||
2.0 * (x * z - y * w),
|
||||
2.0 * (y * z + x * w),
|
||||
1.0 - 2.0 * (x * x + y * y),
|
||||
],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
rotation.setflags(write=False)
|
||||
return rotation
|
||||
|
||||
|
||||
def map_points_to_lidar(
|
||||
points_map_xyz: npt.ArrayLike,
|
||||
*,
|
||||
position_map_xyz: tuple[float, float, float],
|
||||
orientation_map_from_lidar_xyzw: tuple[float, float, float, float],
|
||||
) -> FloatArray:
|
||||
"""Invert the K1 T_map_from_lidar pose for row-vector map points."""
|
||||
|
||||
points = _finite_points(points_map_xyz)
|
||||
position = np.asarray(position_map_xyz, dtype=np.float64)
|
||||
if position.shape != (3,) or not np.isfinite(position).all():
|
||||
raise CalibratedProjectionError("pose position must contain three finite values")
|
||||
rotation_map_from_lidar = quaternion_xyzw_to_rotation_matrix(
|
||||
orientation_map_from_lidar_xyzw
|
||||
)
|
||||
# Column-vector form is R.T @ (p_map - t). With row vectors this is
|
||||
# (p_map - t) @ R.
|
||||
return (points - position) @ rotation_map_from_lidar
|
||||
|
||||
|
||||
def project_map_points_kb4(
|
||||
points_map_xyz: npt.ArrayLike,
|
||||
*,
|
||||
position_map_xyz: tuple[float, float, float],
|
||||
orientation_map_from_lidar_xyzw: tuple[float, float, float, float],
|
||||
profile: Kb4ProjectionProfile,
|
||||
) -> ProjectedPointCloud:
|
||||
"""Project one world-frame K1 lio_pcl frame into an admitted camera image."""
|
||||
|
||||
points_map = _finite_points(points_map_xyz)
|
||||
points_lidar = map_points_to_lidar(
|
||||
points_map,
|
||||
position_map_xyz=position_map_xyz,
|
||||
orientation_map_from_lidar_xyzw=orientation_map_from_lidar_xyzw,
|
||||
)
|
||||
transform = profile.t_camera_from_lidar
|
||||
if transform.shape != (4, 4) or not np.isfinite(transform).all():
|
||||
raise CalibratedProjectionError("camera transform must be a finite 4x4 matrix")
|
||||
points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3]
|
||||
x, y, z = points_camera.T
|
||||
front = z > 1e-6
|
||||
front_indices = np.flatnonzero(front)
|
||||
front_points = points_camera[front]
|
||||
if front_points.size == 0:
|
||||
return ProjectedPointCloud(
|
||||
pixels_xy=np.empty((0, 2), dtype=np.float64),
|
||||
depths_m=np.empty((0,), dtype=np.float64),
|
||||
source_indices=np.empty((0,), dtype=np.int64),
|
||||
source_point_count=int(points_map.shape[0]),
|
||||
camera_front_point_count=0,
|
||||
)
|
||||
|
||||
x, y, z = front_points.T
|
||||
radial = np.hypot(x, y)
|
||||
theta = np.arctan2(radial, z)
|
||||
theta_squared = theta * theta
|
||||
k1, k2, k3, k4 = profile.distortion_kb4
|
||||
theta_distorted = theta * (
|
||||
1.0
|
||||
+ k1 * theta_squared
|
||||
+ k2 * theta_squared**2
|
||||
+ k3 * theta_squared**3
|
||||
+ k4 * theta_squared**4
|
||||
)
|
||||
radial_scale = np.divide(
|
||||
theta_distorted,
|
||||
radial,
|
||||
out=np.zeros_like(theta_distorted),
|
||||
where=radial > 1e-12,
|
||||
)
|
||||
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
|
||||
u = fx * x * radial_scale + cx
|
||||
v = fy * y * radial_scale + cy
|
||||
in_frame = (
|
||||
np.isfinite(u)
|
||||
& np.isfinite(v)
|
||||
& (u >= 0.0)
|
||||
& (u < profile.width)
|
||||
& (v >= 0.0)
|
||||
& (v < profile.height)
|
||||
)
|
||||
pixels = np.column_stack((u[in_frame], v[in_frame])).astype(np.float64, copy=False)
|
||||
depths = z[in_frame].astype(np.float64, copy=False)
|
||||
indices = front_indices[in_frame].astype(np.int64, copy=False)
|
||||
return ProjectedPointCloud(
|
||||
pixels_xy=pixels,
|
||||
depths_m=depths,
|
||||
source_indices=indices,
|
||||
source_point_count=int(points_map.shape[0]),
|
||||
camera_front_point_count=int(front_points.shape[0]),
|
||||
)
|
||||
|
||||
|
||||
def depth_colors(depths_m: npt.ArrayLike) -> npt.NDArray[np.uint8]:
|
||||
"""Return deterministic blue→cyan→green→yellow→red diagnostic colors."""
|
||||
|
||||
depths = np.asarray(depths_m, dtype=np.float64)
|
||||
if depths.ndim != 1 or not np.isfinite(depths).all() or np.any(depths <= 0.0):
|
||||
raise CalibratedProjectionError("projected depths must be a positive finite vector")
|
||||
if depths.size == 0:
|
||||
return np.empty((0, 3), dtype=np.uint8)
|
||||
lower, upper = np.percentile(depths, [5.0, 95.0])
|
||||
span = max(float(upper - lower), 1e-9)
|
||||
normalized = np.clip((depths - lower) / span, 0.0, 1.0)
|
||||
colors = np.column_stack(
|
||||
(
|
||||
255.0 * (1.0 - normalized),
|
||||
255.0 * (1.0 - np.abs(2.0 * normalized - 1.0)),
|
||||
255.0 * normalized,
|
||||
)
|
||||
)
|
||||
return np.rint(colors).astype(np.uint8)
|
||||
|
||||
|
||||
def _finite_points(points_xyz: npt.ArrayLike) -> FloatArray:
|
||||
points = np.asarray(points_xyz, dtype=np.float64)
|
||||
if points.ndim != 2 or points.shape[1:] != (3,):
|
||||
raise CalibratedProjectionError("point cloud must have shape (N, 3)")
|
||||
if not np.isfinite(points).all():
|
||||
raise CalibratedProjectionError("point cloud contains a non-finite coordinate")
|
||||
return points
|
||||
@@ -0,0 +1,472 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import secrets
|
||||
import shutil
|
||||
import stat
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
|
||||
from k1link.artifacts import utc_now_iso, write_json_atomic
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
Kb4ProjectionProfile,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
||||
MAIN_CAMERA_SLOT_BY_SOURCE,
|
||||
parse_k1_factory_calibration,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.calibration_snapshot import (
|
||||
CALIBRATION_SNAPSHOT_MANIFEST_VERSION,
|
||||
)
|
||||
|
||||
VALID_FOV_MASK_SCHEMA = "missioncore.k1-valid-fov-mask/v1"
|
||||
VALID_FOV_MASK_IDENTITY_SCHEMA = "missioncore.k1-valid-fov-mask-identity/v1"
|
||||
DEFAULT_EDGE_MARGIN_PIXELS = 4.0
|
||||
MAX_MANIFEST_BYTES = 4 * 1024 * 1024
|
||||
|
||||
_SHA256 = re.compile(r"^[a-f0-9]{64}$")
|
||||
_SAFE_GENERATION = re.compile(r"^valid-fov-mask-[a-f0-9]{64}$")
|
||||
|
||||
|
||||
class K1ValidFovMaskError(RuntimeError):
|
||||
"""A K1 valid-FOV mask could not be derived or validated safely."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1ValidFovMask:
|
||||
generation_id: str
|
||||
root: Path
|
||||
manifest_path: Path
|
||||
mask_path: Path
|
||||
source_id: str
|
||||
calibration_slot: str
|
||||
calibration_sha256: str
|
||||
width: int
|
||||
height: int
|
||||
center_xy: tuple[float, float]
|
||||
radius_pixels: float
|
||||
crop_xyxy: tuple[int, int, int, int]
|
||||
valid_pixel_count: int
|
||||
valid_fraction: float
|
||||
|
||||
|
||||
def prepare_k1_valid_fov_mask(
|
||||
*,
|
||||
calibration_snapshot_root: Path,
|
||||
source_id: str,
|
||||
output_root: Path,
|
||||
edge_margin_pixels: float = DEFAULT_EDGE_MARGIN_PIXELS,
|
||||
) -> K1ValidFovMask:
|
||||
"""Seal one reusable circular valid-FOV mask bound to exact K1 calibration.
|
||||
|
||||
The mask is generated once from the calibrated principal point and the
|
||||
admitted image transform. It is content-addressed by calibration, source,
|
||||
geometry and construction policy, so repeated experiments load the same
|
||||
PNG rather than estimating a lens boundary from every frame.
|
||||
"""
|
||||
|
||||
if source_id not in MAIN_CAMERA_SLOT_BY_SOURCE:
|
||||
raise K1ValidFovMaskError("source is not an admitted K1 main camera")
|
||||
if (
|
||||
isinstance(edge_margin_pixels, bool)
|
||||
or not isinstance(edge_margin_pixels, (int, float))
|
||||
or not math.isfinite(float(edge_margin_pixels))
|
||||
or not 0.0 <= float(edge_margin_pixels) <= 64.0
|
||||
):
|
||||
raise K1ValidFovMaskError("edge margin must be finite and between 0 and 64 pixels")
|
||||
|
||||
snapshot_root = calibration_snapshot_root.expanduser().resolve(strict=True)
|
||||
snapshot = _validated_snapshot(snapshot_root)
|
||||
calibration = parse_k1_factory_calibration(
|
||||
(snapshot_root / "camera.yaml").read_bytes(),
|
||||
(snapshot_root / "extrinsic_camera_lidar.yaml").read_bytes(),
|
||||
)
|
||||
normalized = calibration.normalized_profile()
|
||||
if snapshot.get("normalized_calibration") != normalized:
|
||||
raise K1ValidFovMaskError("calibration snapshot normalized profile changed")
|
||||
profile = Kb4ProjectionProfile.from_factory_calibration(calibration, source_id)
|
||||
stream_bindings = normalized.get("stream_bindings")
|
||||
if not isinstance(stream_bindings, dict):
|
||||
raise K1ValidFovMaskError("calibration stream bindings are unavailable")
|
||||
stream_binding = stream_bindings.get(source_id)
|
||||
if not isinstance(stream_binding, dict):
|
||||
raise K1ValidFovMaskError("calibration source binding is unavailable")
|
||||
|
||||
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
|
||||
image_edge_radius = min(
|
||||
cx,
|
||||
float(profile.width - 1) - cx,
|
||||
cy,
|
||||
float(profile.height - 1) - cy,
|
||||
)
|
||||
radius = image_edge_radius - float(edge_margin_pixels)
|
||||
if not math.isfinite(radius) or radius < 32.0:
|
||||
raise K1ValidFovMaskError("valid-FOV construction leaves no usable image circle")
|
||||
|
||||
identity = {
|
||||
"schema_version": VALID_FOV_MASK_IDENTITY_SCHEMA,
|
||||
"calibration_sha256": snapshot["content_identity_sha256"],
|
||||
"source_id": source_id,
|
||||
"calibration_slot": profile.calibration_slot,
|
||||
"camera_model": "kb4",
|
||||
"admitted_resolution": [profile.width, profile.height],
|
||||
"admitted_intrinsic_fx_fy_cx_cy": [fx, fy, cx, cy],
|
||||
"distortion_kb4": list(profile.distortion_kb4),
|
||||
"image_transform": stream_binding.get("image_transform"),
|
||||
"construction": {
|
||||
"kind": "calibrated-principal-point-inscribed-circle",
|
||||
"pixel_coordinate_convention": "integer-pixel-centers",
|
||||
"edge_radius_policy": "minimum-distance-to-admitted-image-edge",
|
||||
"edge_margin_pixels": float(edge_margin_pixels),
|
||||
"outside_value": 0,
|
||||
"inside_value": 255,
|
||||
},
|
||||
}
|
||||
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
|
||||
generation_id = f"valid-fov-mask-{identity_sha256}"
|
||||
root = _prepare_private_directory(output_root)
|
||||
final = root / generation_id
|
||||
if final.exists():
|
||||
existing = validate_k1_valid_fov_mask(final)
|
||||
if existing.calibration_sha256 != snapshot["content_identity_sha256"]:
|
||||
raise K1ValidFovMaskError("valid-FOV generation collides with another calibration")
|
||||
return existing
|
||||
|
||||
staging = root / f".{generation_id}.{secrets.token_hex(12)}.incomplete"
|
||||
published = False
|
||||
try:
|
||||
staging.mkdir(mode=0o700)
|
||||
mask = _circle_mask(profile.width, profile.height, cx, cy, radius)
|
||||
valid_y, valid_x = np.nonzero(mask)
|
||||
if valid_x.size == 0 or valid_y.size == 0:
|
||||
raise K1ValidFovMaskError("valid-FOV mask is empty")
|
||||
crop = (
|
||||
int(valid_x.min()),
|
||||
int(valid_y.min()),
|
||||
int(valid_x.max()) + 1,
|
||||
int(valid_y.max()) + 1,
|
||||
)
|
||||
mask_path = staging / "mask.png"
|
||||
Image.fromarray(mask, mode="L").save(mask_path, format="PNG", optimize=False)
|
||||
_fsync_file(mask_path)
|
||||
os.chmod(mask_path, 0o600)
|
||||
mask_sha256 = _sha256_file(mask_path)
|
||||
valid_pixel_count = int(np.count_nonzero(mask))
|
||||
valid_fraction = valid_pixel_count / int(mask.size)
|
||||
manifest = {
|
||||
"schema_version": VALID_FOV_MASK_SCHEMA,
|
||||
"generation_id": generation_id,
|
||||
"identity_sha256": identity_sha256,
|
||||
"identity": identity,
|
||||
"created_at_utc": utc_now_iso(),
|
||||
"geometry": {
|
||||
"center_xy": [cx, cy],
|
||||
"image_edge_radius_pixels": image_edge_radius,
|
||||
"radius_pixels": radius,
|
||||
"crop_xyxy_exclusive": list(crop),
|
||||
"valid_pixel_count": valid_pixel_count,
|
||||
"total_pixel_count": int(mask.size),
|
||||
"valid_fraction": valid_fraction,
|
||||
},
|
||||
"artifact": {
|
||||
"path": "mask.png",
|
||||
"media_type": "image/png",
|
||||
"mode": "L",
|
||||
"inside_value": 255,
|
||||
"outside_value": 0,
|
||||
"byte_length": mask_path.stat().st_size,
|
||||
"sha256": mask_sha256,
|
||||
},
|
||||
"usage": {
|
||||
"quality": "set invalid lens exterior to the model profile's fixed fill value",
|
||||
"speed": (
|
||||
"apply crop_xyxy_exclusive before model preprocessing and map outputs back"
|
||||
),
|
||||
"dense_compute_warning": (
|
||||
"multiplying an unchanged 800x600 tensor by this mask alone does not reduce "
|
||||
"dense neural-network FLOPs"
|
||||
),
|
||||
},
|
||||
}
|
||||
write_json_atomic(staging / "manifest.json", manifest)
|
||||
os.chmod(staging / "manifest.json", 0o600)
|
||||
_fsync_directory(staging)
|
||||
os.replace(staging, final)
|
||||
_fsync_directory(root)
|
||||
published = True
|
||||
finally:
|
||||
if not published and staging.exists():
|
||||
shutil.rmtree(staging)
|
||||
|
||||
return validate_k1_valid_fov_mask(final)
|
||||
|
||||
|
||||
def validate_k1_valid_fov_mask(mask_root: Path) -> K1ValidFovMask:
|
||||
root = mask_root.expanduser().resolve(strict=True)
|
||||
if not root.is_dir() or _SAFE_GENERATION.fullmatch(root.name) is None:
|
||||
raise K1ValidFovMaskError("valid-FOV root is invalid")
|
||||
manifest = _read_json_object(root / "manifest.json", root, MAX_MANIFEST_BYTES)
|
||||
identity = manifest.get("identity")
|
||||
identity_sha256 = manifest.get("identity_sha256")
|
||||
if (
|
||||
manifest.get("schema_version") != VALID_FOV_MASK_SCHEMA
|
||||
or manifest.get("generation_id") != root.name
|
||||
or not isinstance(identity, dict)
|
||||
or identity.get("schema_version") != VALID_FOV_MASK_IDENTITY_SCHEMA
|
||||
or not isinstance(identity_sha256, str)
|
||||
or _SHA256.fullmatch(identity_sha256) is None
|
||||
or root.name != f"valid-fov-mask-{identity_sha256}"
|
||||
or hashlib.sha256(_canonical_json(identity)).hexdigest() != identity_sha256
|
||||
):
|
||||
raise K1ValidFovMaskError("valid-FOV identity is inconsistent")
|
||||
|
||||
artifact = manifest.get("artifact")
|
||||
geometry = manifest.get("geometry")
|
||||
resolution = identity.get("admitted_resolution")
|
||||
intrinsic = identity.get("admitted_intrinsic_fx_fy_cx_cy")
|
||||
if (
|
||||
not isinstance(artifact, dict)
|
||||
or artifact.get("path") != "mask.png"
|
||||
or artifact.get("media_type") != "image/png"
|
||||
or artifact.get("mode") != "L"
|
||||
or artifact.get("inside_value") != 255
|
||||
or artifact.get("outside_value") != 0
|
||||
or not isinstance(geometry, dict)
|
||||
or not isinstance(resolution, list)
|
||||
or len(resolution) != 2
|
||||
or not all(isinstance(value, int) and not isinstance(value, bool) for value in resolution)
|
||||
or not isinstance(intrinsic, list)
|
||||
or len(intrinsic) != 4
|
||||
):
|
||||
raise K1ValidFovMaskError("valid-FOV manifest contract is invalid")
|
||||
width, height = int(resolution[0]), int(resolution[1])
|
||||
if width < 1 or height < 1:
|
||||
raise K1ValidFovMaskError("valid-FOV resolution is invalid")
|
||||
mask_path = root / "mask.png"
|
||||
metadata = _confined_regular_file(mask_path, root)
|
||||
if (
|
||||
artifact.get("byte_length") != metadata.st_size
|
||||
or not isinstance(artifact.get("sha256"), str)
|
||||
or _SHA256.fullmatch(str(artifact["sha256"])) is None
|
||||
or _sha256_file(mask_path) != artifact["sha256"]
|
||||
):
|
||||
raise K1ValidFovMaskError("valid-FOV mask artifact changed")
|
||||
try:
|
||||
with Image.open(mask_path) as opened:
|
||||
if opened.mode != "L" or opened.size != (width, height):
|
||||
raise K1ValidFovMaskError("valid-FOV PNG dimensions or mode changed")
|
||||
mask = np.asarray(opened, dtype=np.uint8)
|
||||
except (OSError, ValueError) as exc:
|
||||
raise K1ValidFovMaskError("valid-FOV PNG is unavailable") from exc
|
||||
if not np.isin(mask, (0, 255)).all():
|
||||
raise K1ValidFovMaskError("valid-FOV PNG is not binary")
|
||||
valid_y, valid_x = np.nonzero(mask)
|
||||
if valid_x.size == 0 or valid_y.size == 0:
|
||||
raise K1ValidFovMaskError("valid-FOV PNG is empty")
|
||||
crop = (
|
||||
int(valid_x.min()),
|
||||
int(valid_y.min()),
|
||||
int(valid_x.max()) + 1,
|
||||
int(valid_y.max()) + 1,
|
||||
)
|
||||
count = int(valid_x.size)
|
||||
center = geometry.get("center_xy")
|
||||
radius = geometry.get("radius_pixels")
|
||||
expected_crop = geometry.get("crop_xyxy_exclusive")
|
||||
expected_count = geometry.get("valid_pixel_count")
|
||||
expected_total = geometry.get("total_pixel_count")
|
||||
expected_fraction = geometry.get("valid_fraction")
|
||||
if (
|
||||
not isinstance(center, list)
|
||||
or len(center) != 2
|
||||
or not all(isinstance(value, (int, float)) for value in center)
|
||||
or not isinstance(radius, (int, float))
|
||||
or not math.isfinite(float(radius))
|
||||
or list(crop) != expected_crop
|
||||
or count != expected_count
|
||||
or mask.size != expected_total
|
||||
or not isinstance(expected_fraction, (int, float))
|
||||
or not math.isclose(count / mask.size, float(expected_fraction), abs_tol=1e-12)
|
||||
):
|
||||
raise K1ValidFovMaskError("valid-FOV geometry changed")
|
||||
source_id = identity.get("source_id")
|
||||
calibration_slot = identity.get("calibration_slot")
|
||||
calibration_sha256 = identity.get("calibration_sha256")
|
||||
if (
|
||||
not isinstance(source_id, str)
|
||||
or source_id not in MAIN_CAMERA_SLOT_BY_SOURCE
|
||||
or not isinstance(calibration_slot, str)
|
||||
or calibration_slot != MAIN_CAMERA_SLOT_BY_SOURCE[source_id]
|
||||
or not isinstance(calibration_sha256, str)
|
||||
or _SHA256.fullmatch(calibration_sha256) is None
|
||||
):
|
||||
raise K1ValidFovMaskError("valid-FOV calibration binding is invalid")
|
||||
return K1ValidFovMask(
|
||||
generation_id=root.name,
|
||||
root=root,
|
||||
manifest_path=root / "manifest.json",
|
||||
mask_path=mask_path,
|
||||
source_id=source_id,
|
||||
calibration_slot=calibration_slot,
|
||||
calibration_sha256=calibration_sha256,
|
||||
width=width,
|
||||
height=height,
|
||||
center_xy=(float(center[0]), float(center[1])),
|
||||
radius_pixels=float(radius),
|
||||
crop_xyxy=crop,
|
||||
valid_pixel_count=count,
|
||||
valid_fraction=count / mask.size,
|
||||
)
|
||||
|
||||
|
||||
def _validated_snapshot(root: Path) -> dict[str, Any]:
|
||||
manifest = _read_json_object(root / "manifest.json", root, MAX_MANIFEST_BYTES)
|
||||
artifacts = manifest.get("artifacts")
|
||||
device = manifest.get("device")
|
||||
identity_sha256 = manifest.get("content_identity_sha256")
|
||||
if (
|
||||
manifest.get("schema_version") != CALIBRATION_SNAPSHOT_MANIFEST_VERSION
|
||||
or not isinstance(artifacts, list)
|
||||
or len(artifacts) != 2
|
||||
or not isinstance(device, dict)
|
||||
or not isinstance(identity_sha256, str)
|
||||
or _SHA256.fullmatch(identity_sha256) is None
|
||||
):
|
||||
raise K1ValidFovMaskError("calibration snapshot manifest is incompatible")
|
||||
expected_names = {"camera.yaml", "extrinsic_camera_lidar.yaml"}
|
||||
by_name: dict[str, dict[str, Any]] = {}
|
||||
by_source: dict[str, str] = {}
|
||||
for artifact in artifacts:
|
||||
if not isinstance(artifact, dict):
|
||||
raise K1ValidFovMaskError("calibration snapshot artifact is invalid")
|
||||
name = artifact.get("artifact_name")
|
||||
source_path = artifact.get("source_path")
|
||||
digest = artifact.get("sha256")
|
||||
byte_length = artifact.get("bytes")
|
||||
if (
|
||||
not isinstance(name, str)
|
||||
or name not in expected_names
|
||||
or name in by_name
|
||||
or not isinstance(source_path, str)
|
||||
or not source_path.startswith("/mnt/system/factory-data/config/")
|
||||
or not isinstance(digest, str)
|
||||
or _SHA256.fullmatch(digest) is None
|
||||
or not isinstance(byte_length, int)
|
||||
or isinstance(byte_length, bool)
|
||||
or byte_length < 1
|
||||
):
|
||||
raise K1ValidFovMaskError("calibration snapshot artifact descriptor is invalid")
|
||||
path = root / name
|
||||
metadata = _confined_regular_file(path, root)
|
||||
if metadata.st_size != byte_length or _sha256_file(path) != digest:
|
||||
raise K1ValidFovMaskError("calibration snapshot artifact changed")
|
||||
by_name[name] = artifact
|
||||
by_source[source_path] = digest
|
||||
if set(by_name) != expected_names or len(by_source) != 2:
|
||||
raise K1ValidFovMaskError("calibration snapshot artifact set changed")
|
||||
vendor_device_id = device.get("vendor_device_id")
|
||||
device_serial = device.get("device_serial")
|
||||
if not isinstance(vendor_device_id, str) or not isinstance(device_serial, str):
|
||||
raise K1ValidFovMaskError("calibration snapshot device binding is unavailable")
|
||||
digest = hashlib.sha256()
|
||||
try:
|
||||
digest.update(vendor_device_id.encode("ascii"))
|
||||
digest.update(b"\x00")
|
||||
digest.update(device_serial.encode("ascii"))
|
||||
except UnicodeEncodeError as exc:
|
||||
raise K1ValidFovMaskError("calibration snapshot device binding is invalid") from exc
|
||||
for source_path in sorted(by_source):
|
||||
digest.update(b"\x00")
|
||||
digest.update(source_path.encode("utf-8"))
|
||||
digest.update(bytes.fromhex(by_source[source_path]))
|
||||
if digest.hexdigest() != identity_sha256:
|
||||
raise K1ValidFovMaskError("calibration snapshot content identity changed")
|
||||
return manifest
|
||||
|
||||
|
||||
def _circle_mask(width: int, height: int, cx: float, cy: float, radius: float) -> np.ndarray:
|
||||
y, x = np.ogrid[:height, :width]
|
||||
inside = (x - cx) ** 2 + (y - cy) ** 2 <= radius**2
|
||||
return np.where(inside, 255, 0).astype(np.uint8)
|
||||
|
||||
|
||||
def _prepare_private_directory(path: Path) -> Path:
|
||||
candidate = path.expanduser()
|
||||
candidate.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
metadata = candidate.lstat()
|
||||
if stat.S_ISLNK(metadata.st_mode) or not stat.S_ISDIR(metadata.st_mode):
|
||||
raise K1ValidFovMaskError("valid-FOV output root must be a real directory")
|
||||
root = candidate.resolve(strict=True)
|
||||
os.chmod(root, 0o700)
|
||||
return root
|
||||
|
||||
|
||||
def _read_json_object(path: Path, root: Path, maximum_bytes: int) -> dict[str, Any]:
|
||||
metadata = _confined_regular_file(path, root)
|
||||
if not 0 < metadata.st_size <= maximum_bytes:
|
||||
raise K1ValidFovMaskError("JSON manifest is outside bounds")
|
||||
try:
|
||||
value = json.loads(path.read_text(encoding="utf-8"))
|
||||
except (OSError, UnicodeDecodeError, json.JSONDecodeError) as exc:
|
||||
raise K1ValidFovMaskError("JSON manifest is unavailable") from exc
|
||||
if not isinstance(value, dict):
|
||||
raise K1ValidFovMaskError("JSON manifest is not an object")
|
||||
return value
|
||||
|
||||
|
||||
def _confined_regular_file(path: Path, root: Path) -> os.stat_result:
|
||||
try:
|
||||
resolved_root = root.resolve(strict=True)
|
||||
resolved = path.resolve(strict=True)
|
||||
metadata = path.lstat()
|
||||
except OSError as exc:
|
||||
raise K1ValidFovMaskError("artifact is unavailable") from exc
|
||||
if (
|
||||
stat.S_ISLNK(metadata.st_mode)
|
||||
or not stat.S_ISREG(metadata.st_mode)
|
||||
or not resolved.is_relative_to(resolved_root)
|
||||
):
|
||||
raise K1ValidFovMaskError("artifact is not a confined regular file")
|
||||
return metadata
|
||||
|
||||
|
||||
def _sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
while chunk := stream.read(1024 * 1024):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _canonical_json(value: object) -> bytes:
|
||||
return json.dumps(
|
||||
value,
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
allow_nan=False,
|
||||
).encode("utf-8")
|
||||
|
||||
|
||||
def _fsync_file(path: Path) -> None:
|
||||
with path.open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def _fsync_directory(path: Path) -> None:
|
||||
flags = os.O_RDONLY
|
||||
if hasattr(os, "O_DIRECTORY"):
|
||||
flags |= os.O_DIRECTORY
|
||||
descriptor = os.open(path, flags)
|
||||
try:
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
@@ -42,10 +42,7 @@ SHA256_PATTERN = re.compile(r"^[a-f0-9]{64}$")
|
||||
MEDIA_SOURCE_PATTERN = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._-]{0,127}$")
|
||||
MEDIA_EPOCH_PATTERN = re.compile(r"^epoch-(?!0+$)[0-9]+$")
|
||||
MEDIA_SEGMENT_PATTERN = re.compile(r"^[0-9]+\.m4s$")
|
||||
MAX_MEDIA_INDEX_BYTES = 32 * 1024 * 1024
|
||||
MAX_RECOVERY_METADATA_BYTES = 64 * 1024 * 1024
|
||||
MAX_RECOVERY_METADATA_LINE_BYTES = 64 * 1024
|
||||
MAX_RECOVERY_MESSAGES = 500_000
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
@@ -560,20 +557,12 @@ def _scan_capture_prefix(
|
||||
first_timestamp: str | None = None
|
||||
last_timestamp: str | None = None
|
||||
metadata_committed_bytes = 0
|
||||
while (
|
||||
consumed_metadata_bytes < MAX_RECOVERY_METADATA_BYTES
|
||||
and message_count < MAX_RECOVERY_MESSAGES
|
||||
):
|
||||
remaining = MAX_RECOVERY_METADATA_BYTES - consumed_metadata_bytes
|
||||
read_limit = min(MAX_RECOVERY_METADATA_LINE_BYTES + 1, remaining + 1)
|
||||
line = metadata_stream.readline(read_limit)
|
||||
while True:
|
||||
line = metadata_stream.readline(MAX_RECOVERY_METADATA_LINE_BYTES + 1)
|
||||
if not line:
|
||||
break
|
||||
consumed_metadata_bytes += len(line)
|
||||
if (
|
||||
len(line) > MAX_RECOVERY_METADATA_LINE_BYTES
|
||||
or consumed_metadata_bytes > MAX_RECOVERY_METADATA_BYTES
|
||||
):
|
||||
if len(line) > MAX_RECOVERY_METADATA_LINE_BYTES:
|
||||
return None
|
||||
if not line.endswith(b"\n"):
|
||||
if tolerate_incomplete_metadata_tail:
|
||||
@@ -1015,7 +1004,7 @@ def _validated_media_epoch(epoch: Path, expected_source_id: str) -> bool:
|
||||
or init_path.stat().st_size <= 0
|
||||
or not segments_root.is_dir()
|
||||
or not index_path.is_file()
|
||||
or not 0 < index_path.stat().st_size <= MAX_MEDIA_INDEX_BYTES
|
||||
or index_path.stat().st_size <= 0
|
||||
or not summary_path.is_file()
|
||||
):
|
||||
return False
|
||||
@@ -1027,30 +1016,37 @@ def _validated_media_epoch(epoch: Path, expected_source_id: str) -> bool:
|
||||
segment_count = _non_negative_int(summary.get("segment_count"))
|
||||
if segment_count < 1:
|
||||
return False
|
||||
segments = [
|
||||
path.resolve()
|
||||
for path in sorted(segments_root.iterdir())
|
||||
if path.is_file() and MEDIA_SEGMENT_PATTERN.fullmatch(path.name)
|
||||
]
|
||||
if len(segments) != segment_count or any(
|
||||
not path.is_relative_to(segments_root) or path.stat().st_size <= 0 for path in segments
|
||||
segment_sequences: set[int] = set()
|
||||
for path in segments_root.iterdir():
|
||||
match = MEDIA_SEGMENT_PATTERN.fullmatch(path.name)
|
||||
if match is None or not path.is_file():
|
||||
continue
|
||||
resolved = path.resolve()
|
||||
sequence = int(path.stem)
|
||||
if (
|
||||
resolved.parent != segments_root
|
||||
or path.name != f"{sequence}.m4s"
|
||||
or resolved.stat().st_size <= 0
|
||||
or sequence in segment_sequences
|
||||
):
|
||||
return False
|
||||
segment_sequences.add(sequence)
|
||||
if (
|
||||
len(segment_sequences) != segment_count
|
||||
or min(segment_sequences, default=0) != 1
|
||||
or max(segment_sequences, default=0) != segment_count
|
||||
):
|
||||
return False
|
||||
try:
|
||||
index_records = [
|
||||
json.loads(line)
|
||||
for line in index_path.read_text(encoding="utf-8").splitlines()
|
||||
if line.strip()
|
||||
]
|
||||
with index_path.open("r", encoding="utf-8") as stream:
|
||||
index_count = 0
|
||||
for index_count, line in enumerate(stream, start=1):
|
||||
record = json.loads(line)
|
||||
if not isinstance(record, dict) or record.get("sequence") != index_count:
|
||||
return False
|
||||
except (OSError, UnicodeDecodeError, json.JSONDecodeError):
|
||||
return False
|
||||
if len(index_records) != segment_count or not all(
|
||||
isinstance(record, dict) and _non_negative_int(record.get("sequence")) > 0
|
||||
for record in index_records
|
||||
):
|
||||
return False
|
||||
sequences = [int(record["sequence"]) for record in index_records]
|
||||
return len(sequences) == len(set(sequences))
|
||||
return index_count == segment_count
|
||||
|
||||
|
||||
def _media_epoch_bytes(epoch: Path) -> int:
|
||||
|
||||
@@ -0,0 +1,293 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
from collections.abc import AsyncIterator
|
||||
from contextlib import asynccontextmanager
|
||||
from importlib.metadata import version
|
||||
from time import monotonic
|
||||
from typing import Literal, TypedDict
|
||||
|
||||
from bleak import BleakClient, BleakScanner
|
||||
from bleak.exc import BleakDeviceNotFoundError, BleakError
|
||||
|
||||
from k1link.artifacts import utc_now_iso
|
||||
from k1link.device_plugins.xgrids_k1.ble.scanner import discovered_device
|
||||
from k1link.device_plugins.xgrids_k1.ble.wifi_provisioning import (
|
||||
AP_FALLBACK_IPV4,
|
||||
SERVICE_UUID,
|
||||
STATUS_CHARACTERISTIC_UUID,
|
||||
WRITE_CHARACTERISTIC_UUID,
|
||||
ResolvedWriteMode,
|
||||
StatusObservation,
|
||||
WifiStatus,
|
||||
WriteMode,
|
||||
parse_wifi_status,
|
||||
)
|
||||
|
||||
PROFILE_ID = "xgrids-k1-fw3-quick-connect-ap-v1"
|
||||
FRAME_LENGTH = 100
|
||||
COMMAND_OFFSET = 99
|
||||
ENABLE_AP_COMMAND = 1
|
||||
|
||||
ApActivationOutcome = Literal[
|
||||
"already_active",
|
||||
"ap_ready_observed",
|
||||
"status_changed",
|
||||
"ble_disconnected_after_write",
|
||||
"no_status_change_before_timeout",
|
||||
]
|
||||
|
||||
|
||||
class ApActivationResult(TypedDict):
|
||||
schema_version: int
|
||||
profile_id: str
|
||||
started_at_utc: str
|
||||
completed_at_utc: str
|
||||
adapter: str
|
||||
bleak_version: str
|
||||
device_macos_uuid: str
|
||||
device_name: str
|
||||
service_uuid: str
|
||||
write_characteristic_uuid: str
|
||||
status_characteristic_uuid: str
|
||||
operation: str
|
||||
write_performed: bool
|
||||
write_mode: ResolvedWriteMode | None
|
||||
write_without_response_advertised: bool
|
||||
max_write_without_response_size: int
|
||||
frame_length: int
|
||||
baseline_status: WifiStatus
|
||||
observations: list[StatusObservation]
|
||||
ready_observed: bool
|
||||
outcome: ApActivationOutcome
|
||||
|
||||
|
||||
def build_ap_activation_frame() -> bytearray:
|
||||
"""Build LixelGO's fixed 100-byte Quick Connect AP-enable frame."""
|
||||
|
||||
frame = bytearray(FRAME_LENGTH)
|
||||
frame[COMMAND_OFFSET] = ENABLE_AP_COMMAND
|
||||
return frame
|
||||
|
||||
|
||||
def is_ap_ready_status(status: WifiStatus) -> bool:
|
||||
# The reviewed LixelGO build maps byte 51 of the 7f02 response to its
|
||||
# Wi-Fi-AP-ready flag. WIFI_AP plus the fallback address describes the
|
||||
# selected control mode, but can remain stale after the beacon disappears.
|
||||
return (
|
||||
status["mode"] == "WIFI_AP"
|
||||
and status["ipv4"] == AP_FALLBACK_IPV4
|
||||
and status["reserved"] not in (None, 0)
|
||||
)
|
||||
|
||||
|
||||
def _outcome(
|
||||
baseline: WifiStatus,
|
||||
observations: list[StatusObservation],
|
||||
disconnected: bool,
|
||||
) -> ApActivationOutcome:
|
||||
if observations:
|
||||
final = observations[-1]["status"]
|
||||
if is_ap_ready_status(final):
|
||||
return "ap_ready_observed"
|
||||
if final != baseline:
|
||||
return "status_changed"
|
||||
if disconnected:
|
||||
return "ble_disconnected_after_write"
|
||||
return "no_status_change_before_timeout"
|
||||
|
||||
|
||||
@asynccontextmanager
|
||||
async def device_ap_activation_session(
|
||||
device_macos_uuid: str,
|
||||
timeout_seconds: float = 15.0,
|
||||
poll_interval_seconds: float = 0.5,
|
||||
write_mode: WriteMode = "auto",
|
||||
) -> AsyncIterator[ApActivationResult]:
|
||||
"""Keep BLE connected around one reviewed Quick Connect AP-enable write.
|
||||
|
||||
The payload is the exact fixed frame used by the reviewed LixelGO build.
|
||||
``auto`` follows the live GATT properties because the owner-controlled K1
|
||||
advertises a write with response even though the Android client requests a
|
||||
write without response. The caller receives the result while the same BLE
|
||||
session is still alive, matching LixelGO's AP-ready -> native Wi-Fi handoff.
|
||||
No transport or command retry is attempted.
|
||||
"""
|
||||
|
||||
if timeout_seconds <= 0:
|
||||
raise ValueError("timeout_seconds must be positive")
|
||||
if poll_interval_seconds <= 0:
|
||||
raise ValueError("poll_interval_seconds must be positive")
|
||||
if write_mode not in ("auto", "with_response", "without_response"):
|
||||
raise ValueError(f"Unsupported write mode: {write_mode}")
|
||||
|
||||
frame = build_ap_activation_frame()
|
||||
started_at = utc_now_iso()
|
||||
observations: list[StatusObservation] = []
|
||||
disconnected = False
|
||||
|
||||
try:
|
||||
async with asyncio.timeout(timeout_seconds + 25.0):
|
||||
device = discovered_device(device_macos_uuid)
|
||||
if device is None:
|
||||
device = await BleakScanner.find_device_by_address(
|
||||
device_macos_uuid,
|
||||
timeout=min(20.0, timeout_seconds),
|
||||
)
|
||||
if device is None:
|
||||
raise BleakDeviceNotFoundError(
|
||||
device_macos_uuid,
|
||||
"Device was not rediscovered; keep the K1 powered and nearby.",
|
||||
)
|
||||
|
||||
async with BleakClient(device, timeout=timeout_seconds, pair=False) as client:
|
||||
async with asyncio.timeout(timeout_seconds + 10.0):
|
||||
device_name = client.name
|
||||
service = client.services.get_service(SERVICE_UUID)
|
||||
write_characteristic = client.services.get_characteristic(
|
||||
WRITE_CHARACTERISTIC_UUID
|
||||
)
|
||||
status_characteristic = client.services.get_characteristic(
|
||||
STATUS_CHARACTERISTIC_UUID
|
||||
)
|
||||
if service is None:
|
||||
raise ValueError(f"Reviewed K1 service not found: {SERVICE_UUID}")
|
||||
if write_characteristic is None:
|
||||
raise ValueError(
|
||||
"Reviewed K1 AP-control characteristic not found: "
|
||||
f"{WRITE_CHARACTERISTIC_UUID}"
|
||||
)
|
||||
if status_characteristic is None:
|
||||
raise ValueError(
|
||||
f"Reviewed K1 status characteristic not found: {STATUS_CHARACTERISTIC_UUID}"
|
||||
)
|
||||
if write_characteristic.service_uuid != service.uuid:
|
||||
raise ValueError(
|
||||
"K1 AP-control characteristic is attached to an unexpected service"
|
||||
)
|
||||
if status_characteristic.service_uuid != service.uuid:
|
||||
raise ValueError(
|
||||
"K1 status characteristic is attached to an unexpected service"
|
||||
)
|
||||
if "read" not in status_characteristic.properties:
|
||||
raise ValueError("Reviewed K1 status characteristic is not readable")
|
||||
|
||||
properties = set(write_characteristic.properties)
|
||||
max_without_response = write_characteristic.max_write_without_response_size
|
||||
baseline = parse_wifi_status(
|
||||
bytes(await client.read_gatt_char(status_characteristic))
|
||||
)
|
||||
# WIFI_AP is a control-mode status, not proof that the radio is
|
||||
# still beaconing. A physical run found the exact SSID shortly
|
||||
# after AP-enable, then found no beacon while 7f02 continued to
|
||||
# report WIFI_AP. LixelGO emits the reviewed enable frame for
|
||||
# each explicit Quick Connect action, so Mission Core does the
|
||||
# same once per operator action instead of short-circuiting on
|
||||
# a stale-ready status. There is still no automatic retry.
|
||||
|
||||
resolved_write_mode: ResolvedWriteMode
|
||||
if write_mode == "auto":
|
||||
if "write-without-response" in properties:
|
||||
resolved_write_mode = "without_response"
|
||||
elif "write" in properties:
|
||||
resolved_write_mode = "with_response"
|
||||
else:
|
||||
raise ValueError("Reviewed K1 characteristic is not writable")
|
||||
elif write_mode == "with_response":
|
||||
if "write" not in properties:
|
||||
raise ValueError(
|
||||
"Reviewed K1 characteristic does not advertise writes with response"
|
||||
)
|
||||
resolved_write_mode = "with_response"
|
||||
else:
|
||||
if len(frame) > max_without_response:
|
||||
raise ValueError(
|
||||
"AP activation frame exceeds the negotiated "
|
||||
"write-without-response size"
|
||||
)
|
||||
resolved_write_mode = "without_response"
|
||||
|
||||
await client.write_gatt_char(
|
||||
write_characteristic,
|
||||
frame,
|
||||
response=resolved_write_mode == "with_response",
|
||||
)
|
||||
write_completed = monotonic()
|
||||
deadline = write_completed + timeout_seconds
|
||||
|
||||
while monotonic() < deadline:
|
||||
try:
|
||||
status = parse_wifi_status(
|
||||
bytes(await client.read_gatt_char(status_characteristic))
|
||||
)
|
||||
except BleakError:
|
||||
if not client.is_connected:
|
||||
disconnected = True
|
||||
break
|
||||
raise
|
||||
observation: StatusObservation = {
|
||||
"observed_at_utc": utc_now_iso(),
|
||||
"seconds_after_write": round(monotonic() - write_completed, 3),
|
||||
"status": status,
|
||||
}
|
||||
if not observations or status != observations[-1]["status"]:
|
||||
observations.append(observation)
|
||||
if is_ap_ready_status(status):
|
||||
break
|
||||
await asyncio.sleep(poll_interval_seconds)
|
||||
|
||||
result: ApActivationResult = {
|
||||
"schema_version": 1,
|
||||
"profile_id": PROFILE_ID,
|
||||
"started_at_utc": started_at,
|
||||
"completed_at_utc": utc_now_iso(),
|
||||
"adapter": "CoreBluetooth",
|
||||
"bleak_version": version("bleak"),
|
||||
"device_macos_uuid": device_macos_uuid,
|
||||
"device_name": device_name,
|
||||
"service_uuid": service.uuid,
|
||||
"write_characteristic_uuid": write_characteristic.uuid,
|
||||
"status_characteristic_uuid": status_characteristic.uuid,
|
||||
"operation": "single_reviewed_quick_connect_ap_activation",
|
||||
"write_performed": True,
|
||||
"write_mode": resolved_write_mode,
|
||||
"write_without_response_advertised": (
|
||||
"write-without-response" in properties
|
||||
),
|
||||
"max_write_without_response_size": max_without_response,
|
||||
"frame_length": len(frame),
|
||||
"baseline_status": baseline,
|
||||
"observations": observations,
|
||||
"ready_observed": bool(
|
||||
observations and is_ap_ready_status(observations[-1]["status"])
|
||||
),
|
||||
"outcome": _outcome(baseline, observations, disconnected),
|
||||
}
|
||||
# Keep the same CoreBluetooth session alive while the caller waits
|
||||
# for and performs the host-side CoreWLAN association. LixelGO does
|
||||
# not tear down this BLE manager between its AP-ready callback and
|
||||
# native Wi-Fi connect call.
|
||||
yield result
|
||||
finally:
|
||||
frame[:] = b"\x00" * len(frame)
|
||||
|
||||
|
||||
async def activate_device_ap_once(
|
||||
device_macos_uuid: str,
|
||||
timeout_seconds: float = 15.0,
|
||||
poll_interval_seconds: float = 0.5,
|
||||
write_mode: WriteMode = "auto",
|
||||
) -> ApActivationResult:
|
||||
"""Run one AP activation and release BLE immediately after its result.
|
||||
|
||||
Host association flows must use :func:`device_ap_activation_session` so
|
||||
the reviewed LixelGO BLE-to-Wi-Fi handoff remains one connected session.
|
||||
"""
|
||||
|
||||
async with device_ap_activation_session(
|
||||
device_macos_uuid,
|
||||
timeout_seconds=timeout_seconds,
|
||||
poll_interval_seconds=poll_interval_seconds,
|
||||
write_mode=write_mode,
|
||||
) as result:
|
||||
return result
|
||||
@@ -1,6 +1,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from importlib.metadata import version
|
||||
from threading import Lock
|
||||
from typing import TypedDict
|
||||
|
||||
from bleak import BleakScanner
|
||||
@@ -9,6 +10,9 @@ from bleak.backends.scanner import AdvertisementData
|
||||
|
||||
from k1link.artifacts import utc_now_iso
|
||||
|
||||
_runtime_handle_lock = Lock()
|
||||
_runtime_handles: dict[str, BLEDevice] = {}
|
||||
|
||||
|
||||
class BleDeviceRecord(TypedDict):
|
||||
macos_uuid: str
|
||||
@@ -58,12 +62,24 @@ def advertisement_record(device: BLEDevice, advertisement: AdvertisementData) ->
|
||||
}
|
||||
|
||||
|
||||
def discovered_device(macos_uuid: str) -> BLEDevice | None:
|
||||
"""Return the live CoreBluetooth handle retained by the latest explicit scan."""
|
||||
|
||||
with _runtime_handle_lock:
|
||||
return _runtime_handles.get(macos_uuid)
|
||||
|
||||
|
||||
async def scan(duration_seconds: float) -> BleScanResult:
|
||||
if duration_seconds <= 0:
|
||||
raise ValueError("duration_seconds must be positive")
|
||||
|
||||
started_at = utc_now_iso()
|
||||
discovered = await BleakScanner.discover(timeout=duration_seconds, return_adv=True)
|
||||
with _runtime_handle_lock:
|
||||
_runtime_handles.clear()
|
||||
_runtime_handles.update(
|
||||
{device.address: device for device, _advertisement in discovered.values()}
|
||||
)
|
||||
devices = [
|
||||
advertisement_record(device, advertisement) for device, advertisement in discovered.values()
|
||||
]
|
||||
|
||||
@@ -10,6 +10,7 @@ from bleak import BleakClient, BleakScanner
|
||||
from bleak.exc import BleakDeviceNotFoundError, BleakError
|
||||
|
||||
from k1link.artifacts import utc_now_iso
|
||||
from k1link.device_plugins.xgrids_k1.ble.scanner import discovered_device
|
||||
|
||||
PROFILE_ID = "xgrids-k1-fw3-wifi-v1"
|
||||
SERVICE_UUID = "00007f00-0000-1000-8000-00805f9b34fb"
|
||||
@@ -66,6 +67,21 @@ class WifiProvisioningResult(TypedDict):
|
||||
outcome: ProvisioningOutcome
|
||||
|
||||
|
||||
class WifiStatusReadResult(TypedDict):
|
||||
schema_version: int
|
||||
profile_id: str
|
||||
observed_at_utc: str
|
||||
adapter: str
|
||||
bleak_version: str
|
||||
device_macos_uuid: str
|
||||
device_name: str
|
||||
service_uuid: str
|
||||
status_characteristic_uuid: str
|
||||
operation: Literal["single_reviewed_wifi_status_read"]
|
||||
write_performed: Literal[False]
|
||||
status: WifiStatus
|
||||
|
||||
|
||||
def build_wifi_provisioning_frame(ssid: str, password: str) -> bytearray:
|
||||
"""Build the deterministic 99-byte frame used by LixelGO for K1 Wi-Fi setup."""
|
||||
ssid_bytes = ssid.encode("utf-8")
|
||||
@@ -143,6 +159,62 @@ def _outcome(
|
||||
return "no_status_change_before_timeout"
|
||||
|
||||
|
||||
async def read_wifi_status_once(
|
||||
device_macos_uuid: str,
|
||||
*,
|
||||
timeout_seconds: float = 20.0,
|
||||
) -> WifiStatusReadResult:
|
||||
"""Read the K1's current DHCP status over BLE without writing a characteristic."""
|
||||
|
||||
if timeout_seconds <= 0:
|
||||
raise ValueError("timeout_seconds must be positive")
|
||||
async with asyncio.timeout(timeout_seconds + 5.0):
|
||||
device = discovered_device(device_macos_uuid)
|
||||
if device is None:
|
||||
device = await BleakScanner.find_device_by_address(
|
||||
device_macos_uuid,
|
||||
timeout=timeout_seconds,
|
||||
)
|
||||
if device is None:
|
||||
raise BleakDeviceNotFoundError(
|
||||
device_macos_uuid,
|
||||
"Device was not rediscovered; keep the K1 powered and nearby.",
|
||||
)
|
||||
|
||||
async with BleakClient(device, timeout=timeout_seconds, pair=False) as client:
|
||||
service = client.services.get_service(SERVICE_UUID)
|
||||
status_characteristic = client.services.get_characteristic(
|
||||
STATUS_CHARACTERISTIC_UUID
|
||||
)
|
||||
if service is None:
|
||||
raise ValueError(f"Reviewed K1 service not found: {SERVICE_UUID}")
|
||||
if status_characteristic is None:
|
||||
raise ValueError(
|
||||
f"Reviewed K1 status characteristic not found: {STATUS_CHARACTERISTIC_UUID}"
|
||||
)
|
||||
if status_characteristic.service_uuid != service.uuid:
|
||||
raise ValueError(
|
||||
"K1 status characteristic is attached to an unexpected service"
|
||||
)
|
||||
if "read" not in status_characteristic.properties:
|
||||
raise ValueError("Reviewed K1 status characteristic is not readable")
|
||||
value = bytes(await client.read_gatt_char(status_characteristic))
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"profile_id": PROFILE_ID,
|
||||
"observed_at_utc": utc_now_iso(),
|
||||
"adapter": "CoreBluetooth",
|
||||
"bleak_version": version("bleak"),
|
||||
"device_macos_uuid": device_macos_uuid,
|
||||
"device_name": client.name or "",
|
||||
"service_uuid": service.uuid,
|
||||
"status_characteristic_uuid": status_characteristic.uuid,
|
||||
"operation": "single_reviewed_wifi_status_read",
|
||||
"write_performed": False,
|
||||
"status": parse_wifi_status(value),
|
||||
}
|
||||
|
||||
|
||||
async def provision_wifi_once(
|
||||
device_macos_uuid: str,
|
||||
ssid: str,
|
||||
@@ -166,10 +238,12 @@ async def provision_wifi_once(
|
||||
|
||||
try:
|
||||
async with asyncio.timeout(timeout_seconds + 25.0):
|
||||
device = await BleakScanner.find_device_by_address(
|
||||
device_macos_uuid,
|
||||
timeout=min(20.0, timeout_seconds),
|
||||
)
|
||||
device = discovered_device(device_macos_uuid)
|
||||
if device is None:
|
||||
device = await BleakScanner.find_device_by_address(
|
||||
device_macos_uuid,
|
||||
timeout=min(20.0, timeout_seconds),
|
||||
)
|
||||
if device is None:
|
||||
raise BleakDeviceNotFoundError(
|
||||
device_macos_uuid,
|
||||
|
||||
@@ -0,0 +1,460 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Final, cast
|
||||
|
||||
import yaml
|
||||
from yaml.constructor import ConstructorError
|
||||
from yaml.tokens import AliasToken, AnchorToken, TagToken
|
||||
|
||||
MAX_FACTORY_YAML_BYTES: Final = 64 * 1024
|
||||
CAMERA_KEYS: Final = ("camera_0", "camera_1", "camera_2", "camera_3")
|
||||
MAIN_CAMERA_SLOT_BY_SOURCE: Final = {
|
||||
"sensor.camera.left": "camera_0",
|
||||
"sensor.camera.right": "camera_1",
|
||||
}
|
||||
MAIN_CAMERA_RTSP_PATH_BY_SOURCE: Final = {
|
||||
"sensor.camera.left": "/live/chn_left_main",
|
||||
"sensor.camera.right": "/live/chn_right_main",
|
||||
}
|
||||
EXPECTED_CAMERA_RESOLUTION: Final = {
|
||||
"camera_0": (4000, 3000),
|
||||
"camera_1": (4000, 3000),
|
||||
"camera_2": (1280, 800),
|
||||
"camera_3": (1280, 800),
|
||||
}
|
||||
ADMITTED_MAIN_STREAM_RESOLUTION: Final = (800, 600)
|
||||
|
||||
Matrix4 = tuple[
|
||||
tuple[float, float, float, float],
|
||||
tuple[float, float, float, float],
|
||||
tuple[float, float, float, float],
|
||||
tuple[float, float, float, float],
|
||||
]
|
||||
|
||||
|
||||
class FactoryCalibrationSchemaError(ValueError):
|
||||
"""A factory calibration document is unsafe or outside the reviewed K1 schema."""
|
||||
|
||||
|
||||
class _UniqueSafeLoader(yaml.SafeLoader):
|
||||
pass
|
||||
|
||||
|
||||
def _construct_unique_mapping(
|
||||
loader: _UniqueSafeLoader,
|
||||
node: yaml.MappingNode,
|
||||
deep: bool = False,
|
||||
) -> dict[object, object]:
|
||||
mapping: dict[object, object] = {}
|
||||
for key_node, value_node in node.value:
|
||||
key = loader.construct_object(key_node, deep=deep)
|
||||
try:
|
||||
duplicate = key in mapping
|
||||
except TypeError as exc:
|
||||
raise ConstructorError(
|
||||
"while constructing a mapping",
|
||||
node.start_mark,
|
||||
"found an unhashable key",
|
||||
key_node.start_mark,
|
||||
) from exc
|
||||
if duplicate:
|
||||
raise ConstructorError(
|
||||
"while constructing a mapping",
|
||||
node.start_mark,
|
||||
f"found duplicate key {key!r}",
|
||||
key_node.start_mark,
|
||||
)
|
||||
mapping[key] = loader.construct_object(value_node, deep=deep)
|
||||
return mapping
|
||||
|
||||
|
||||
_UniqueSafeLoader.add_constructor(
|
||||
yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
|
||||
_construct_unique_mapping,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibration:
|
||||
slot: str
|
||||
camera_model: str
|
||||
camera_pose: Matrix4
|
||||
distortion: tuple[float, float, float, float]
|
||||
image_width: int
|
||||
image_height: int
|
||||
intrinsic: tuple[float, float, float, float]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class K1FactoryCalibration:
|
||||
version: str
|
||||
cameras: tuple[
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
]
|
||||
t_camera_0_from_lidar: Matrix4
|
||||
|
||||
def camera(self, slot: str) -> CameraCalibration:
|
||||
for camera in self.cameras:
|
||||
if camera.slot == slot:
|
||||
return camera
|
||||
raise KeyError(slot)
|
||||
|
||||
def t_camera_from_lidar(self, slot: str) -> Matrix4:
|
||||
camera = self.camera(slot)
|
||||
return _matrix_multiply(
|
||||
_rigid_inverse(camera.camera_pose),
|
||||
self.t_camera_0_from_lidar,
|
||||
)
|
||||
|
||||
def normalized_profile(self) -> dict[str, object]:
|
||||
cameras: list[dict[str, object]] = []
|
||||
for camera in self.cameras:
|
||||
cameras.append(
|
||||
{
|
||||
"slot": camera.slot,
|
||||
"model": camera.camera_model,
|
||||
"native_resolution": [camera.image_width, camera.image_height],
|
||||
"intrinsic_fx_fy_cx_cy": list(camera.intrinsic),
|
||||
"distortion_kb4": list(camera.distortion),
|
||||
"serialized_camera_pose": {
|
||||
"direction": f"T_camera_0_from_{camera.slot}",
|
||||
"row_major": _matrix_as_lists(camera.camera_pose),
|
||||
},
|
||||
"t_camera_from_lidar": {
|
||||
"direction": f"T_{camera.slot}_from_lidar",
|
||||
"row_major": _matrix_as_lists(
|
||||
self.t_camera_from_lidar(camera.slot)
|
||||
),
|
||||
},
|
||||
}
|
||||
)
|
||||
|
||||
streams: dict[str, object] = {}
|
||||
target_width, target_height = ADMITTED_MAIN_STREAM_RESOLUTION
|
||||
for source_id, slot in MAIN_CAMERA_SLOT_BY_SOURCE.items():
|
||||
camera = self.camera(slot)
|
||||
scale_x = target_width / camera.image_width
|
||||
scale_y = target_height / camera.image_height
|
||||
streams[source_id] = {
|
||||
"calibration_slot": slot,
|
||||
"rtsp_path": MAIN_CAMERA_RTSP_PATH_BY_SOURCE[source_id],
|
||||
"native_resolution": [camera.image_width, camera.image_height],
|
||||
"admitted_resolution": [target_width, target_height],
|
||||
"image_transform": {
|
||||
"kind": "firmware-configured-linear-resize",
|
||||
"scale_x": scale_x,
|
||||
"scale_y": scale_y,
|
||||
"crop": None,
|
||||
"warp": None,
|
||||
},
|
||||
"admitted_intrinsic_fx_fy_cx_cy": [
|
||||
camera.intrinsic[0] * scale_x,
|
||||
camera.intrinsic[1] * scale_y,
|
||||
camera.intrinsic[2] * scale_x,
|
||||
camera.intrinsic[3] * scale_y,
|
||||
],
|
||||
"distortion_kb4": list(camera.distortion),
|
||||
"t_camera_from_lidar": {
|
||||
"direction": f"T_{slot}_from_lidar",
|
||||
"row_major": _matrix_as_lists(self.t_camera_from_lidar(slot)),
|
||||
},
|
||||
}
|
||||
|
||||
return {
|
||||
"schema_version": "missioncore.k1-normalized-calibration/v1",
|
||||
"vendor_schema_version": self.version,
|
||||
"transform_notation": "T_destination_from_source",
|
||||
"matrix_storage": "row-major-homogeneous-4x4",
|
||||
"translation_unit": "meter",
|
||||
"base_transform": {
|
||||
"direction": "T_camera_0_from_lidar",
|
||||
"row_major": _matrix_as_lists(self.t_camera_0_from_lidar),
|
||||
},
|
||||
"camera_pose_interpretation": (
|
||||
"serialized T_camera_0_from_camera_N; firmware xcolor inverts it "
|
||||
"before composing T_camera_N_from_lidar"
|
||||
),
|
||||
"cameras": cameras,
|
||||
"stream_bindings": streams,
|
||||
"mapping_proof": {
|
||||
"status": "firmware-profile-verified",
|
||||
"basis": [
|
||||
"K1 firmware main-camera declaration order",
|
||||
"xcolor camera_N sequential loader and main-topic order",
|
||||
"factory native resolutions matching K1 main/secondary profiles",
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def parse_k1_factory_calibration(
|
||||
camera_yaml: bytes,
|
||||
camera_lidar_yaml: bytes,
|
||||
) -> K1FactoryCalibration:
|
||||
camera_document = _load_reviewed_yaml(camera_yaml, "camera.yaml")
|
||||
extrinsic_document = _load_reviewed_yaml(
|
||||
camera_lidar_yaml,
|
||||
"extrinsic_camera_lidar.yaml",
|
||||
)
|
||||
camera_root = _exact_mapping(
|
||||
camera_document,
|
||||
{"calibrated", "version", *CAMERA_KEYS},
|
||||
"camera.yaml",
|
||||
)
|
||||
extrinsic_root = _exact_mapping(
|
||||
extrinsic_document,
|
||||
{"calibrated", "version", "transform"},
|
||||
"extrinsic_camera_lidar.yaml",
|
||||
)
|
||||
if camera_root["calibrated"] is not True:
|
||||
raise FactoryCalibrationSchemaError("camera.yaml is not marked calibrated")
|
||||
if extrinsic_root["calibrated"] is not True:
|
||||
raise FactoryCalibrationSchemaError(
|
||||
"extrinsic_camera_lidar.yaml is not marked calibrated"
|
||||
)
|
||||
camera_version = _short_text(camera_root["version"], "camera.yaml.version")
|
||||
extrinsic_version = _short_text(
|
||||
extrinsic_root["version"],
|
||||
"extrinsic_camera_lidar.yaml.version",
|
||||
)
|
||||
if camera_version != extrinsic_version:
|
||||
raise FactoryCalibrationSchemaError("factory calibration versions do not match")
|
||||
|
||||
parsed_cameras = tuple(
|
||||
_parse_camera(slot, camera_root[slot]) for slot in CAMERA_KEYS
|
||||
)
|
||||
camera_tuple = cast(
|
||||
tuple[
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
CameraCalibration,
|
||||
],
|
||||
parsed_cameras,
|
||||
)
|
||||
if not _matrix_close(camera_tuple[0].camera_pose, _identity_matrix(), 1e-6):
|
||||
raise FactoryCalibrationSchemaError("camera_0 pose must be the identity reference")
|
||||
|
||||
base_transform = _matrix4(
|
||||
extrinsic_root["transform"],
|
||||
"extrinsic_camera_lidar.yaml.transform",
|
||||
)
|
||||
_validate_rigid_transform(
|
||||
base_transform,
|
||||
"extrinsic_camera_lidar.yaml.transform",
|
||||
)
|
||||
return K1FactoryCalibration(
|
||||
version=camera_version,
|
||||
cameras=camera_tuple,
|
||||
t_camera_0_from_lidar=base_transform,
|
||||
)
|
||||
|
||||
|
||||
def _load_reviewed_yaml(payload: bytes, label: str) -> object:
|
||||
if len(payload) > MAX_FACTORY_YAML_BYTES:
|
||||
raise FactoryCalibrationSchemaError(f"{label} exceeds the parser byte bound")
|
||||
if b"\x00" in payload:
|
||||
raise FactoryCalibrationSchemaError(f"{label} contains NUL bytes")
|
||||
try:
|
||||
text = payload.decode("utf-8", errors="strict")
|
||||
except UnicodeDecodeError as exc:
|
||||
raise FactoryCalibrationSchemaError(f"{label} is not valid UTF-8") from exc
|
||||
try:
|
||||
for token in yaml.scan(text, Loader=_UniqueSafeLoader):
|
||||
if isinstance(token, (AliasToken, AnchorToken, TagToken)):
|
||||
raise FactoryCalibrationSchemaError(
|
||||
f"{label} contains YAML anchors, aliases or explicit tags"
|
||||
)
|
||||
value = yaml.load(text, Loader=_UniqueSafeLoader)
|
||||
except FactoryCalibrationSchemaError:
|
||||
raise
|
||||
except yaml.YAMLError as exc:
|
||||
raise FactoryCalibrationSchemaError(f"{label} is not safe valid YAML") from exc
|
||||
return value
|
||||
|
||||
|
||||
def _exact_mapping(value: object, keys: set[str], label: str) -> dict[str, object]:
|
||||
if type(value) is not dict:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must be a mapping")
|
||||
mapping = cast(dict[object, object], value)
|
||||
if not all(type(key) is str for key in mapping):
|
||||
raise FactoryCalibrationSchemaError(f"{label} keys must be strings")
|
||||
typed = cast(dict[str, object], mapping)
|
||||
if set(typed) != keys:
|
||||
raise FactoryCalibrationSchemaError(f"{label} has an unexpected key set")
|
||||
return typed
|
||||
|
||||
|
||||
def _parse_camera(slot: str, value: object) -> CameraCalibration:
|
||||
label = f"camera.yaml.{slot}"
|
||||
node = _exact_mapping(
|
||||
value,
|
||||
{
|
||||
"camera_model",
|
||||
"camera_pose",
|
||||
"distortion",
|
||||
"image_height",
|
||||
"image_width",
|
||||
"intrinsic",
|
||||
},
|
||||
label,
|
||||
)
|
||||
model = _short_text(node["camera_model"], f"{label}.camera_model")
|
||||
if model != "kb4":
|
||||
raise FactoryCalibrationSchemaError(f"{label} must use the reviewed kb4 model")
|
||||
width = _positive_integer(node["image_width"], f"{label}.image_width")
|
||||
height = _positive_integer(node["image_height"], f"{label}.image_height")
|
||||
if (width, height) != EXPECTED_CAMERA_RESOLUTION[slot]:
|
||||
raise FactoryCalibrationSchemaError(f"{label} resolution does not match K1 FW 3.0.2")
|
||||
intrinsic = _float4(node["intrinsic"], f"{label}.intrinsic")
|
||||
fx, fy, cx, cy = intrinsic
|
||||
if fx <= 0 or fy <= 0 or not (0 <= cx <= width) or not (0 <= cy <= height):
|
||||
raise FactoryCalibrationSchemaError(f"{label}.intrinsic is not physically admissible")
|
||||
pose = _matrix4(node["camera_pose"], f"{label}.camera_pose")
|
||||
_validate_rigid_transform(pose, f"{label}.camera_pose")
|
||||
return CameraCalibration(
|
||||
slot=slot,
|
||||
camera_model=model,
|
||||
camera_pose=pose,
|
||||
distortion=_float4(node["distortion"], f"{label}.distortion"),
|
||||
image_width=width,
|
||||
image_height=height,
|
||||
intrinsic=intrinsic,
|
||||
)
|
||||
|
||||
|
||||
def _short_text(value: object, label: str) -> str:
|
||||
if type(value) is not str:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must be a string")
|
||||
text = value
|
||||
if not text or len(text) > 64 or any(ord(character) < 32 for character in text):
|
||||
raise FactoryCalibrationSchemaError(f"{label} is invalid")
|
||||
return text
|
||||
|
||||
|
||||
def _positive_integer(value: object, label: str) -> int:
|
||||
if type(value) is not int or value <= 0:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must be a positive integer")
|
||||
return value
|
||||
|
||||
|
||||
def _finite_number(value: object, label: str) -> float:
|
||||
if type(value) not in {int, float}:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must be numeric")
|
||||
number = float(cast(int | float, value))
|
||||
if not math.isfinite(number):
|
||||
raise FactoryCalibrationSchemaError(f"{label} must be finite")
|
||||
return number
|
||||
|
||||
|
||||
def _float4(value: object, label: str) -> tuple[float, float, float, float]:
|
||||
if type(value) is not list or len(cast(list[object], value)) != 4:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must contain exactly four numbers")
|
||||
numbers = tuple(
|
||||
_finite_number(item, f"{label}[{index}]")
|
||||
for index, item in enumerate(cast(list[object], value))
|
||||
)
|
||||
return cast(tuple[float, float, float, float], numbers)
|
||||
|
||||
|
||||
def _matrix4(value: object, label: str) -> Matrix4:
|
||||
if type(value) is not list or len(cast(list[object], value)) != 16:
|
||||
raise FactoryCalibrationSchemaError(f"{label} must contain exactly 16 numbers")
|
||||
numbers = [
|
||||
_finite_number(item, f"{label}[{index}]")
|
||||
for index, item in enumerate(cast(list[object], value))
|
||||
]
|
||||
return cast(
|
||||
Matrix4,
|
||||
tuple(tuple(numbers[row * 4 : row * 4 + 4]) for row in range(4)),
|
||||
)
|
||||
|
||||
|
||||
def _validate_rigid_transform(matrix: Matrix4, label: str) -> None:
|
||||
if not _matrix_close_row(matrix[3], (0.0, 0.0, 0.0, 1.0), 1e-6):
|
||||
raise FactoryCalibrationSchemaError(f"{label} has an invalid homogeneous row")
|
||||
rotation = tuple(tuple(matrix[row][column] for column in range(3)) for row in range(3))
|
||||
for left in range(3):
|
||||
for right in range(3):
|
||||
dot = sum(rotation[left][axis] * rotation[right][axis] for axis in range(3))
|
||||
expected = 1.0 if left == right else 0.0
|
||||
if abs(dot - expected) > 1e-3:
|
||||
raise FactoryCalibrationSchemaError(f"{label} rotation is not orthonormal")
|
||||
determinant = (
|
||||
rotation[0][0]
|
||||
* (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1])
|
||||
- rotation[0][1]
|
||||
* (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0])
|
||||
+ rotation[0][2]
|
||||
* (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0])
|
||||
)
|
||||
if abs(determinant - 1.0) > 1e-3:
|
||||
raise FactoryCalibrationSchemaError(f"{label} rotation determinant is not +1")
|
||||
if math.sqrt(sum(matrix[row][3] ** 2 for row in range(3))) > 10.0:
|
||||
raise FactoryCalibrationSchemaError(f"{label} translation is outside the meter bound")
|
||||
|
||||
|
||||
def _rigid_inverse(matrix: Matrix4) -> Matrix4:
|
||||
rotation_transpose = tuple(
|
||||
tuple(matrix[column][row] for column in range(3)) for row in range(3)
|
||||
)
|
||||
translation = tuple(matrix[row][3] for row in range(3))
|
||||
inverted_translation = tuple(
|
||||
-sum(rotation_transpose[row][axis] * translation[axis] for axis in range(3))
|
||||
for row in range(3)
|
||||
)
|
||||
return cast(
|
||||
Matrix4,
|
||||
tuple(
|
||||
tuple((*rotation_transpose[row], inverted_translation[row]))
|
||||
for row in range(3)
|
||||
)
|
||||
+ ((0.0, 0.0, 0.0, 1.0),),
|
||||
)
|
||||
|
||||
|
||||
def _matrix_multiply(left: Matrix4, right: Matrix4) -> Matrix4:
|
||||
return cast(
|
||||
Matrix4,
|
||||
tuple(
|
||||
tuple(
|
||||
sum(left[row][axis] * right[axis][column] for axis in range(4))
|
||||
for column in range(4)
|
||||
)
|
||||
for row in range(4)
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _identity_matrix() -> Matrix4:
|
||||
return (
|
||||
(1.0, 0.0, 0.0, 0.0),
|
||||
(0.0, 1.0, 0.0, 0.0),
|
||||
(0.0, 0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 0.0, 1.0),
|
||||
)
|
||||
|
||||
|
||||
def _matrix_close(left: Matrix4, right: Matrix4, tolerance: float) -> bool:
|
||||
return all(
|
||||
abs(left[row][column] - right[row][column]) <= tolerance
|
||||
for row in range(4)
|
||||
for column in range(4)
|
||||
)
|
||||
|
||||
|
||||
def _matrix_close_row(
|
||||
left: tuple[float, float, float, float],
|
||||
right: tuple[float, float, float, float],
|
||||
tolerance: float,
|
||||
) -> bool:
|
||||
return all(abs(left[index] - right[index]) <= tolerance for index in range(4))
|
||||
|
||||
|
||||
def _matrix_as_lists(matrix: Matrix4) -> list[list[float]]:
|
||||
return [list(row) for row in matrix]
|
||||
@@ -0,0 +1,258 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
from collections.abc import Callable
|
||||
from contextlib import suppress
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from uuid import uuid4
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
||||
parse_k1_factory_calibration,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_execution import (
|
||||
ApplicationAuthorityLoader,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.calibration_file import (
|
||||
FACTORY_CAMERA_CALIBRATION_PATH,
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH,
|
||||
CalibrationFileContent,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.calibration_mqtt import (
|
||||
FactoryCalibrationReadResult,
|
||||
ReviewedCalibrationMqttReader,
|
||||
)
|
||||
|
||||
DEVICE_CALIBRATION_SCHEMA_VERSION = "missioncore.device-calibration/v1alpha2"
|
||||
CALIBRATION_SNAPSHOT_MANIFEST_VERSION = "missioncore.k1-calibration-snapshot/v1"
|
||||
CALIBRATION_CAPTURE_SUFFIX = "k1_factory_calibration"
|
||||
|
||||
CalibrationTransportFactory = Callable[[str, bool], ReviewedCalibrationMqttReader]
|
||||
|
||||
|
||||
class DeviceCalibrationSnapshotError(RuntimeError):
|
||||
"""A live factory-calibration snapshot could not be sealed safely."""
|
||||
|
||||
|
||||
class DeviceCalibrationSnapshotReader:
|
||||
"""Read two exact K1 files and seal an append-only private snapshot."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
authority_loader: ApplicationAuthorityLoader,
|
||||
*,
|
||||
compatibility_profile_id: str,
|
||||
transport_factory: CalibrationTransportFactory | None = None,
|
||||
) -> None:
|
||||
self._authority_loader = authority_loader
|
||||
self._compatibility_profile_id = compatibility_profile_id
|
||||
self._transport_factory = transport_factory or _default_transport_factory
|
||||
|
||||
def capture(
|
||||
self,
|
||||
*,
|
||||
host: str,
|
||||
evidence_root: Path,
|
||||
allow_device_ap: bool,
|
||||
) -> dict[str, object]:
|
||||
authority = self._authority_loader.load()
|
||||
transport = self._transport_factory(host, allow_device_ap)
|
||||
result = transport.read_factory_calibration(authority)
|
||||
return seal_factory_calibration_snapshot(
|
||||
result,
|
||||
evidence_root=evidence_root,
|
||||
compatibility_profile_id=self._compatibility_profile_id,
|
||||
)
|
||||
|
||||
|
||||
def unavailable_device_calibration_snapshot(
|
||||
compatibility_profile_id: str | None,
|
||||
) -> dict[str, object]:
|
||||
return {
|
||||
"schema_version": DEVICE_CALIBRATION_SCHEMA_VERSION,
|
||||
"status": "unavailable",
|
||||
"compatibility_profile_id": compatibility_profile_id,
|
||||
"device_internal_calibration": None,
|
||||
"vehicle_extrinsics": "host-domain-not-owned-by-plugin",
|
||||
"reason_code": "device-calibration-not-observed",
|
||||
}
|
||||
|
||||
|
||||
def seal_factory_calibration_snapshot(
|
||||
result: FactoryCalibrationReadResult,
|
||||
*,
|
||||
evidence_root: Path,
|
||||
compatibility_profile_id: str,
|
||||
captured_at: datetime | None = None,
|
||||
) -> dict[str, object]:
|
||||
files_by_path = {item.path: item for item in result.files}
|
||||
if set(files_by_path) != {
|
||||
FACTORY_CAMERA_CALIBRATION_PATH,
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH,
|
||||
}:
|
||||
raise DeviceCalibrationSnapshotError(
|
||||
"factory calibration result does not contain the exact two-file set"
|
||||
)
|
||||
normalized_calibration = parse_k1_factory_calibration(
|
||||
files_by_path[FACTORY_CAMERA_CALIBRATION_PATH].content,
|
||||
files_by_path[FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH].content,
|
||||
).normalized_profile()
|
||||
|
||||
observed_at = (captured_at or datetime.now(UTC)).astimezone(UTC)
|
||||
captured_at_utc = observed_at.isoformat(timespec="milliseconds").replace("+00:00", "Z")
|
||||
snapshot_id = uuid4().hex
|
||||
content_identity = hashlib.sha256()
|
||||
content_identity.update(result.binding.vendor_device_id.encode("ascii"))
|
||||
content_identity.update(b"\x00")
|
||||
content_identity.update(result.binding.device_serial.encode("ascii"))
|
||||
for path in sorted(files_by_path):
|
||||
content_identity.update(b"\x00")
|
||||
content_identity.update(path.encode("utf-8"))
|
||||
content_identity.update(bytes.fromhex(files_by_path[path].content_sha256))
|
||||
|
||||
artifact_specs = (
|
||||
(
|
||||
FACTORY_CAMERA_CALIBRATION_PATH,
|
||||
"camera.yaml",
|
||||
files_by_path[FACTORY_CAMERA_CALIBRATION_PATH],
|
||||
),
|
||||
(
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH,
|
||||
"extrinsic_camera_lidar.yaml",
|
||||
files_by_path[FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH],
|
||||
),
|
||||
)
|
||||
artifact_documents = [
|
||||
_artifact_document(source_path, artifact_name, content)
|
||||
for source_path, artifact_name, content in artifact_specs
|
||||
]
|
||||
private_root = evidence_root.expanduser().resolve() / "private" / "device-calibration"
|
||||
_ensure_private_directory(private_root)
|
||||
stamp = observed_at.strftime("%Y%m%dT%H%M%SZ")
|
||||
final_name = f"{stamp}_{CALIBRATION_CAPTURE_SUFFIX}_{snapshot_id[:12]}"
|
||||
final_dir = private_root / final_name
|
||||
staging_dir = private_root / f".{final_name}.incomplete"
|
||||
try:
|
||||
staging_dir.mkdir(mode=0o700, exist_ok=False)
|
||||
for _source_path, artifact_name, content in artifact_specs:
|
||||
_write_exclusive(staging_dir / artifact_name, content.content)
|
||||
private_reference = str(final_dir)
|
||||
manifest = {
|
||||
"schema_version": CALIBRATION_SNAPSHOT_MANIFEST_VERSION,
|
||||
"snapshot_id": snapshot_id,
|
||||
"captured_at_utc": captured_at_utc,
|
||||
"compatibility_profile_id": compatibility_profile_id,
|
||||
"content_identity_sha256": content_identity.hexdigest(),
|
||||
"device": {
|
||||
"vendor_device_id": result.binding.vendor_device_id,
|
||||
"device_serial": result.binding.device_serial,
|
||||
"device_model": result.binding.device_model,
|
||||
"platform_type": result.binding.device_type,
|
||||
"software_version": result.binding.software_version,
|
||||
"system_version": result.binding.system_version,
|
||||
"is_activated": result.binding.is_activated,
|
||||
},
|
||||
"source": {
|
||||
"transport": "mqtt-protobuf-read-only",
|
||||
"request_command": 5,
|
||||
"write_command_available": False,
|
||||
"automatic_retry": False,
|
||||
"path_allowlist": [
|
||||
FACTORY_CAMERA_CALIBRATION_PATH,
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH,
|
||||
],
|
||||
"transport_snapshot": result.transport,
|
||||
},
|
||||
"artifacts": artifact_documents,
|
||||
"normalized_calibration": normalized_calibration,
|
||||
"storage": {
|
||||
"classification": "private-device-calibration",
|
||||
"append_only": True,
|
||||
"snapshot_path": private_reference,
|
||||
},
|
||||
}
|
||||
manifest_bytes = (
|
||||
json.dumps(manifest, ensure_ascii=False, indent=2, sort_keys=True) + "\n"
|
||||
).encode("utf-8")
|
||||
_write_exclusive(staging_dir / "manifest.json", manifest_bytes)
|
||||
os.rename(staging_dir, final_dir)
|
||||
_fsync_directory(private_root)
|
||||
except Exception:
|
||||
with suppress(OSError):
|
||||
shutil.rmtree(staging_dir)
|
||||
raise
|
||||
|
||||
return {
|
||||
"schema_version": DEVICE_CALIBRATION_SCHEMA_VERSION,
|
||||
"status": "available",
|
||||
"compatibility_profile_id": compatibility_profile_id,
|
||||
"snapshot_id": snapshot_id,
|
||||
"captured_at_utc": captured_at_utc,
|
||||
"device_internal_calibration": {
|
||||
"source": "xgrids-factory-data-live-read",
|
||||
"device_serial": result.binding.device_serial,
|
||||
"firmware_version": result.binding.software_version,
|
||||
"content_identity_sha256": content_identity.hexdigest(),
|
||||
"documents": artifact_documents,
|
||||
"private_snapshot_path": str(final_dir),
|
||||
"normalized_calibration": normalized_calibration,
|
||||
"camera_stream_mapping": normalized_calibration["mapping_proof"],
|
||||
},
|
||||
"vehicle_extrinsics": "host-domain-not-owned-by-plugin",
|
||||
"reason_code": None,
|
||||
}
|
||||
|
||||
|
||||
def _default_transport_factory(host: str, allow_device_ap: bool) -> ReviewedCalibrationMqttReader:
|
||||
return ReviewedCalibrationMqttReader(host, allow_device_ap=allow_device_ap)
|
||||
|
||||
|
||||
def _artifact_document(
|
||||
source_path: str,
|
||||
artifact_name: str,
|
||||
content: CalibrationFileContent,
|
||||
) -> dict[str, object]:
|
||||
return {
|
||||
"source_path": source_path,
|
||||
"artifact_name": artifact_name,
|
||||
"sha256": content.content_sha256,
|
||||
"bytes": content.content_bytes,
|
||||
"encoding": "utf-8",
|
||||
}
|
||||
|
||||
|
||||
def _ensure_private_directory(path: Path) -> None:
|
||||
path.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
current = path
|
||||
while current.name in {"private", "device-calibration"}:
|
||||
with suppress(OSError):
|
||||
current.chmod(0o700)
|
||||
current = current.parent
|
||||
|
||||
|
||||
def _write_exclusive(path: Path, payload: bytes) -> None:
|
||||
flags = os.O_WRONLY | os.O_CREAT | os.O_EXCL
|
||||
if hasattr(os, "O_NOFOLLOW"):
|
||||
flags |= os.O_NOFOLLOW
|
||||
descriptor = os.open(path, flags, 0o600)
|
||||
try:
|
||||
with os.fdopen(descriptor, "wb", closefd=False) as stream:
|
||||
stream.write(payload)
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
|
||||
|
||||
def _fsync_directory(path: Path) -> None:
|
||||
flags = os.O_RDONLY
|
||||
if hasattr(os, "O_DIRECTORY"):
|
||||
flags |= os.O_DIRECTORY
|
||||
descriptor = os.open(path, flags)
|
||||
try:
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
@@ -7,6 +7,7 @@ import signal
|
||||
import subprocess
|
||||
import threading
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from contextlib import suppress
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
@@ -52,6 +53,22 @@ class CameraProcessLease:
|
||||
failure_code: str | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CommittedCameraSegment:
|
||||
"""A camera fragment observed only after its raw archive commit."""
|
||||
|
||||
source_id: CameraSourceId
|
||||
generation: int
|
||||
kind: CameraArchiveKind
|
||||
sequence: int
|
||||
host_epoch_ns: int
|
||||
host_monotonic_ns: int
|
||||
payload: bytes
|
||||
|
||||
|
||||
CommittedCameraSegmentObserver = Callable[[CommittedCameraSegment], None]
|
||||
|
||||
|
||||
@dataclass
|
||||
class _CameraProducer:
|
||||
generation: int
|
||||
@@ -78,7 +95,13 @@ class XgridsK1CameraGateway:
|
||||
Outside an acquisition the legacy lazy-preview lifecycle remains available.
|
||||
"""
|
||||
|
||||
def __init__(self, repository_root: Path, plugin_id: str) -> None:
|
||||
def __init__(
|
||||
self,
|
||||
repository_root: Path,
|
||||
plugin_id: str,
|
||||
*,
|
||||
committed_segment_observer: CommittedCameraSegmentObserver | None = None,
|
||||
) -> None:
|
||||
self._repository_root = repository_root.resolve()
|
||||
self._plugin_id = plugin_id
|
||||
self._lock = threading.RLock()
|
||||
@@ -94,6 +117,8 @@ class XgridsK1CameraGateway:
|
||||
self._archive_summaries: list[dict[str, Any]] = []
|
||||
self._error: dict[str, str] | None = None
|
||||
self._closed = False
|
||||
self._committed_segment_observer = committed_segment_observer
|
||||
self._committed_segment_observer_errors = 0
|
||||
self._ffmpeg_path, self._ffmpeg_source = _resolve_ffmpeg(self._repository_root)
|
||||
|
||||
def snapshot(self) -> dict[str, Any]:
|
||||
@@ -148,6 +173,7 @@ class XgridsK1CameraGateway:
|
||||
"source": self._ffmpeg_source,
|
||||
},
|
||||
"error": dict(self._error) if self._error is not None else None,
|
||||
"derived_observer_errors": self._committed_segment_observer_errors,
|
||||
}
|
||||
|
||||
def select(self, source_id: CameraSourceId, target_host: str) -> dict[str, Any]:
|
||||
@@ -531,7 +557,7 @@ class XgridsK1CameraGateway:
|
||||
if archive is not None:
|
||||
try:
|
||||
# Source of record first; preview is always expendable.
|
||||
archive.append(kind, payload)
|
||||
committed = archive.append(kind, payload)
|
||||
except (CameraArchiveError, OSError, ValueError):
|
||||
self._mark_producer_failure(
|
||||
producer,
|
||||
@@ -539,6 +565,26 @@ class XgridsK1CameraGateway:
|
||||
"Долговременная запись camera stream завершилась ошибкой.",
|
||||
)
|
||||
return False
|
||||
observer = self._committed_segment_observer
|
||||
if observer is not None:
|
||||
try:
|
||||
observer(
|
||||
CommittedCameraSegment(
|
||||
source_id=producer.source_id,
|
||||
generation=producer.generation,
|
||||
kind=kind,
|
||||
sequence=int(committed["sequence"]),
|
||||
host_epoch_ns=int(committed["host_epoch_ns"]),
|
||||
host_monotonic_ns=int(committed["host_monotonic_ns"]),
|
||||
payload=payload,
|
||||
)
|
||||
)
|
||||
except Exception:
|
||||
# Derived diagnostics can be dropped, but can never fail or
|
||||
# back-pressure the authoritative camera recording.
|
||||
with self._lock:
|
||||
self._committed_segment_observer_errors += 1
|
||||
self._revision += 1
|
||||
|
||||
with self._lock:
|
||||
producer_owned = self._producer is producer or (
|
||||
|
||||
@@ -21,6 +21,8 @@ from k1link.compute import prepare_camera_compute_job
|
||||
from k1link.device_plugins.xgrids_k1.analyze import (
|
||||
DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES,
|
||||
MAX_STREAM_SUMMARY_PAYLOAD_BYTES,
|
||||
CalibratedOverlayExperimentError,
|
||||
run_calibrated_overlay_experiment,
|
||||
summarize_mqtt_streams,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.archive import discover_legacy_viewer_sessions
|
||||
@@ -31,6 +33,10 @@ from k1link.device_plugins.xgrids_k1.ble.wifi_provisioning import (
|
||||
WriteMode,
|
||||
provision_wifi_once,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.firmware_credential import (
|
||||
FirmwareCredentialError,
|
||||
import_k1_fw302_ap_credential,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.mqtt import (
|
||||
DEFAULT_MAX_MESSAGE_BYTES,
|
||||
MAX_CONFIGURABLE_MESSAGE_BYTES,
|
||||
@@ -151,6 +157,53 @@ def authority_provision(
|
||||
)
|
||||
|
||||
|
||||
@authority_app.command("import-k1-fw302-ap")
|
||||
def import_k1_fw302_ap(
|
||||
firmware: Annotated[
|
||||
Path,
|
||||
typer.Option(
|
||||
exists=True,
|
||||
dir_okay=False,
|
||||
readable=True,
|
||||
resolve_path=True,
|
||||
help="Official XGRIDS K1 3.0.2 full firmware archive.",
|
||||
),
|
||||
],
|
||||
confirm_reviewed_firmware: Annotated[
|
||||
bool,
|
||||
typer.Option(
|
||||
"--confirm-reviewed-firmware",
|
||||
help="Confirm offline import from the exact reviewed official artifact.",
|
||||
),
|
||||
] = False,
|
||||
) -> None:
|
||||
"""Install the exact K1 AP material in the OS credential store."""
|
||||
|
||||
if not confirm_reviewed_firmware:
|
||||
console.print(
|
||||
"[red]Firmware credential import not confirmed.[/red] "
|
||||
"Add --confirm-reviewed-firmware for the reviewed official 3.0.2 image."
|
||||
)
|
||||
raise typer.Exit(code=2)
|
||||
helper = (
|
||||
Path(__file__).resolve().parents[4]
|
||||
/ "plugins"
|
||||
/ "xgrids-k1"
|
||||
/ "macos"
|
||||
/ "associate_wifi.swift"
|
||||
)
|
||||
try:
|
||||
result = import_k1_fw302_ap_credential(firmware, helper)
|
||||
except (FirmwareCredentialError, OSError, ValueError) as exc:
|
||||
console.print(f"[red]Firmware credential import failed:[/red] {exc}")
|
||||
raise typer.Exit(code=2) from exc
|
||||
console.print(
|
||||
"[green]Exact firmware credential provider installed.[/green] "
|
||||
f"provider={result.provider_id!r}; adapter={result.host_adapter!r}; "
|
||||
"secret_exposed=false; no K1 command was sent."
|
||||
)
|
||||
|
||||
|
||||
def _default_route_interface() -> str | None:
|
||||
output = _command_output(["route", "-n", "get", "default"])
|
||||
if output is None:
|
||||
@@ -463,7 +516,7 @@ def net_mqtt_capture(
|
||||
] = 1883,
|
||||
duration: Annotated[
|
||||
float,
|
||||
typer.Option(min=1.0, max=3600.0, help="Capture duration after SUBACK, in seconds."),
|
||||
typer.Option(min=1.0, help="Capture duration after SUBACK, in seconds."),
|
||||
] = 60.0,
|
||||
max_message_bytes: Annotated[
|
||||
int,
|
||||
@@ -587,6 +640,90 @@ def analyze_mqtt_streams(
|
||||
console.print(f"Saved: {out}")
|
||||
|
||||
|
||||
@analyze_app.command("calibrated-overlay")
|
||||
def analyze_calibrated_overlay(
|
||||
session_root: Annotated[
|
||||
Path,
|
||||
typer.Option(
|
||||
"--session-root",
|
||||
exists=True,
|
||||
file_okay=False,
|
||||
dir_okay=True,
|
||||
readable=True,
|
||||
resolve_path=True,
|
||||
help="Sealed observation session containing MQTT and camera evidence.",
|
||||
),
|
||||
],
|
||||
calibration_snapshot: Annotated[
|
||||
Path,
|
||||
typer.Option(
|
||||
"--calibration-snapshot",
|
||||
exists=True,
|
||||
file_okay=False,
|
||||
dir_okay=True,
|
||||
readable=True,
|
||||
resolve_path=True,
|
||||
help="Private physical K1 factory-calibration snapshot directory.",
|
||||
),
|
||||
],
|
||||
out_root: Annotated[
|
||||
Path,
|
||||
typer.Option(
|
||||
"--out-root",
|
||||
help="Mission Core evidence root; the result is sealed below private/.",
|
||||
),
|
||||
],
|
||||
source_id: Annotated[
|
||||
str,
|
||||
typer.Option("--source", help="Canonical archived K1 main-camera source id."),
|
||||
] = "sensor.camera.right",
|
||||
video_offsets: Annotated[
|
||||
list[float] | None,
|
||||
typer.Option(
|
||||
"--video-offset",
|
||||
help="Repeat for each diagnostic second inside the camera epoch.",
|
||||
),
|
||||
] = None,
|
||||
temporal_offset_seconds: Annotated[
|
||||
float,
|
||||
typer.Option(
|
||||
"--temporal-offset",
|
||||
min=-5.0,
|
||||
max=5.0,
|
||||
help="Explicit LiDAR minus camera host-arrival offset for experiments.",
|
||||
),
|
||||
] = 0.0,
|
||||
) -> None:
|
||||
"""Build a read-only LiDAR→KB4→camera diagnostic from sealed evidence."""
|
||||
|
||||
ffmpeg = shutil.which("ffmpeg")
|
||||
if ffmpeg is None:
|
||||
console.print("[red]Calibrated overlay failed:[/red] ffmpeg is unavailable")
|
||||
raise typer.Exit(code=2)
|
||||
offsets = tuple(video_offsets or (60.0, 180.0, 300.0, 420.0))
|
||||
try:
|
||||
result = run_calibrated_overlay_experiment(
|
||||
session_root=session_root,
|
||||
calibration_snapshot_root=calibration_snapshot,
|
||||
source_id=source_id,
|
||||
video_offsets_seconds=offsets,
|
||||
output_root=out_root,
|
||||
ffmpeg_path=Path(ffmpeg),
|
||||
temporal_offset_seconds=temporal_offset_seconds,
|
||||
)
|
||||
except (CalibratedOverlayExperimentError, OSError, RuntimeError, ValueError) as exc:
|
||||
console.print(f"[red]Calibrated overlay failed:[/red] {type(exc).__name__}: {exc}")
|
||||
raise typer.Exit(code=2) from exc
|
||||
console.print(
|
||||
f"[green]Calibrated diagnostic ready:[/green] {result.experiment_id}; "
|
||||
f"frames: {result.frame_count}; source: {result.source_id}"
|
||||
)
|
||||
console.print(f"Calibration: {result.calibration_content_identity}")
|
||||
console.print(f"Rerun: {result.rerun_path}")
|
||||
console.print(f"Mosaic: {result.mosaic_path}")
|
||||
console.print("P0 remains open until static-landmark reprojection is measured.")
|
||||
|
||||
|
||||
@compute_app.command("prepare-camera-job")
|
||||
def prepare_camera_job(
|
||||
session_root: Annotated[
|
||||
|
||||
@@ -32,20 +32,36 @@ from pydantic import (
|
||||
)
|
||||
|
||||
from k1link.artifacts import write_json_atomic
|
||||
from k1link.compute.live_perception import LivePerceptionIngress
|
||||
from k1link.device_plugins.xgrids_k1.ble.ap_activation import (
|
||||
device_ap_activation_session,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.ble.scanner import scan
|
||||
from k1link.device_plugins.xgrids_k1.ble.wifi_provisioning import (
|
||||
AP_FALLBACK_IPV4,
|
||||
provision_wifi_once,
|
||||
read_wifi_status_once,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.calibration_snapshot import (
|
||||
DeviceCalibrationSnapshotReader,
|
||||
unavailable_device_calibration_snapshot,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.camera import (
|
||||
CAMERA_EXCLUSIVE_GROUP,
|
||||
CAMERA_SOURCE_LABELS,
|
||||
CAMERA_SOURCE_PATHS,
|
||||
CameraSourceId,
|
||||
CommittedCameraSegment,
|
||||
XgridsK1CameraGateway,
|
||||
build_xgrids_k1_camera_router,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.macos_wifi import associate_with_wifi_once
|
||||
from k1link.device_plugins.xgrids_k1.firmware_credential import (
|
||||
K1_FW302_CREDENTIAL_PROVIDER_ID,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.live_perception_shadow import (
|
||||
build_live_perception_shadow_router,
|
||||
ensure_live_shadow_token,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.mqtt import validate_private_ipv4
|
||||
from k1link.device_plugins.xgrids_k1.mqtt.capture import seal_capture_clock
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_authority import (
|
||||
@@ -71,11 +87,19 @@ from k1link.device_plugins.xgrids_k1.protocol.modeling_safety import (
|
||||
LiveModelingControlSafety,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.normalizer import normalize_k1_message
|
||||
from k1link.device_plugins.xgrids_k1.quick_connect_profile import (
|
||||
quick_connect_host_profile_id,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage
|
||||
from k1link.device_plugins.xgrids_k1.viewer.runtime import (
|
||||
VisualizationRuntime,
|
||||
new_live_session_dir,
|
||||
)
|
||||
from k1link.host_network import (
|
||||
HostWifiProfileError,
|
||||
associate_with_wifi_profile_once,
|
||||
ensure_wifi_profile_from_credential_source,
|
||||
)
|
||||
from k1link.sessions import ActiveSessionLease, resolve_missioncore_evidence_dir
|
||||
from k1link.viewer.metrics import BridgeMetrics
|
||||
from k1link.viewer.rerun_bridge import RerunSceneSettings
|
||||
@@ -203,8 +227,8 @@ class CompatibilityAttestationRequest(StrictRequest):
|
||||
|
||||
class ConnectRequest(StrictRequest):
|
||||
device_id: str = Field(min_length=1, max_length=128)
|
||||
ssid: str = Field(min_length=1, max_length=128)
|
||||
password: SecretStr = Field(min_length=1, max_length=256)
|
||||
ssid: str | None = Field(default=None, min_length=1, max_length=128)
|
||||
password: SecretStr | None = Field(default=None, min_length=1, max_length=256)
|
||||
connection_mode: ConnectionMode = "bridge"
|
||||
compatibility_attestation: CompatibilityAttestationRequest
|
||||
operation_id: str | None = Field(default=None, min_length=1, max_length=128)
|
||||
@@ -212,24 +236,35 @@ class ConnectRequest(StrictRequest):
|
||||
|
||||
@model_validator(mode="after")
|
||||
def validate_connection_topology(self) -> Self:
|
||||
expected = CONNECTION_TOPOLOGY_BY_MODE[self.connection_mode]
|
||||
if self.compatibility_attestation.topology != expected:
|
||||
raise ValueError(
|
||||
f"connection_mode={self.connection_mode} requires topology={expected}"
|
||||
)
|
||||
if self.connection_mode == "quick-connect":
|
||||
if self.ssid is not None or self.password is not None:
|
||||
raise ValueError(
|
||||
"Quick Connect resolves its credential from the host Wi-Fi profile"
|
||||
)
|
||||
return self
|
||||
if self.ssid is None or self.password is None:
|
||||
raise ValueError("SSID and Wi-Fi password are required for this connection mode")
|
||||
if not 1 <= len(self.ssid.encode("utf-8")) <= 32:
|
||||
raise ValueError("SSID must contain between 1 and 32 UTF-8 bytes")
|
||||
if not 1 <= len(self.password.get_secret_value().encode("utf-8")) <= 64:
|
||||
raise ValueError(
|
||||
"Wi-Fi password must contain between 1 and 64 UTF-8 bytes"
|
||||
)
|
||||
expected = CONNECTION_TOPOLOGY_BY_MODE[self.connection_mode]
|
||||
if self.compatibility_attestation.topology != expected:
|
||||
raise ValueError(
|
||||
f"connection_mode={self.connection_mode} requires topology={expected}"
|
||||
)
|
||||
return self
|
||||
|
||||
|
||||
class LiveRequest(StrictRequest):
|
||||
project_name: str = Field(min_length=1, max_length=96)
|
||||
host: str | None = Field(default=None, max_length=15)
|
||||
duration_seconds: float = Field(default=3600.0, ge=1.0, le=86_400.0)
|
||||
# Omitted means run until the operator explicitly stops the acquisition.
|
||||
# A positive value remains available to compatibility clients that need a
|
||||
# bounded capture, but there is no application-level maximum.
|
||||
duration_seconds: float | None = Field(default=None, ge=1.0, allow_inf_nan=False)
|
||||
compatibility_attestation: CompatibilityAttestationRequest
|
||||
|
||||
@field_validator("project_name")
|
||||
@@ -269,7 +304,7 @@ class PrepareAcquisitionRequest(OperationContextRequest):
|
||||
mount_type: Literal["handheld"] = "handheld"
|
||||
gnss_mode: Literal["none"] = "none"
|
||||
host: str | None = Field(default=None, max_length=15)
|
||||
duration_seconds: float = Field(default=3600.0, ge=1.0, le=86_400.0)
|
||||
duration_seconds: float | None = Field(default=None, ge=1.0, allow_inf_nan=False)
|
||||
requested_streams: tuple[RequestedStreamId, ...] = DEFAULT_LIVE_STREAMS
|
||||
evidence_policy: Literal["required", "best-effort", "disabled"] = "required"
|
||||
compatibility_attestation: CompatibilityAttestationRequest
|
||||
@@ -322,6 +357,9 @@ class ViewerSettingsRequest(StrictRequest):
|
||||
show_points: bool = True
|
||||
show_trajectory: bool = True
|
||||
show_grid: bool = True
|
||||
show_detections_2d: bool = False
|
||||
show_segmentation: bool = False
|
||||
show_cuboids_3d: bool = False
|
||||
|
||||
|
||||
class ShadowApplicationControlArmRequest(StrictRequest):
|
||||
@@ -354,12 +392,14 @@ class XgridsK1CompatibilityService:
|
||||
repository_root: Path,
|
||||
*,
|
||||
application_authority_loader: ApplicationAuthorityLoader | None = None,
|
||||
calibration_snapshot_reader: DeviceCalibrationSnapshotReader | None = None,
|
||||
) -> None:
|
||||
self.repository_root = repository_root.resolve()
|
||||
self.evidence_root = resolve_missioncore_evidence_dir(self.repository_root)
|
||||
self._lock = threading.Lock()
|
||||
self._acquisition_lifecycle_gate = threading.RLock()
|
||||
self._provisioning_gate = threading.Lock()
|
||||
self._calibration_gate = threading.Lock()
|
||||
self._provisioning_active = False
|
||||
self._fingerprint_key = secrets.token_bytes(32)
|
||||
self._devices: list[dict[str, Any]] = []
|
||||
@@ -391,9 +431,22 @@ class XgridsK1CompatibilityService:
|
||||
# The host-owned visual runtime receives the vendor normalizer
|
||||
# explicitly. There is no implicit K1 decoder in the visual layer.
|
||||
self._modeling_control_safety = LiveModelingControlSafety()
|
||||
self.live_perception_ingress = LivePerceptionIngress()
|
||||
(
|
||||
self.live_perception_token_path,
|
||||
self._live_perception_token,
|
||||
) = ensure_live_shadow_token(self.repository_root)
|
||||
authority_loader = (
|
||||
application_authority_loader or MacOSKeychainApplicationAuthorityLoader()
|
||||
)
|
||||
self._calibration_snapshot_reader = (
|
||||
calibration_snapshot_reader
|
||||
or DeviceCalibrationSnapshotReader(
|
||||
authority_loader,
|
||||
compatibility_profile_id=XGRIDS_K1_COMPATIBILITY_PROFILE_ID,
|
||||
)
|
||||
)
|
||||
self._device_calibration = unavailable_device_calibration_snapshot(None)
|
||||
self._application_control = DormantApplicationControlCoordinator(
|
||||
authority_loader,
|
||||
WriteDisabledOneShotPublisher(UninstalledApplicationPublishSink()),
|
||||
@@ -409,6 +462,7 @@ class XgridsK1CompatibilityService:
|
||||
self.camera_preview = XgridsK1CameraGateway(
|
||||
self.repository_root,
|
||||
XGRIDS_K1_PLUGIN_ID,
|
||||
committed_segment_observer=self._observe_committed_camera_segment,
|
||||
)
|
||||
|
||||
def _application_control_transport(self, host: str) -> ReviewedApplicationMqttTransport:
|
||||
@@ -445,6 +499,7 @@ class XgridsK1CompatibilityService:
|
||||
if self._compatibility_attestation is not None
|
||||
else None
|
||||
)
|
||||
device_calibration = dict(self._device_calibration)
|
||||
acquisition = self._acquisition.as_dict() if self._acquisition is not None else None
|
||||
if acquisition is not None:
|
||||
acquisition["project_name"] = self._acquisition_project_name
|
||||
@@ -487,6 +542,8 @@ class XgridsK1CompatibilityService:
|
||||
active_profile_id = (
|
||||
XGRIDS_K1_COMPATIBILITY_PROFILE_ID if compatibility_attestation is not None else None
|
||||
)
|
||||
if device_calibration.get("status") != "available":
|
||||
device_calibration = unavailable_device_calibration_snapshot(active_profile_id)
|
||||
active_control = application_control_session["state"] not in {
|
||||
"idle",
|
||||
"completed",
|
||||
@@ -559,8 +616,9 @@ class XgridsK1CompatibilityService:
|
||||
device_session_id,
|
||||
camera_preview,
|
||||
),
|
||||
"device_calibration": _device_calibration_snapshot(active_profile_id),
|
||||
"device_calibration": device_calibration,
|
||||
"camera_preview": camera_preview,
|
||||
"live_perception_shadow": self.live_perception_ingress.snapshot(),
|
||||
"acquisition": acquisition,
|
||||
"operations": operation_documents,
|
||||
"last_operation": operation_documents[-1] if operation_documents else None,
|
||||
@@ -577,6 +635,11 @@ class XgridsK1CompatibilityService:
|
||||
"frame_rate_hz": metrics["pcl_fps"],
|
||||
"point_count": metrics["last_point_count"],
|
||||
"dropped_preview_frames": metrics["preview_dropped"],
|
||||
"ai_end_to_end_ms": metrics.get("perception_end_to_end_ms"),
|
||||
"ai_end_to_end_p95_ms": metrics.get("perception_end_to_end_p95_ms"),
|
||||
"ai_frame_rate_hz": metrics.get("perception_fps"),
|
||||
"ai_dropped_frames": metrics.get("perception_dropped"),
|
||||
"ai_stale_ms": metrics.get("perception_stale_ms"),
|
||||
**metrics,
|
||||
},
|
||||
}
|
||||
@@ -639,17 +702,45 @@ class XgridsK1CompatibilityService:
|
||||
known_ids = {str(item["device_id"]) for item in self.state()["devices"]}
|
||||
if request.device_id not in known_ids:
|
||||
raise ValueError("сначала найдите и выберите устройство через Bluetooth")
|
||||
# Unwrap once at the network service boundary. The plain value is kept
|
||||
# only in this stack frame, included in a keyed request digest, and
|
||||
# passed either to the reviewed BLE write or to the short-lived macOS
|
||||
# CoreWLAN helper over stdin; it is never journaled.
|
||||
password = request.password.get_secret_value()
|
||||
quick_connect = request.connection_mode == "quick-connect"
|
||||
selected_device = next(
|
||||
item for item in self.state()["devices"] if item["device_id"] == request.device_id
|
||||
)
|
||||
selected_device_name = str(selected_device.get("name") or "").strip()
|
||||
if quick_connect and not selected_device_name:
|
||||
raise ValueError(
|
||||
"выбранный BLE-кандидат не сообщил имя точки доступа; "
|
||||
"Quick Connect остановлен без команды устройству"
|
||||
)
|
||||
quick_connect_profile_id = (
|
||||
quick_connect_host_profile_id(selected_device_name) if quick_connect else None
|
||||
)
|
||||
host_wifi_helper_path = (
|
||||
self.repository_root
|
||||
/ "plugins"
|
||||
/ "xgrids-k1"
|
||||
/ "macos"
|
||||
/ "associate_wifi.swift"
|
||||
)
|
||||
# Bridge and Direct Connect unwrap once at the BLE service boundary.
|
||||
# Quick Connect carries no browser/API credential. It first sends the
|
||||
# reviewed fixed AP-enable command, then the host-network adapter uses
|
||||
# the selected device's advertised name as its exact SSID and resolves
|
||||
# a device-scoped profile inside the OS credential store.
|
||||
password = (
|
||||
""
|
||||
if request.password is None
|
||||
else request.password.get_secret_value()
|
||||
)
|
||||
request_fingerprint = self._request_fingerprint(
|
||||
ACTION_NETWORK_PROVISION,
|
||||
{
|
||||
"device_id": request.device_id,
|
||||
"ssid": request.ssid,
|
||||
"password": password,
|
||||
"password": password if not quick_connect else None,
|
||||
"host_wifi_profile": (
|
||||
quick_connect_profile_id if quick_connect else None
|
||||
),
|
||||
"connection_mode": request.connection_mode,
|
||||
"compatibility_attestation": request.compatibility_attestation.model_dump(
|
||||
mode="json"
|
||||
@@ -662,7 +753,7 @@ class XgridsK1CompatibilityService:
|
||||
idempotency_key=request.idempotency_key,
|
||||
device_id=self._device_id,
|
||||
device_session_id=self._device_session_id,
|
||||
deadline_seconds=60.0,
|
||||
deadline_seconds=240.0,
|
||||
request_fingerprint=request_fingerprint,
|
||||
)
|
||||
if not created:
|
||||
@@ -688,8 +779,34 @@ class XgridsK1CompatibilityService:
|
||||
|
||||
session_dir: Path | None = None
|
||||
network_change_attempted = False
|
||||
quick_connect = request.connection_mode == "quick-connect"
|
||||
operation_stage = (
|
||||
"device-ap-activation" if quick_connect else "ble-provisioning-write"
|
||||
)
|
||||
try:
|
||||
if quick_connect:
|
||||
assert quick_connect_profile_id is not None
|
||||
operation_stage = "host-wifi-profile-preflight"
|
||||
self._set_operation(
|
||||
"credential_preflight",
|
||||
"Проверяем локальный профиль выбранного K1 до команды устройству.",
|
||||
)
|
||||
self._operations.transition(
|
||||
operation.operation_id,
|
||||
"running",
|
||||
stage_code=operation_stage,
|
||||
message_code="network.provision.host_wifi_profile_preflight",
|
||||
)
|
||||
profile_preflight = await asyncio.to_thread(
|
||||
ensure_wifi_profile_from_credential_source,
|
||||
host_wifi_helper_path,
|
||||
quick_connect_profile_id,
|
||||
selected_device_name,
|
||||
K1_FW302_CREDENTIAL_PROVIDER_ID,
|
||||
timeout_seconds=30.0,
|
||||
)
|
||||
if not profile_preflight["available"]:
|
||||
raise HostWifiProfileError("credential-source-unavailable")
|
||||
|
||||
with self._lock:
|
||||
active_acquisition = self._acquisition
|
||||
if (
|
||||
@@ -708,6 +825,7 @@ class XgridsK1CompatibilityService:
|
||||
self._k1_ip = None
|
||||
self._connection_mode = None
|
||||
self._compatibility_attestation = None
|
||||
self._device_calibration = unavailable_device_calibration_snapshot(None)
|
||||
self._device_session_id = None
|
||||
self._device_session_opened_at = None
|
||||
self._connection_verification = {
|
||||
@@ -725,7 +843,7 @@ class XgridsK1CompatibilityService:
|
||||
self.camera_preview.stop_current()
|
||||
|
||||
operation_message = (
|
||||
"Подключаем этот Mac к точке доступа выбранного K1 одним запросом."
|
||||
"Включаем точку доступа выбранного K1 и подключаем к ней управляющее устройство."
|
||||
if quick_connect
|
||||
else "Передаём устройству настройки Wi-Fi одним подтверждённым запросом."
|
||||
)
|
||||
@@ -740,38 +858,102 @@ class XgridsK1CompatibilityService:
|
||||
self._operations.transition(
|
||||
operation.operation_id,
|
||||
"running",
|
||||
stage_code=(
|
||||
"host-wifi-association" if quick_connect else "ble-provisioning-write"
|
||||
),
|
||||
stage_code=operation_stage,
|
||||
message_code="network.provision.running",
|
||||
)
|
||||
network_change_attempted = True
|
||||
if quick_connect:
|
||||
started_at = _utc_now_iso()
|
||||
association = await asyncio.to_thread(
|
||||
associate_with_wifi_once,
|
||||
self.repository_root
|
||||
/ "plugins"
|
||||
/ "xgrids-k1"
|
||||
/ "macos"
|
||||
/ "associate_wifi.swift",
|
||||
request.ssid,
|
||||
password,
|
||||
timeout_seconds=45.0,
|
||||
)
|
||||
assert quick_connect_profile_id is not None
|
||||
operation_stage = "device-ap-activation"
|
||||
async with device_ap_activation_session(
|
||||
request.device_id,
|
||||
timeout_seconds=15.0,
|
||||
write_mode="auto",
|
||||
) as activation:
|
||||
write_json_atomic(
|
||||
session_dir / "ap-activation.redacted.json",
|
||||
activation,
|
||||
)
|
||||
if not activation["ready_observed"]:
|
||||
raise RuntimeError(
|
||||
"K1 не подтвердил готовность точки доступа; "
|
||||
"системное подключение Wi-Fi не запускалось"
|
||||
)
|
||||
operation_stage = "host-wifi-association"
|
||||
self._operations.transition(
|
||||
operation.operation_id,
|
||||
"running",
|
||||
stage_code=operation_stage,
|
||||
message_code="network.provision.host_wifi_association",
|
||||
)
|
||||
# Keep the same BLE connection alive through the native
|
||||
# CoreWLAN discovery/association handoff, as the reviewed
|
||||
# LixelGO Quick Connect flow does.
|
||||
try:
|
||||
association = await asyncio.to_thread(
|
||||
associate_with_wifi_profile_once,
|
||||
host_wifi_helper_path,
|
||||
quick_connect_profile_id,
|
||||
selected_device_name,
|
||||
scan_timeout_seconds=15.0,
|
||||
timeout_seconds=180.0,
|
||||
)
|
||||
except HostWifiProfileError as exc:
|
||||
write_json_atomic(
|
||||
session_dir / "host-wifi-association.redacted.json",
|
||||
{
|
||||
"schema_version": 1,
|
||||
"completed_at_utc": _utc_now_iso(),
|
||||
"adapter": "CoreWLAN",
|
||||
"outcome": "failed",
|
||||
"reason_code": exc.reason_code,
|
||||
"scan_attempt_count": exc.scan_attempt_count,
|
||||
"scan_elapsed_ms": exc.scan_elapsed_ms,
|
||||
},
|
||||
)
|
||||
raise
|
||||
completed_at = _utc_now_iso()
|
||||
ipv4: str | None = AP_FALLBACK_IPV4
|
||||
connection_manifest: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"started_at_utc": started_at,
|
||||
"started_at_utc": activation["started_at_utc"],
|
||||
"completed_at_utc": completed_at,
|
||||
"operation": "single_corewlan_k1_ap_association",
|
||||
"operation": "single_k1_ap_activation_and_host_profile_association",
|
||||
"connection_mode": request.connection_mode,
|
||||
"topology": request.compatibility_attestation.topology,
|
||||
"outcome": association["outcome"],
|
||||
"credentials_persisted_by_connector": False,
|
||||
"device_ap_activation_profile_id": activation["profile_id"],
|
||||
"device_ap_activation_outcome": activation["outcome"],
|
||||
"device_ap_ready_observed": activation["ready_observed"],
|
||||
"device_ap_activation_write_performed": activation["write_performed"],
|
||||
"device_ap_activation_write_mode": activation["write_mode"],
|
||||
"host_wifi_adapter": association["adapter"],
|
||||
"host_wifi_profile_id": quick_connect_profile_id,
|
||||
"host_wifi_profile_ready_before_device_write": profile_preflight[
|
||||
"available"
|
||||
],
|
||||
"host_wifi_profile_preflight_adapter": profile_preflight["adapter"],
|
||||
"host_wifi_profile_materialized_before_device_write": profile_preflight[
|
||||
"profile_enrolled"
|
||||
],
|
||||
"credential_provider_id": K1_FW302_CREDENTIAL_PROVIDER_ID,
|
||||
"credential_provider_source": profile_preflight[
|
||||
"credential_source"
|
||||
],
|
||||
"host_wifi_profile_enrolled_now": association["profile_enrolled"],
|
||||
"host_wifi_scan_attempt_count": association["scan_attempt_count"],
|
||||
"host_wifi_scan_elapsed_ms": association["scan_elapsed_ms"],
|
||||
"host_wifi_credential_source": association["credential_source"],
|
||||
"device_ap_ssid_source": "selected-ble-advertised-name",
|
||||
"credentials_resolved_by_plugin": (
|
||||
profile_preflight["credential_source"]
|
||||
== "exact-firmware-profile"
|
||||
),
|
||||
"credentials_persisted_by_host_adapter": True,
|
||||
}
|
||||
else:
|
||||
if request.ssid is None:
|
||||
raise RuntimeError("SSID отсутствует после проверки запроса")
|
||||
result = await provision_wifi_once(
|
||||
request.device_id,
|
||||
request.ssid,
|
||||
@@ -825,6 +1007,9 @@ class XgridsK1CompatibilityService:
|
||||
self._compatibility_attestation = _attestation_snapshot(
|
||||
request.compatibility_attestation
|
||||
)
|
||||
self._device_calibration = unavailable_device_calibration_snapshot(
|
||||
XGRIDS_K1_COMPATIBILITY_PROFILE_ID
|
||||
)
|
||||
self._connection_verification = {
|
||||
"status": "not-probed",
|
||||
"endpoint_validation": "not-performed",
|
||||
@@ -863,7 +1048,7 @@ class XgridsK1CompatibilityService:
|
||||
self._operations.transition_if_pending(
|
||||
operation.operation_id,
|
||||
"failed",
|
||||
stage_code="failed",
|
||||
stage_code=f"{operation_stage}-failed",
|
||||
message_code="network.provision.failed",
|
||||
error=_operation_error(
|
||||
exc,
|
||||
@@ -896,26 +1081,143 @@ class XgridsK1CompatibilityService:
|
||||
message: StreamMessage,
|
||||
metrics: BridgeMetrics,
|
||||
) -> bool:
|
||||
self._observe_live_perception_mqtt(message)
|
||||
if self._modeling_control_safety.observe(message, metrics):
|
||||
return True
|
||||
return observe_modeling_report(message, metrics)
|
||||
|
||||
def _observe_live_perception_mqtt(self, message: StreamMessage) -> None:
|
||||
if message.source != "live_mqtt":
|
||||
return
|
||||
if message.topic.endswith("/lio_pcl") or message.topic == "RealtimePointcloud":
|
||||
modality: Literal["lidar", "pose"] = "lidar"
|
||||
elif message.topic.endswith("/lio_pose") or message.topic == "RealtimePath":
|
||||
modality = "pose"
|
||||
else:
|
||||
return
|
||||
self.live_perception_ingress.publish(
|
||||
modality=modality,
|
||||
source_id=message.topic,
|
||||
source_sequence=message.sequence,
|
||||
captured_at_epoch_ns=message.received_at_epoch_ns,
|
||||
received_monotonic_ns=(
|
||||
message.received_monotonic_ns
|
||||
if message.received_monotonic_ns is not None
|
||||
else time.monotonic_ns()
|
||||
),
|
||||
payload=message.payload,
|
||||
)
|
||||
|
||||
def _observe_committed_camera_segment(
|
||||
self,
|
||||
segment: CommittedCameraSegment,
|
||||
) -> None:
|
||||
self.live_perception_ingress.publish(
|
||||
modality="camera-init" if segment.kind == "init" else "camera-frame",
|
||||
source_id=segment.source_id,
|
||||
source_sequence=segment.sequence,
|
||||
captured_at_epoch_ns=segment.host_epoch_ns,
|
||||
received_monotonic_ns=segment.host_monotonic_ns,
|
||||
payload=segment.payload,
|
||||
)
|
||||
|
||||
def verify_connection(self) -> dict[str, Any]:
|
||||
"""Validate the recorded endpoint only; no network packet is emitted."""
|
||||
"""Refresh the session-scoped DHCP address from the read-only BLE status."""
|
||||
|
||||
self._refresh_live_lan_address()
|
||||
return self.state()
|
||||
|
||||
def _refresh_live_lan_address(self) -> str:
|
||||
with self._lock:
|
||||
selected_device_id = self._selected_device_id
|
||||
connection_mode = self._connection_mode
|
||||
acquisition = self._acquisition
|
||||
current_target = self._k1_ip
|
||||
if selected_device_id is None or connection_mode is None:
|
||||
raise ValueError("сначала выберите и подключите K1 через BLE/Wi-Fi")
|
||||
if acquisition is not None and acquisition.state not in TERMINAL_ACQUISITION_STATES:
|
||||
raise RuntimeError(
|
||||
"нельзя менять DHCP-привязку во время активной acquisition-сессии"
|
||||
)
|
||||
control_state = str(self._application_control_session.snapshot()["state"])
|
||||
if control_state not in {"idle", "completed", "closed", "failed"}:
|
||||
raise RuntimeError("нельзя менять DHCP-привязку при открытой control-сессии")
|
||||
if connection_mode == "quick-connect":
|
||||
if current_target != AP_FALLBACK_IPV4:
|
||||
raise RuntimeError("Quick Connect потерял фиксированный адрес точки доступа K1")
|
||||
return AP_FALLBACK_IPV4
|
||||
|
||||
status_read = asyncio.run(
|
||||
read_wifi_status_once(selected_device_id, timeout_seconds=20.0)
|
||||
)
|
||||
observed_target = status_read["status"]["ipv4"]
|
||||
if observed_target is None or observed_target == AP_FALLBACK_IPV4:
|
||||
raise RuntimeError("K1 не сообщил актуальный DHCP-адрес через BLE status")
|
||||
target = validate_private_ipv4(observed_target)
|
||||
if _target_is_local_ipv4(target):
|
||||
raise RuntimeError("BLE status сообщил адрес, принадлежащий этому компьютеру")
|
||||
|
||||
with self._lock:
|
||||
target = self._k1_ip
|
||||
if target is None:
|
||||
raise ValueError("у плагина нет подтверждённого локального адреса устройства")
|
||||
validate_private_ipv4(target)
|
||||
with self._lock:
|
||||
if (
|
||||
self._selected_device_id != selected_device_id
|
||||
or self._connection_mode != connection_mode
|
||||
):
|
||||
raise RuntimeError("выбранное подключение K1 изменилось во время DHCP refresh")
|
||||
address_changed = self._k1_ip != target
|
||||
self._k1_ip = target
|
||||
if address_changed:
|
||||
self._device_session_id = new_device_session_id()
|
||||
self._device_session_opened_at = _utc_now_iso()
|
||||
self._device_calibration = unavailable_device_calibration_snapshot(
|
||||
XGRIDS_K1_COMPATIBILITY_PROFILE_ID
|
||||
)
|
||||
self._connection_verification = {
|
||||
"status": "endpoint-valid",
|
||||
"endpoint_validation": "private-ip-syntax-only",
|
||||
"network_reachability": "unknown",
|
||||
"observed_at": _utc_now_iso(),
|
||||
"status": "live-address-observed",
|
||||
"endpoint_validation": "ble-wifi-status-read",
|
||||
"network_reachability": "not-probed",
|
||||
"address_changed": address_changed,
|
||||
"previous_address_present": current_target is not None,
|
||||
"write_performed": False,
|
||||
"observed_at": status_read["observed_at_utc"],
|
||||
}
|
||||
return self.state()
|
||||
return target
|
||||
|
||||
def read_device_calibration_snapshot(self) -> dict[str, Any]:
|
||||
"""Read and seal the two reviewed factory YAML files without device mutation."""
|
||||
|
||||
if not self._calibration_gate.acquire(blocking=False):
|
||||
raise RuntimeError("чтение заводской калибровки уже выполняется")
|
||||
try:
|
||||
self._refresh_live_lan_address()
|
||||
with self._lock:
|
||||
target = self._k1_ip
|
||||
connection_mode = self._connection_mode
|
||||
attestation = self._compatibility_attestation
|
||||
if target is None or connection_mode is None:
|
||||
raise ValueError("у плагина нет подтверждённого локального адреса K1")
|
||||
if attestation is None:
|
||||
raise ValueError(
|
||||
"для чтения калибровки сначала должен быть выбран точный профиль K1 FW 3.0.2"
|
||||
)
|
||||
snapshot = self._calibration_snapshot_reader.capture(
|
||||
host=target,
|
||||
evidence_root=self.evidence_root,
|
||||
allow_device_ap=connection_mode == "quick-connect",
|
||||
)
|
||||
with self._lock:
|
||||
if (
|
||||
self._k1_ip != target
|
||||
or self._connection_mode != connection_mode
|
||||
or self._compatibility_attestation != attestation
|
||||
):
|
||||
raise RuntimeError(
|
||||
"подключение K1 изменилось во время чтения; "
|
||||
"снимок сохранён, но не активирован"
|
||||
)
|
||||
self._device_calibration = dict(snapshot)
|
||||
return dict(snapshot)
|
||||
finally:
|
||||
self._calibration_gate.release()
|
||||
|
||||
@_serialized_acquisition_access
|
||||
def open_application_control_session(
|
||||
@@ -927,6 +1229,8 @@ class XgridsK1CompatibilityService:
|
||||
with self._lock:
|
||||
if self._provisioning_active:
|
||||
raise RuntimeError("нельзя открывать control-сессию во время настройки Wi-Fi")
|
||||
self._refresh_live_lan_address()
|
||||
with self._lock:
|
||||
target = self._k1_ip
|
||||
attestation = self._compatibility_attestation
|
||||
acquisition = self._acquisition
|
||||
@@ -1018,6 +1322,8 @@ class XgridsK1CompatibilityService:
|
||||
"plugin-commanded" if control_state == "workspace-ready" else "operator-manual"
|
||||
)
|
||||
requested_streams = _validated_requested_streams(request)
|
||||
if request.host is None and control_mode == "operator-manual":
|
||||
self._refresh_live_lan_address()
|
||||
target = request.host or self.state()["k1_ip"]
|
||||
if not isinstance(target, str) or not target:
|
||||
raise ValueError(
|
||||
@@ -1235,6 +1541,7 @@ class XgridsK1CompatibilityService:
|
||||
lease.release()
|
||||
raise RuntimeError("evidence-сессия уже удерживается активным acquisition")
|
||||
self._acquisition_session_lease = lease
|
||||
self.live_perception_ingress.begin_session(out_dir.name)
|
||||
self._modeling_control_safety.reset()
|
||||
self.runtime.start_live(
|
||||
acquisition.target_host,
|
||||
@@ -1621,7 +1928,7 @@ class XgridsK1CompatibilityService:
|
||||
self,
|
||||
project_name: str,
|
||||
host: str | None,
|
||||
duration_seconds: float,
|
||||
duration_seconds: float | None,
|
||||
compatibility_attestation: CompatibilityAttestationRequest,
|
||||
) -> dict[str, Any]:
|
||||
"""Deprecated compatibility shim over prepare + operator-manual start."""
|
||||
@@ -1770,6 +2077,11 @@ class XgridsK1CompatibilityService:
|
||||
)
|
||||
self._terminalize_acquisition_operations_on_shutdown(terminal_error)
|
||||
finally:
|
||||
with self._lock:
|
||||
out_dir = self._acquisition_out_dir
|
||||
if out_dir is not None:
|
||||
self.live_perception_ingress.end_session(out_dir.name)
|
||||
self.live_perception_ingress.close()
|
||||
if terminal_error is None:
|
||||
self._release_acquisition_session_lease()
|
||||
if terminal_error is not None:
|
||||
@@ -1786,6 +2098,9 @@ class XgridsK1CompatibilityService:
|
||||
show_points=request.show_points,
|
||||
show_trajectory=request.show_trajectory,
|
||||
show_grid=request.show_grid,
|
||||
show_detections_2d=request.show_detections_2d,
|
||||
show_segmentation=request.show_segmentation,
|
||||
show_cuboids_3d=request.show_cuboids_3d,
|
||||
)
|
||||
)
|
||||
return self.state()
|
||||
@@ -1826,6 +2141,8 @@ class XgridsK1CompatibilityService:
|
||||
camera_status: Literal["complete", "interrupted", "failed"],
|
||||
camera_failure_code: str | None,
|
||||
) -> None:
|
||||
with self._lock:
|
||||
out_dir = self._acquisition_out_dir
|
||||
camera_error: Exception | None = None
|
||||
runtime_error: Exception | None = None
|
||||
try:
|
||||
@@ -1853,6 +2170,8 @@ class XgridsK1CompatibilityService:
|
||||
self._seal_acquisition_capture_clock()
|
||||
cleanup_complete = True
|
||||
finally:
|
||||
if out_dir is not None:
|
||||
self.live_perception_ingress.end_session(out_dir.name)
|
||||
if cleanup_complete:
|
||||
self._release_acquisition_session_lease()
|
||||
|
||||
@@ -2333,6 +2652,8 @@ class XgridsK1ServicePort(Protocol):
|
||||
|
||||
def verify_connection(self) -> dict[str, Any]: ...
|
||||
|
||||
def read_device_calibration_snapshot(self) -> dict[str, Any]: ...
|
||||
|
||||
def open_application_control_session(
|
||||
self,
|
||||
request: OpenApplicationControlSessionRequest,
|
||||
@@ -2364,7 +2685,7 @@ class XgridsK1ServicePort(Protocol):
|
||||
self,
|
||||
project_name: str,
|
||||
host: str | None,
|
||||
duration_seconds: float,
|
||||
duration_seconds: float | None,
|
||||
compatibility_attestation: CompatibilityAttestationRequest,
|
||||
) -> dict[str, Any]: ...
|
||||
|
||||
@@ -2442,13 +2763,17 @@ class XgridsK1PluginFacade:
|
||||
if action_id in {
|
||||
ACTION_DEVICE_INSPECT,
|
||||
ACTION_SENSOR_CATALOG_READ,
|
||||
ACTION_DEVICE_CALIBRATION_SNAPSHOT_READ,
|
||||
ACTION_ACQUISITION_STATE_READ,
|
||||
}:
|
||||
EmptyRequest.model_validate(payload)
|
||||
if action_id == ACTION_DEVICE_INSPECT:
|
||||
return await asyncio.to_thread(self.service.inspect_device)
|
||||
return await asyncio.to_thread(self.service.state)
|
||||
if action_id == ACTION_DEVICE_CALIBRATION_SNAPSHOT_READ:
|
||||
EmptyRequest.model_validate(payload)
|
||||
return await asyncio.to_thread(
|
||||
self.service.read_device_calibration_snapshot
|
||||
)
|
||||
if action_id == ACTION_NETWORK_PROVISION:
|
||||
connect_request = ConnectRequest.model_validate(payload)
|
||||
return await self.service.connect(connect_request)
|
||||
@@ -2567,9 +2892,10 @@ def _operation_error(
|
||||
side_effect_status: Literal["none", "possible", "confirmed", "unknown"],
|
||||
safe_to_retry: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
reason_code = getattr(exc, "reason_code", None)
|
||||
return {
|
||||
"category": category,
|
||||
"code": type(exc).__name__,
|
||||
"code": reason_code if isinstance(reason_code, str) and reason_code else type(exc).__name__,
|
||||
"retryable": False,
|
||||
"safe_to_retry": safe_to_retry,
|
||||
"side_effect_status": side_effect_status,
|
||||
@@ -2716,17 +3042,6 @@ def _sensor_catalog(
|
||||
}
|
||||
|
||||
|
||||
def _device_calibration_snapshot(active_profile_id: str | None) -> dict[str, Any]:
|
||||
return {
|
||||
"schema_version": "missioncore.device-calibration/v1alpha2",
|
||||
"status": "unavailable",
|
||||
"compatibility_profile_id": active_profile_id,
|
||||
"device_internal_calibration": None,
|
||||
"vehicle_extrinsics": "host-domain-not-owned-by-plugin",
|
||||
"reason_code": "device-calibration-not-observed",
|
||||
}
|
||||
|
||||
|
||||
def _new_operation_session_dir(sessions_root: Path, suffix: str) -> Path:
|
||||
stamp = datetime.now(UTC).strftime("%Y%m%dT%H%M%SZ")
|
||||
base = sessions_root / f"{stamp}_{suffix}"
|
||||
@@ -2827,6 +3142,12 @@ def build_xgrids_k1_plugin(repository_root: Path) -> DevicePluginRuntimeContribu
|
||||
service.camera_preview,
|
||||
XGRIDS_K1_PLUGIN_ID,
|
||||
),
|
||||
build_live_perception_shadow_router(
|
||||
service.live_perception_ingress,
|
||||
XGRIDS_K1_PLUGIN_ID,
|
||||
bearer_token=service._live_perception_token,
|
||||
result_receiver=service.runtime.publish_perception_result,
|
||||
),
|
||||
),
|
||||
observation=build_xgrids_k1_observation(repository_root),
|
||||
)
|
||||
|
||||
@@ -0,0 +1,197 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import hmac
|
||||
import tarfile
|
||||
from collections.abc import Iterator
|
||||
from contextlib import contextmanager
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import IO
|
||||
|
||||
from k1link.host_network.wifi import (
|
||||
HostWifiCredentialMaterialStoreResult,
|
||||
store_wifi_credential_material,
|
||||
)
|
||||
|
||||
K1_FW302_CREDENTIAL_PROVIDER_ID = (
|
||||
"xgrids.lixelkity-k1.quick-connect.fw-3.0.2.official-firmware.v1"
|
||||
)
|
||||
K1_FW302_OFFICIAL_ARCHIVE_SHA256 = (
|
||||
"e5830feae54d586cdeda2824495d08598920dc9cf4541059d01f0efeb858a750"
|
||||
)
|
||||
K1_FW302_OUTER_MEMBER = "upgrade.tar.gz"
|
||||
K1_FW302_RK_IMAGE_MEMBER = (
|
||||
"upgrade/rk/rk_normal/"
|
||||
"OTA-BOOT-ROOTFS-APP-V3.0.2-20250624.153447.img"
|
||||
)
|
||||
K1_FW302_APPS_OFFSET = 4_338_829_862
|
||||
K1_FW302_APPS_SIZE = 230_801_408
|
||||
|
||||
_AP_PSK_DECLARATION = b"nmcli conn modify WIFI_AP 802-11-wireless-security.psk "
|
||||
_READ_CHUNK_SIZE = 4 * 1024 * 1024
|
||||
_SCAN_OVERLAP = len(_AP_PSK_DECLARATION) + 128
|
||||
|
||||
|
||||
class FirmwareCredentialError(RuntimeError):
|
||||
"""An exact firmware artifact cannot provide the reviewed credential."""
|
||||
|
||||
|
||||
class SecretBuffer:
|
||||
"""Short-lived credential bytes whose repr and str are always redacted."""
|
||||
|
||||
__slots__ = ("_value",)
|
||||
|
||||
def __init__(self, value: bytearray) -> None:
|
||||
self._value = value
|
||||
|
||||
def __repr__(self) -> str:
|
||||
return "SecretBuffer(<redacted>)"
|
||||
|
||||
__str__ = __repr__
|
||||
|
||||
def reveal_ascii(self) -> str:
|
||||
return self._value.decode("ascii")
|
||||
|
||||
def zeroize(self) -> None:
|
||||
self._value[:] = b"\x00" * len(self._value)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FirmwareCredentialImportResult:
|
||||
provider_id: str
|
||||
firmware_sha256: str
|
||||
host_adapter: str
|
||||
outcome: str
|
||||
|
||||
|
||||
def _sha256(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
while chunk := stream.read(_READ_CHUNK_SIZE):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _discard_exact(stream: IO[bytes], size: int) -> None:
|
||||
remaining = size
|
||||
while remaining:
|
||||
chunk = stream.read(min(_READ_CHUNK_SIZE, remaining))
|
||||
if not chunk:
|
||||
raise FirmwareCredentialError("firmware image ended before the apps partition")
|
||||
remaining -= len(chunk)
|
||||
|
||||
|
||||
def _bounded_chunks(stream: IO[bytes], size: int) -> Iterator[bytes]:
|
||||
remaining = size
|
||||
while remaining:
|
||||
chunk = stream.read(min(_READ_CHUNK_SIZE, remaining))
|
||||
if not chunk:
|
||||
raise FirmwareCredentialError("firmware apps partition is truncated")
|
||||
remaining -= len(chunk)
|
||||
yield chunk
|
||||
|
||||
|
||||
def _credential_from_chunks(chunks: Iterator[bytes]) -> SecretBuffer:
|
||||
overlap = b""
|
||||
processed = 0
|
||||
matches: dict[int, bytearray] = {}
|
||||
for chunk in chunks:
|
||||
combined = overlap + chunk
|
||||
combined_offset = processed - len(overlap)
|
||||
start = 0
|
||||
while True:
|
||||
index = combined.find(_AP_PSK_DECLARATION, start)
|
||||
if index < 0:
|
||||
break
|
||||
value_start = index + len(_AP_PSK_DECLARATION)
|
||||
value_end = value_start
|
||||
while value_end < len(combined) and combined[value_end] not in b"\x00\r\n":
|
||||
value_end += 1
|
||||
if value_end < len(combined):
|
||||
absolute_offset = combined_offset + index
|
||||
matches[absolute_offset] = bytearray(combined[value_start:value_end].strip())
|
||||
start = index + 1
|
||||
processed += len(chunk)
|
||||
overlap = combined[-_SCAN_OVERLAP:]
|
||||
|
||||
if len(matches) != 1:
|
||||
for value in matches.values():
|
||||
value[:] = b"\x00" * len(value)
|
||||
raise FirmwareCredentialError(
|
||||
"reviewed AP credential declaration was not unique in the apps partition"
|
||||
)
|
||||
value = next(iter(matches.values()))
|
||||
if not 8 <= len(value) <= 63 or any(byte <= 0x20 or byte >= 0x7F for byte in value):
|
||||
value[:] = b"\x00" * len(value)
|
||||
raise FirmwareCredentialError("reviewed AP credential has an invalid WPA-PSK shape")
|
||||
return SecretBuffer(value)
|
||||
|
||||
|
||||
@contextmanager
|
||||
def _official_rk_image(archive_path: Path) -> Iterator[IO[bytes]]:
|
||||
try:
|
||||
with tarfile.open(archive_path, mode="r:*") as outer:
|
||||
outer_member = outer.getmember(K1_FW302_OUTER_MEMBER)
|
||||
upgrade_stream = outer.extractfile(outer_member)
|
||||
if upgrade_stream is None:
|
||||
raise FirmwareCredentialError("official upgrade member has no readable payload")
|
||||
with upgrade_stream, tarfile.open(
|
||||
fileobj=upgrade_stream, mode="r|gz"
|
||||
) as upgrade:
|
||||
for member in upgrade:
|
||||
if member.name != K1_FW302_RK_IMAGE_MEMBER:
|
||||
continue
|
||||
image_stream = upgrade.extractfile(member)
|
||||
if image_stream is None:
|
||||
raise FirmwareCredentialError(
|
||||
"official Rockchip image has no readable payload"
|
||||
)
|
||||
with image_stream:
|
||||
yield image_stream
|
||||
return
|
||||
except (KeyError, OSError, tarfile.TarError) as exc:
|
||||
raise FirmwareCredentialError("official firmware archive is unreadable") from exc
|
||||
raise FirmwareCredentialError("reviewed Rockchip image is absent from the archive")
|
||||
|
||||
|
||||
def extract_k1_fw302_ap_credential(archive_path: Path) -> tuple[SecretBuffer, str]:
|
||||
"""Resolve the FW 3.0.2 AP material without printing or persisting it.
|
||||
|
||||
The exact official archive is authenticated first. Only then is the reviewed
|
||||
apps-partition range scanned for one bounded NetworkManager declaration.
|
||||
"""
|
||||
|
||||
resolved_path = archive_path.resolve(strict=True)
|
||||
firmware_sha256 = _sha256(resolved_path)
|
||||
if not hmac.compare_digest(firmware_sha256, K1_FW302_OFFICIAL_ARCHIVE_SHA256):
|
||||
raise FirmwareCredentialError("firmware SHA-256 does not match the reviewed 3.0.2 image")
|
||||
with _official_rk_image(resolved_path) as image_stream:
|
||||
_discard_exact(image_stream, K1_FW302_APPS_OFFSET)
|
||||
secret = _credential_from_chunks(
|
||||
_bounded_chunks(image_stream, K1_FW302_APPS_SIZE)
|
||||
)
|
||||
return secret, firmware_sha256
|
||||
|
||||
|
||||
def import_k1_fw302_ap_credential(
|
||||
archive_path: Path,
|
||||
helper_path: Path,
|
||||
) -> FirmwareCredentialImportResult:
|
||||
"""Install one firmware-scoped material in the host's secure store."""
|
||||
|
||||
secret, firmware_sha256 = extract_k1_fw302_ap_credential(archive_path)
|
||||
try:
|
||||
stored: HostWifiCredentialMaterialStoreResult = store_wifi_credential_material(
|
||||
helper_path,
|
||||
K1_FW302_CREDENTIAL_PROVIDER_ID,
|
||||
secret.reveal_ascii(),
|
||||
)
|
||||
finally:
|
||||
secret.zeroize()
|
||||
return FirmwareCredentialImportResult(
|
||||
provider_id=K1_FW302_CREDENTIAL_PROVIDER_ID,
|
||||
firmware_sha256=firmware_sha256,
|
||||
host_adapter=stored["adapter"],
|
||||
outcome=stored["outcome"],
|
||||
)
|
||||
@@ -0,0 +1,144 @@
|
||||
"""Authenticated localhost transport for the K1 shadow perception worker."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import hmac
|
||||
import os
|
||||
import secrets
|
||||
from collections.abc import Callable
|
||||
from contextlib import suppress
|
||||
from pathlib import Path
|
||||
from uuid import uuid4
|
||||
|
||||
from fastapi import APIRouter, WebSocket, WebSocketDisconnect
|
||||
|
||||
from k1link.compute.live_perception import LivePerceptionIngress
|
||||
|
||||
TOKEN_BYTES = 32
|
||||
TOKEN_FILE_NAME = "shadow-worker.token"
|
||||
|
||||
|
||||
def ensure_live_shadow_token(repository_root: Path) -> tuple[Path, str]:
|
||||
"""Load or create the private bearer used only through the SSH tunnel."""
|
||||
|
||||
token_root = repository_root.resolve() / ".runtime" / "live-perception"
|
||||
token_root.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
with suppress(OSError):
|
||||
token_root.chmod(0o700)
|
||||
token_path = token_root / TOKEN_FILE_NAME
|
||||
try:
|
||||
token = token_path.read_text(encoding="ascii").strip()
|
||||
except FileNotFoundError:
|
||||
token = secrets.token_urlsafe(TOKEN_BYTES)
|
||||
descriptor = os.open(
|
||||
token_path,
|
||||
os.O_WRONLY | os.O_CREAT | os.O_EXCL,
|
||||
0o600,
|
||||
)
|
||||
try:
|
||||
os.write(descriptor, f"{token}\n".encode("ascii"))
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
if len(token) < 40 or len(token) > 128 or not token.isascii():
|
||||
raise RuntimeError("live perception shadow token is invalid")
|
||||
with suppress(OSError):
|
||||
token_path.chmod(0o600)
|
||||
return token_path, token
|
||||
|
||||
|
||||
def build_live_perception_shadow_router(
|
||||
ingress: LivePerceptionIngress,
|
||||
plugin_id: str,
|
||||
*,
|
||||
bearer_token: str,
|
||||
result_receiver: Callable[[bytes], bool] | None = None,
|
||||
) -> APIRouter:
|
||||
"""Expose one exclusive sensor stream with bounded diagnostic results back."""
|
||||
|
||||
router = APIRouter(include_in_schema=False)
|
||||
|
||||
@router.websocket(
|
||||
f"/api/v1/device-plugins/{plugin_id}/live-perception-shadow"
|
||||
)
|
||||
async def live_perception_shadow(websocket: WebSocket) -> None:
|
||||
authorization = websocket.headers.get("authorization", "")
|
||||
supplied = authorization.removeprefix("Bearer ")
|
||||
if not supplied or not hmac.compare_digest(supplied, bearer_token):
|
||||
await websocket.close(code=1008, reason="Shadow worker authentication failed")
|
||||
return
|
||||
|
||||
consumer_id = f"shadow-worker-{uuid4().hex}"
|
||||
try:
|
||||
ingress.open_consumer(consumer_id)
|
||||
except RuntimeError:
|
||||
await websocket.close(code=1008, reason="Shadow worker lease is unavailable")
|
||||
return
|
||||
|
||||
await websocket.accept()
|
||||
client_event = asyncio.create_task(websocket.receive())
|
||||
try:
|
||||
while True:
|
||||
ingress_event = asyncio.create_task(
|
||||
asyncio.to_thread(
|
||||
ingress.take_next,
|
||||
consumer_id,
|
||||
timeout=0.5,
|
||||
)
|
||||
)
|
||||
completed, _ = await asyncio.wait(
|
||||
{client_event, ingress_event},
|
||||
return_when=asyncio.FIRST_COMPLETED,
|
||||
)
|
||||
if client_event in completed:
|
||||
message = client_event.result()
|
||||
if message.get("type") == "websocket.disconnect":
|
||||
ingress_event.cancel()
|
||||
with suppress(asyncio.CancelledError):
|
||||
await ingress_event
|
||||
break
|
||||
result = message.get("bytes")
|
||||
if not isinstance(result, bytes) or result_receiver is None:
|
||||
ingress_event.cancel()
|
||||
with suppress(asyncio.CancelledError):
|
||||
await ingress_event
|
||||
await websocket.close(
|
||||
code=1008,
|
||||
reason="Shadow result direction is unavailable",
|
||||
)
|
||||
return
|
||||
try:
|
||||
await asyncio.to_thread(result_receiver, result)
|
||||
except (RuntimeError, ValueError):
|
||||
ingress_event.cancel()
|
||||
with suppress(asyncio.CancelledError):
|
||||
await ingress_event
|
||||
await websocket.close(
|
||||
code=1008,
|
||||
reason="Shadow result contract is invalid",
|
||||
)
|
||||
return
|
||||
client_event = asyncio.create_task(websocket.receive())
|
||||
if ingress_event not in completed:
|
||||
ingress_event.cancel()
|
||||
with suppress(asyncio.CancelledError):
|
||||
await ingress_event
|
||||
continue
|
||||
event = await ingress_event
|
||||
if event is None:
|
||||
if ingress.snapshot()["closed"]:
|
||||
break
|
||||
continue
|
||||
await websocket.send_bytes(event.wire_bytes())
|
||||
except (WebSocketDisconnect, RuntimeError):
|
||||
return
|
||||
finally:
|
||||
client_event.cancel()
|
||||
with suppress(asyncio.CancelledError):
|
||||
await client_event
|
||||
ingress.close_consumer(consumer_id)
|
||||
with suppress(RuntimeError):
|
||||
await websocket.close()
|
||||
|
||||
return router
|
||||
@@ -1,99 +0,0 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
from collections.abc import Callable
|
||||
from pathlib import Path
|
||||
from typing import Any, TypedDict
|
||||
|
||||
|
||||
class HostWifiAssociationResult(TypedDict):
|
||||
schema_version: int
|
||||
adapter: str
|
||||
outcome: str
|
||||
already_associated: bool
|
||||
|
||||
|
||||
class HostWifiAssociationError(RuntimeError):
|
||||
"""One bounded host-side Wi-Fi association attempt failed."""
|
||||
|
||||
def __init__(self, reason_code: str) -> None:
|
||||
self.reason_code = reason_code
|
||||
super().__init__(f"macOS Wi-Fi association failed: {reason_code}")
|
||||
|
||||
|
||||
RunProcess = Callable[..., subprocess.CompletedProcess[bytes]]
|
||||
|
||||
|
||||
def associate_with_wifi_once(
|
||||
helper_path: Path,
|
||||
ssid: str,
|
||||
password: str,
|
||||
*,
|
||||
timeout_seconds: float = 45.0,
|
||||
runner: RunProcess = subprocess.run,
|
||||
) -> HostWifiAssociationResult:
|
||||
"""Associate the Mac with one operator-selected Wi-Fi network exactly once.
|
||||
|
||||
The credential is sent to the short-lived CoreWLAN helper through stdin. It
|
||||
never appears in argv, the environment, stdout, stderr, or a persisted
|
||||
artifact. The helper performs at most one scan and one association call.
|
||||
"""
|
||||
|
||||
if sys.platform != "darwin":
|
||||
raise HostWifiAssociationError("unsupported-platform")
|
||||
if not helper_path.is_file():
|
||||
raise HostWifiAssociationError("corewlan-helper-missing")
|
||||
if not 1 <= len(ssid.encode("utf-8")) <= 32:
|
||||
raise ValueError("SSID must contain between 1 and 32 UTF-8 bytes")
|
||||
if not 1 <= len(password.encode("utf-8")) <= 64:
|
||||
raise ValueError("Wi-Fi password must contain between 1 and 64 UTF-8 bytes")
|
||||
if timeout_seconds <= 0:
|
||||
raise ValueError("timeout_seconds must be positive")
|
||||
|
||||
request_bytes = bytearray(
|
||||
json.dumps(
|
||||
{"ssid": ssid, "password": password},
|
||||
ensure_ascii=False,
|
||||
separators=(",", ":"),
|
||||
).encode("utf-8")
|
||||
)
|
||||
try:
|
||||
completed = runner(
|
||||
["/usr/bin/xcrun", "swift", str(helper_path.resolve())],
|
||||
input=request_bytes,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE,
|
||||
timeout=timeout_seconds,
|
||||
check=False,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired) as exc:
|
||||
raise HostWifiAssociationError("corewlan-helper-unavailable") from exc
|
||||
finally:
|
||||
request_bytes[:] = b"\x00" * len(request_bytes)
|
||||
|
||||
if len(completed.stdout) > 4096:
|
||||
raise HostWifiAssociationError("corewlan-response-too-large")
|
||||
try:
|
||||
response: Any = json.loads(completed.stdout.decode("utf-8"))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError) as exc:
|
||||
raise HostWifiAssociationError("corewlan-response-invalid") from exc
|
||||
if not isinstance(response, dict):
|
||||
raise HostWifiAssociationError("corewlan-response-invalid")
|
||||
|
||||
reason_code = response.get("reason_code")
|
||||
if completed.returncode != 0 or response.get("ok") is not True:
|
||||
if not isinstance(reason_code, str) or not reason_code:
|
||||
reason_code = "corewlan-association-failed"
|
||||
raise HostWifiAssociationError(reason_code)
|
||||
|
||||
already_associated = response.get("already_associated")
|
||||
if not isinstance(already_associated, bool):
|
||||
raise HostWifiAssociationError("corewlan-response-invalid")
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"adapter": "CoreWLAN",
|
||||
"outcome": "already-associated" if already_associated else "associated",
|
||||
"already_associated": already_associated,
|
||||
}
|
||||
@@ -101,7 +101,7 @@ class CaptureSummary(TypedDict):
|
||||
reconnect_enabled: bool
|
||||
publishing_enabled: bool
|
||||
subscriptions: list[str]
|
||||
requested_duration_seconds: float
|
||||
requested_duration_seconds: float | None
|
||||
capture_elapsed_seconds: float
|
||||
session_elapsed_seconds: float
|
||||
operation_elapsed_seconds: float
|
||||
@@ -753,7 +753,7 @@ def capture_mqtt(
|
||||
out_dir: Path,
|
||||
*,
|
||||
port: int = 1883,
|
||||
duration_seconds: float = 60.0,
|
||||
duration_seconds: float | None = 60.0,
|
||||
max_message_bytes: int = DEFAULT_MAX_MESSAGE_BYTES,
|
||||
on_clock_established: Callable[[], None] | None = None,
|
||||
on_ready: Callable[[], None] | None = None,
|
||||
@@ -765,7 +765,9 @@ def capture_mqtt(
|
||||
target_ipv4 = validate_private_ipv4(host)
|
||||
if not 1 <= port <= 65535:
|
||||
raise ValueError("port must be between 1 and 65535")
|
||||
if not math.isfinite(duration_seconds) or duration_seconds <= 0:
|
||||
if duration_seconds is not None and (
|
||||
not math.isfinite(duration_seconds) or duration_seconds <= 0
|
||||
):
|
||||
raise ValueError("duration_seconds must be finite and greater than zero")
|
||||
if not 1 <= max_message_bytes <= MAX_CONFIGURABLE_MESSAGE_BYTES:
|
||||
raise ValueError(
|
||||
@@ -898,7 +900,11 @@ def capture_mqtt(
|
||||
capture_started = now
|
||||
if on_ready is not None:
|
||||
on_ready()
|
||||
if capture_started is not None and now - capture_started >= duration_seconds:
|
||||
if (
|
||||
capture_started is not None
|
||||
and duration_seconds is not None
|
||||
and now - capture_started >= duration_seconds
|
||||
):
|
||||
state.stop_reason = "duration_elapsed"
|
||||
break
|
||||
if capture_started is None and now - operation_started >= CONNECT_TIMEOUT_SECONDS:
|
||||
@@ -970,7 +976,7 @@ def _build_summary(
|
||||
writer: _CaptureWriter,
|
||||
target_ipv4: str,
|
||||
port: int,
|
||||
duration_seconds: float,
|
||||
duration_seconds: float | None,
|
||||
capture_elapsed: float,
|
||||
operation_elapsed: float,
|
||||
max_message_bytes: int,
|
||||
|
||||
@@ -0,0 +1,319 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import hmac
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_bootstrap import (
|
||||
ApplicationControlAuthority,
|
||||
ApplicationRequestHeader,
|
||||
LiveDeviceControlBinding,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_publish import (
|
||||
OneShotPublishEnvelope,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.modeling_control import OPENAPI_SUCCESS
|
||||
from k1link.device_plugins.xgrids_k1.protocol.protobuf_wire import (
|
||||
ProtobufWireError,
|
||||
ProtoField,
|
||||
iter_fields,
|
||||
)
|
||||
|
||||
CALIBRATION_FILE_REQUEST_TOPIC = "lixel/calibration/request/file"
|
||||
CALIBRATION_FILE_RESPONSE_TOPIC = "lixel/calibration/response/file"
|
||||
CALIBRATION_FILE_MESSAGE_TYPE = "CalibFileRequest"
|
||||
CALIBRATION_FILE_READ_COMMAND = 5
|
||||
|
||||
FACTORY_CAMERA_CALIBRATION_PATH = "/mnt/system/factory-data/config/camera.yaml"
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH = (
|
||||
"/mnt/system/factory-data/config/extrinsic_camera_lidar.yaml"
|
||||
)
|
||||
FACTORY_CALIBRATION_PATHS = (
|
||||
FACTORY_CAMERA_CALIBRATION_PATH,
|
||||
FACTORY_CAMERA_LIDAR_EXTRINSIC_PATH,
|
||||
)
|
||||
|
||||
MAX_CALIBRATION_FILE_BYTES = 1024 * 1024
|
||||
MAX_CALIBRATION_RESPONSE_BYTES = MAX_CALIBRATION_FILE_BYTES + 16 * 1024
|
||||
MAX_CALIBRATION_PATH_BYTES = 256
|
||||
MAX_CALIBRATION_HEADER_BYTES = 4 * 1024
|
||||
|
||||
|
||||
class CalibrationFileProtocolError(ValueError):
|
||||
"""A K1 calibration-file message violated the read-only contract."""
|
||||
|
||||
|
||||
class CalibrationFileRejected(CalibrationFileProtocolError):
|
||||
"""The K1 rejected a correlated, exact-path calibration-file read."""
|
||||
|
||||
def __init__(self, result_code: int) -> None:
|
||||
self.result_code = result_code
|
||||
super().__init__(f"K1 calibration-file read rejected with result code {result_code}")
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class EncodedCalibrationFileRead:
|
||||
path: str
|
||||
session_id: str = field(repr=False)
|
||||
vendor_device_id: str = field(repr=False)
|
||||
payload: bytes = field(repr=False)
|
||||
payload_sha256: str
|
||||
payload_bytes: int
|
||||
topic: str = CALIBRATION_FILE_REQUEST_TOPIC
|
||||
response_topic: str = CALIBRATION_FILE_RESPONSE_TOPIC
|
||||
qos: int = 2
|
||||
retain: bool = False
|
||||
mutates_device: bool = False
|
||||
automatic_retry: bool = False
|
||||
|
||||
def envelope(self, *, ordinal: int) -> OneShotPublishEnvelope:
|
||||
if ordinal not in (1, 2):
|
||||
raise CalibrationFileProtocolError(
|
||||
"factory calibration read ordinal must be one or two"
|
||||
)
|
||||
return OneShotPublishEnvelope(
|
||||
operation_key=f"calibration:read:{ordinal}",
|
||||
topic=self.topic,
|
||||
payload=self.payload,
|
||||
payload_sha256=self.payload_sha256,
|
||||
payload_bytes=self.payload_bytes,
|
||||
qos=self.qos,
|
||||
retain=self.retain,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CalibrationFileContent:
|
||||
path: str
|
||||
content: bytes = field(repr=False)
|
||||
content_sha256: str
|
||||
content_bytes: int
|
||||
result_code: int
|
||||
|
||||
|
||||
def build_factory_calibration_file_read(
|
||||
authority: ApplicationControlAuthority,
|
||||
binding: LiveDeviceControlBinding,
|
||||
path: str,
|
||||
) -> EncodedCalibrationFileRead:
|
||||
"""Build command 5 for one of the two reviewed factory YAML paths.
|
||||
|
||||
The command is deliberately not a parameter. This module has no API that
|
||||
can encode command 6 (file write), and an arbitrary path cannot cross this
|
||||
boundary.
|
||||
"""
|
||||
|
||||
_require_reviewed_binding(binding)
|
||||
_require_exact_factory_path(path)
|
||||
header = ApplicationRequestHeader(
|
||||
message_type=CALIBRATION_FILE_MESSAGE_TYPE,
|
||||
authority=authority,
|
||||
binding=binding,
|
||||
)
|
||||
encoded_header = b"".join(
|
||||
(
|
||||
_text_field(4, binding.vendor_device_id),
|
||||
_text_field(5, header.session_id),
|
||||
_text_field(6, authority.openapi_key),
|
||||
)
|
||||
)
|
||||
if len(encoded_header) > MAX_CALIBRATION_HEADER_BYTES:
|
||||
raise CalibrationFileProtocolError("calibration request header exceeds bound")
|
||||
payload = b"".join(
|
||||
(
|
||||
_bytes_field(1, encoded_header),
|
||||
_varint_field(2, CALIBRATION_FILE_READ_COMMAND),
|
||||
_text_field(3, path),
|
||||
)
|
||||
)
|
||||
if len(payload) > MAX_CALIBRATION_RESPONSE_BYTES:
|
||||
raise CalibrationFileProtocolError("calibration request exceeds bound")
|
||||
return EncodedCalibrationFileRead(
|
||||
path=path,
|
||||
session_id=header.session_id,
|
||||
vendor_device_id=binding.vendor_device_id,
|
||||
payload=payload,
|
||||
payload_sha256=hashlib.sha256(payload).hexdigest(),
|
||||
payload_bytes=len(payload),
|
||||
)
|
||||
|
||||
|
||||
def decode_factory_calibration_file_response(
|
||||
payload: bytes,
|
||||
request: EncodedCalibrationFileRead,
|
||||
authority: ApplicationControlAuthority,
|
||||
binding: LiveDeviceControlBinding,
|
||||
) -> CalibrationFileContent:
|
||||
"""Decode and correlate one exact-path command-5 response."""
|
||||
|
||||
_require_reviewed_binding(binding)
|
||||
_require_exact_factory_path(request.path)
|
||||
if request.vendor_device_id != binding.vendor_device_id:
|
||||
raise CalibrationFileProtocolError("calibration request binding changed")
|
||||
if len(payload) > MAX_CALIBRATION_RESPONSE_BYTES:
|
||||
raise CalibrationFileProtocolError("calibration response exceeds bound")
|
||||
|
||||
top = _selected_unique_fields(
|
||||
payload,
|
||||
"calibration response",
|
||||
selected={1, 2, 3, 4, 15},
|
||||
max_fields=32,
|
||||
)
|
||||
header = _selected_unique_fields(
|
||||
_required_bytes(top, 1, "response.header"),
|
||||
"calibration response header",
|
||||
selected={4, 5, 6},
|
||||
max_fields=16,
|
||||
)
|
||||
device_id = _required_ascii(header, 4, "response.header.device_id")
|
||||
session_id = _required_ascii(header, 5, "response.header.session_id")
|
||||
openapi_key = _required_ascii(header, 6, "response.header.openapi_key")
|
||||
if not hmac.compare_digest(device_id, binding.vendor_device_id):
|
||||
raise CalibrationFileProtocolError("calibration response device identity mismatch")
|
||||
if not hmac.compare_digest(session_id, request.session_id):
|
||||
raise CalibrationFileProtocolError("calibration response session mismatch")
|
||||
if not hmac.compare_digest(openapi_key, authority.openapi_key):
|
||||
raise CalibrationFileProtocolError("calibration response authority mismatch")
|
||||
|
||||
command = _required_uint(top, 2, "response.cmd")
|
||||
if command != CALIBRATION_FILE_READ_COMMAND:
|
||||
raise CalibrationFileProtocolError("calibration response is not a file-read result")
|
||||
observed_path = _required_utf8(top, 3, "response.file_path", MAX_CALIBRATION_PATH_BYTES)
|
||||
if not hmac.compare_digest(observed_path, request.path):
|
||||
raise CalibrationFileProtocolError("calibration response path mismatch")
|
||||
|
||||
error = _selected_unique_fields(
|
||||
_required_bytes(top, 15, "response.error"),
|
||||
"calibration response error",
|
||||
selected={1, 2},
|
||||
max_fields=8,
|
||||
)
|
||||
result_code = _required_uint(error, 1, "response.error.code")
|
||||
if result_code != OPENAPI_SUCCESS:
|
||||
raise CalibrationFileRejected(result_code)
|
||||
|
||||
content = _required_bytes(top, 4, "response.file_content")
|
||||
if not content:
|
||||
raise CalibrationFileProtocolError("calibration response file content is empty")
|
||||
if len(content) > MAX_CALIBRATION_FILE_BYTES:
|
||||
raise CalibrationFileProtocolError("calibration response file exceeds bound")
|
||||
try:
|
||||
content.decode("utf-8", errors="strict")
|
||||
except UnicodeDecodeError as exc:
|
||||
raise CalibrationFileProtocolError(
|
||||
"calibration response file is not valid UTF-8"
|
||||
) from exc
|
||||
if b"\x00" in content:
|
||||
raise CalibrationFileProtocolError("calibration response file contains NUL")
|
||||
return CalibrationFileContent(
|
||||
path=observed_path,
|
||||
content=content,
|
||||
content_sha256=hashlib.sha256(content).hexdigest(),
|
||||
content_bytes=len(content),
|
||||
result_code=result_code,
|
||||
)
|
||||
|
||||
|
||||
def _require_reviewed_binding(binding: LiveDeviceControlBinding) -> None:
|
||||
if not binding.ready_for_reviewed_profile:
|
||||
raise CalibrationFileProtocolError(
|
||||
"live DeviceInfo does not match the reviewed activated K1 FW 3.0.2 profile"
|
||||
)
|
||||
|
||||
|
||||
def _require_exact_factory_path(path: object) -> None:
|
||||
if not isinstance(path, str) or path not in FACTORY_CALIBRATION_PATHS:
|
||||
raise CalibrationFileProtocolError(
|
||||
"calibration file path is outside the exact two-file allowlist"
|
||||
)
|
||||
|
||||
|
||||
def _selected_unique_fields(
|
||||
payload: bytes,
|
||||
name: str,
|
||||
*,
|
||||
selected: set[int],
|
||||
max_fields: int,
|
||||
) -> dict[int, ProtoField]:
|
||||
result: dict[int, ProtoField] = {}
|
||||
try:
|
||||
for item in iter_fields(payload, max_fields=max_fields):
|
||||
if item.number not in selected:
|
||||
continue
|
||||
if item.number in result:
|
||||
raise CalibrationFileProtocolError(
|
||||
f"{name} field {item.number} is duplicated"
|
||||
)
|
||||
result[item.number] = item
|
||||
except ProtobufWireError as exc:
|
||||
raise CalibrationFileProtocolError(f"invalid {name}: {exc}") from exc
|
||||
return result
|
||||
|
||||
|
||||
def _required_bytes(fields: dict[int, ProtoField], number: int, name: str) -> bytes:
|
||||
item = fields.get(number)
|
||||
if item is None or item.wire_type != 2 or not isinstance(item.value, bytes):
|
||||
raise CalibrationFileProtocolError(f"{name} is missing or has wrong wire type")
|
||||
return item.value
|
||||
|
||||
|
||||
def _required_uint(fields: dict[int, ProtoField], number: int, name: str) -> int:
|
||||
item = fields.get(number)
|
||||
if item is None or item.wire_type != 0 or not isinstance(item.value, int):
|
||||
raise CalibrationFileProtocolError(f"{name} is missing or has wrong wire type")
|
||||
return item.value
|
||||
|
||||
|
||||
def _required_utf8(
|
||||
fields: dict[int, ProtoField],
|
||||
number: int,
|
||||
name: str,
|
||||
maximum_bytes: int,
|
||||
) -> str:
|
||||
raw = _required_bytes(fields, number, name)
|
||||
if not raw or len(raw) > maximum_bytes:
|
||||
raise CalibrationFileProtocolError(f"{name} is empty or exceeds bound")
|
||||
try:
|
||||
return raw.decode("utf-8", errors="strict")
|
||||
except UnicodeDecodeError as exc:
|
||||
raise CalibrationFileProtocolError(f"{name} is not valid UTF-8") from exc
|
||||
|
||||
|
||||
def _required_ascii(fields: dict[int, ProtoField], number: int, name: str) -> str:
|
||||
value = _required_utf8(fields, number, name, MAX_CALIBRATION_HEADER_BYTES)
|
||||
try:
|
||||
encoded = value.encode("ascii", errors="strict")
|
||||
except UnicodeEncodeError as exc:
|
||||
raise CalibrationFileProtocolError(f"{name} is not printable ASCII") from exc
|
||||
if any(byte <= 0x20 or byte > 0x7E for byte in encoded):
|
||||
raise CalibrationFileProtocolError(f"{name} is not printable ASCII")
|
||||
return value
|
||||
|
||||
|
||||
def _varint(value: int) -> bytes:
|
||||
if value < 0:
|
||||
raise CalibrationFileProtocolError("negative protobuf varint is unsupported")
|
||||
encoded = bytearray()
|
||||
while value > 0x7F:
|
||||
encoded.append((value & 0x7F) | 0x80)
|
||||
value >>= 7
|
||||
encoded.append(value)
|
||||
return bytes(encoded)
|
||||
|
||||
|
||||
def _key(number: int, wire_type: int) -> bytes:
|
||||
if number < 1:
|
||||
raise CalibrationFileProtocolError("protobuf field number must be positive")
|
||||
return _varint((number << 3) | wire_type)
|
||||
|
||||
|
||||
def _varint_field(number: int, value: int) -> bytes:
|
||||
return _key(number, 0) + _varint(value)
|
||||
|
||||
|
||||
def _bytes_field(number: int, value: bytes) -> bytes:
|
||||
return _key(number, 2) + _varint(len(value)) + value
|
||||
|
||||
|
||||
def _text_field(number: int, value: str) -> bytes:
|
||||
return _bytes_field(number, value.encode("utf-8", errors="strict"))
|
||||
@@ -0,0 +1,534 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import math
|
||||
import secrets
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
|
||||
import paho.mqtt.client as mqtt
|
||||
from paho.mqtt.enums import CallbackAPIVersion
|
||||
from paho.mqtt.properties import Properties
|
||||
from paho.mqtt.reasoncodes import ReasonCode
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.ble.wifi_provisioning import AP_FALLBACK_IPV4
|
||||
from k1link.device_plugins.xgrids_k1.mqtt import validate_private_ipv4
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_bootstrap import (
|
||||
DEVICE_INFO_REQUEST_TOPIC,
|
||||
DEVICE_INFO_RESPONSE_TOPIC,
|
||||
ApplicationControlAuthority,
|
||||
ApplicationRequestHeader,
|
||||
EncodedApplicationRequest,
|
||||
LiveDeviceControlBinding,
|
||||
decode_and_bind_device_info_response,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.application_publish import (
|
||||
OneShotPublishEnvelope,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.protocol.calibration_file import (
|
||||
CALIBRATION_FILE_RESPONSE_TOPIC,
|
||||
FACTORY_CALIBRATION_PATHS,
|
||||
MAX_CALIBRATION_RESPONSE_BYTES,
|
||||
CalibrationFileContent,
|
||||
build_factory_calibration_file_read,
|
||||
decode_factory_calibration_file_response,
|
||||
)
|
||||
|
||||
CALIBRATION_CONNECT_TIMEOUT_SECONDS = 10.0
|
||||
CALIBRATION_EXCHANGE_TIMEOUT_SECONDS = 5.0
|
||||
CALIBRATION_KEEPALIVE_SECONDS = 30
|
||||
CALIBRATION_LOOP_INTERVAL_SECONDS = 0.05
|
||||
MAX_DEVICE_INFO_RESPONSE_BYTES = 64 * 1024
|
||||
|
||||
CALIBRATION_READ_SUBSCRIPTIONS: tuple[tuple[str, int], ...] = (
|
||||
(DEVICE_INFO_RESPONSE_TOPIC, 0),
|
||||
(CALIBRATION_FILE_RESPONSE_TOPIC, 2),
|
||||
)
|
||||
|
||||
|
||||
class CalibrationMqttTransportError(RuntimeError):
|
||||
"""The read-only calibration transport failed before a publish."""
|
||||
|
||||
def __init__(self, message: str, *, reason_code: str = "transport_failure") -> None:
|
||||
self.reason_code = reason_code
|
||||
super().__init__(message)
|
||||
|
||||
|
||||
class CalibrationReadOutcomeUnknown(RuntimeError):
|
||||
"""A read was published, but its exact response could not be established."""
|
||||
|
||||
def __init__(self, message: str, *, reason_code: str = "read_outcome_unknown") -> None:
|
||||
self.reason_code = reason_code
|
||||
super().__init__(message)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class FactoryCalibrationReadResult:
|
||||
binding: LiveDeviceControlBinding
|
||||
files: tuple[CalibrationFileContent, CalibrationFileContent]
|
||||
transport: dict[str, object]
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CalibrationMqttSnapshot:
|
||||
state: str
|
||||
connect_attempts: int
|
||||
subscribe_attempts: int
|
||||
publish_attempts: int
|
||||
qos2_completions: int
|
||||
correlated_responses: int
|
||||
|
||||
def as_dict(self) -> dict[str, object]:
|
||||
return {
|
||||
"mode": "read-only-factory-calibration",
|
||||
"state": self.state,
|
||||
"connect_attempts": self.connect_attempts,
|
||||
"subscribe_attempts": self.subscribe_attempts,
|
||||
"publish_attempts": self.publish_attempts,
|
||||
"qos2_completions": self.qos2_completions,
|
||||
"correlated_responses": self.correlated_responses,
|
||||
"clean_session": True,
|
||||
"automatic_reconnect": False,
|
||||
"automatic_retry": False,
|
||||
"request_command": 5,
|
||||
"write_command_available": False,
|
||||
}
|
||||
|
||||
|
||||
class ReviewedCalibrationMqttReader:
|
||||
"""One-connection, no-retry reader for two exact factory calibration files."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
host: str,
|
||||
*,
|
||||
port: int = 1883,
|
||||
connect_timeout_seconds: float = CALIBRATION_CONNECT_TIMEOUT_SECONDS,
|
||||
exchange_timeout_seconds: float = CALIBRATION_EXCHANGE_TIMEOUT_SECONDS,
|
||||
allow_device_ap: bool = False,
|
||||
client_factory: Callable[[], mqtt.Client] | None = None,
|
||||
monotonic: Callable[[], float] = time.monotonic,
|
||||
) -> None:
|
||||
self._target_ipv4 = validate_private_ipv4(host)
|
||||
if self._target_ipv4 == AP_FALLBACK_IPV4 and not allow_device_ap:
|
||||
raise ValueError("K1 access-point address is not allowed for this connection mode")
|
||||
if not 1 <= port <= 65535:
|
||||
raise ValueError("port must be between 1 and 65535")
|
||||
for name, value in (
|
||||
("connect_timeout_seconds", connect_timeout_seconds),
|
||||
("exchange_timeout_seconds", exchange_timeout_seconds),
|
||||
):
|
||||
if not math.isfinite(value) or value <= 0:
|
||||
raise ValueError(f"{name} must be finite and greater than zero")
|
||||
self._port = port
|
||||
self._connect_timeout_seconds = connect_timeout_seconds
|
||||
self._exchange_timeout_seconds = exchange_timeout_seconds
|
||||
self._client_factory = client_factory
|
||||
self._monotonic = monotonic
|
||||
self._lock = threading.Lock()
|
||||
self._client: mqtt.Client | None = None
|
||||
self._state = "new"
|
||||
self._connected = False
|
||||
self._subscribed = False
|
||||
self._closing = False
|
||||
self._subscription_mid: int | None = None
|
||||
self._completed_publish_mids: set[int] = set()
|
||||
self._messages: deque[tuple[str, bytes]] = deque()
|
||||
self._callback_error: str | None = None
|
||||
self._connect_attempts = 0
|
||||
self._subscribe_attempts = 0
|
||||
self._publish_attempts = 0
|
||||
self._qos2_completions = 0
|
||||
self._correlated_responses = 0
|
||||
|
||||
def read_factory_calibration(
|
||||
self,
|
||||
authority: ApplicationControlAuthority,
|
||||
) -> FactoryCalibrationReadResult:
|
||||
self.open()
|
||||
try:
|
||||
discovery = _build_device_info_discovery(authority)
|
||||
device_info_payload = self._exchange_once(
|
||||
OneShotPublishEnvelope.from_bootstrap_request(discovery),
|
||||
expected_response_topic=DEVICE_INFO_RESPONSE_TOPIC,
|
||||
)
|
||||
binding = decode_and_bind_device_info_response(device_info_payload, authority).binding
|
||||
if not binding.ready_for_reviewed_profile:
|
||||
raise CalibrationMqttTransportError(
|
||||
"live DeviceInfo does not attest the reviewed activated K1 FW 3.0.2 profile",
|
||||
reason_code="compatibility_profile_mismatch",
|
||||
)
|
||||
|
||||
files: list[CalibrationFileContent] = []
|
||||
for ordinal, path in enumerate(FACTORY_CALIBRATION_PATHS, start=1):
|
||||
request = build_factory_calibration_file_read(authority, binding, path)
|
||||
response_payload = self._exchange_once(
|
||||
request.envelope(ordinal=ordinal),
|
||||
expected_response_topic=CALIBRATION_FILE_RESPONSE_TOPIC,
|
||||
)
|
||||
files.append(
|
||||
decode_factory_calibration_file_response(
|
||||
response_payload,
|
||||
request,
|
||||
authority,
|
||||
binding,
|
||||
)
|
||||
)
|
||||
if len(files) != 2:
|
||||
raise CalibrationMqttTransportError(
|
||||
"factory calibration read did not return exactly two files"
|
||||
)
|
||||
snapshot = self.snapshot().as_dict()
|
||||
return FactoryCalibrationReadResult(
|
||||
binding=binding,
|
||||
files=(files[0], files[1]),
|
||||
transport=snapshot,
|
||||
)
|
||||
finally:
|
||||
self.close()
|
||||
|
||||
def open(self) -> CalibrationMqttSnapshot:
|
||||
with self._lock:
|
||||
if self._state != "new":
|
||||
raise CalibrationMqttTransportError(
|
||||
"calibration transport can be opened only once",
|
||||
reason_code="transport_already_opened",
|
||||
)
|
||||
self._state = "connecting"
|
||||
self._connect_attempts = 1
|
||||
client = self._new_client()
|
||||
client.connect_timeout = self._connect_timeout_seconds
|
||||
self._install_callbacks(client)
|
||||
self._client = client
|
||||
try:
|
||||
result = client.connect(
|
||||
self._target_ipv4,
|
||||
port=self._port,
|
||||
keepalive=CALIBRATION_KEEPALIVE_SECONDS,
|
||||
)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self._fail_before_publish("calibration MQTT connect call failed", exc)
|
||||
if result != mqtt.MQTT_ERR_SUCCESS:
|
||||
self._fail_before_publish("calibration MQTT connect call was rejected")
|
||||
deadline = self._monotonic() + self._connect_timeout_seconds
|
||||
self._drive_until(lambda: self._subscribed, deadline, post_publish=False)
|
||||
with self._lock:
|
||||
self._state = "ready"
|
||||
return self.snapshot()
|
||||
|
||||
def close(self) -> None:
|
||||
with self._lock:
|
||||
if self._state == "closed":
|
||||
return
|
||||
self._closing = True
|
||||
client = self._client
|
||||
if client is not None:
|
||||
try:
|
||||
if self._subscribed:
|
||||
client.unsubscribe([topic for topic, _qos in CALIBRATION_READ_SUBSCRIPTIONS])
|
||||
client.disconnect()
|
||||
except (OSError, RuntimeError, ValueError):
|
||||
pass
|
||||
with self._lock:
|
||||
self._connected = False
|
||||
self._subscribed = False
|
||||
self._messages.clear()
|
||||
if self._state not in {"failed", "poisoned"}:
|
||||
self._state = "closed"
|
||||
|
||||
def snapshot(self) -> CalibrationMqttSnapshot:
|
||||
with self._lock:
|
||||
return CalibrationMqttSnapshot(
|
||||
state=self._state,
|
||||
connect_attempts=self._connect_attempts,
|
||||
subscribe_attempts=self._subscribe_attempts,
|
||||
publish_attempts=self._publish_attempts,
|
||||
qos2_completions=self._qos2_completions,
|
||||
correlated_responses=self._correlated_responses,
|
||||
)
|
||||
|
||||
def _exchange_once(
|
||||
self,
|
||||
envelope: OneShotPublishEnvelope,
|
||||
*,
|
||||
expected_response_topic: str,
|
||||
) -> bytes:
|
||||
if envelope.topic not in {
|
||||
DEVICE_INFO_REQUEST_TOPIC,
|
||||
"lixel/calibration/request/file",
|
||||
}:
|
||||
raise ValueError("read-only calibration request topic is not allowlisted")
|
||||
if expected_response_topic not in {
|
||||
DEVICE_INFO_RESPONSE_TOPIC,
|
||||
CALIBRATION_FILE_RESPONSE_TOPIC,
|
||||
}:
|
||||
raise ValueError("read-only calibration response topic is not allowlisted")
|
||||
with self._lock:
|
||||
if self._state != "ready" or not self._connected or not self._subscribed:
|
||||
raise CalibrationMqttTransportError("calibration transport is not ready")
|
||||
if self._messages:
|
||||
self._state = "poisoned"
|
||||
raise CalibrationReadOutcomeUnknown(
|
||||
"an uncorrelated response preceded the next calibration read",
|
||||
reason_code="unexpected_response_before_publish",
|
||||
)
|
||||
self._publish_attempts += 1
|
||||
|
||||
client = self._require_client()
|
||||
try:
|
||||
info = client.publish(
|
||||
envelope.topic,
|
||||
payload=envelope.payload,
|
||||
qos=envelope.qos,
|
||||
retain=envelope.retain,
|
||||
)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self._fail_after_publish("calibration MQTT publish call failed", exc)
|
||||
if info.rc != mqtt.MQTT_ERR_SUCCESS or info.mid is None:
|
||||
self._fail_after_publish("calibration MQTT publish returned an unsafe result")
|
||||
publish_mid = int(info.mid)
|
||||
deadline = self._monotonic() + self._exchange_timeout_seconds
|
||||
|
||||
def complete() -> bool:
|
||||
with self._lock:
|
||||
return publish_mid in self._completed_publish_mids and bool(self._messages)
|
||||
|
||||
self._drive_until(complete, deadline, post_publish=True)
|
||||
self._service_once(post_publish=True)
|
||||
with self._lock:
|
||||
if len(self._messages) != 1:
|
||||
self._state = "poisoned"
|
||||
raise CalibrationReadOutcomeUnknown(
|
||||
"duplicate calibration response made read correlation ambiguous",
|
||||
reason_code="duplicate_response",
|
||||
)
|
||||
topic, payload = self._messages.popleft()
|
||||
if topic != expected_response_topic:
|
||||
self._state = "poisoned"
|
||||
raise CalibrationReadOutcomeUnknown(
|
||||
"calibration response topic did not match the issued read",
|
||||
reason_code="response_topic_mismatch",
|
||||
)
|
||||
self._correlated_responses += 1
|
||||
return payload
|
||||
|
||||
def _new_client(self) -> mqtt.Client:
|
||||
if self._client_factory is not None:
|
||||
return self._client_factory()
|
||||
return mqtt.Client(
|
||||
callback_api_version=CallbackAPIVersion.VERSION2,
|
||||
client_id=f"mck1-cal-{secrets.token_hex(7)}",
|
||||
clean_session=True,
|
||||
protocol=mqtt.MQTTv311,
|
||||
reconnect_on_failure=False,
|
||||
)
|
||||
|
||||
def _install_callbacks(self, client: mqtt.Client) -> None:
|
||||
def on_connect(
|
||||
callback_client: mqtt.Client,
|
||||
_userdata: object,
|
||||
_flags: mqtt.ConnectFlags,
|
||||
reason_code: ReasonCode,
|
||||
_properties: Properties | None,
|
||||
) -> None:
|
||||
if reason_code.is_failure:
|
||||
self._set_callback_error("calibration MQTT broker rejected connection")
|
||||
return
|
||||
with self._lock:
|
||||
self._connected = True
|
||||
self._subscribe_attempts = 1
|
||||
try:
|
||||
result, mid = callback_client.subscribe(list(CALIBRATION_READ_SUBSCRIPTIONS))
|
||||
except (OSError, RuntimeError, ValueError):
|
||||
self._set_callback_error("calibration MQTT response subscription failed")
|
||||
return
|
||||
if result != mqtt.MQTT_ERR_SUCCESS or mid is None:
|
||||
self._set_callback_error("calibration MQTT response subscription was rejected")
|
||||
return
|
||||
with self._lock:
|
||||
self._subscription_mid = mid
|
||||
|
||||
def on_subscribe(
|
||||
_callback_client: mqtt.Client,
|
||||
_userdata: object,
|
||||
mid: int,
|
||||
reason_codes: list[ReasonCode],
|
||||
_properties: Properties | None,
|
||||
) -> None:
|
||||
with self._lock:
|
||||
expected_mid = self._subscription_mid
|
||||
if mid != expected_mid or len(reason_codes) != len(CALIBRATION_READ_SUBSCRIPTIONS):
|
||||
self._set_callback_error("calibration MQTT received an unexpected SUBACK")
|
||||
return
|
||||
if any(reason_code.is_failure for reason_code in reason_codes):
|
||||
self._set_callback_error("calibration MQTT broker rejected a subscription")
|
||||
return
|
||||
with self._lock:
|
||||
self._subscribed = True
|
||||
|
||||
def on_publish(
|
||||
_callback_client: mqtt.Client,
|
||||
_userdata: object,
|
||||
mid: int,
|
||||
reason_code: ReasonCode,
|
||||
_properties: Properties | None,
|
||||
) -> None:
|
||||
if reason_code.is_failure:
|
||||
self._set_callback_error("calibration MQTT QoS2 transaction failed")
|
||||
return
|
||||
with self._lock:
|
||||
self._completed_publish_mids.add(mid)
|
||||
self._qos2_completions += 1
|
||||
|
||||
def on_message(
|
||||
_callback_client: mqtt.Client,
|
||||
_userdata: object,
|
||||
message: mqtt.MQTTMessage,
|
||||
) -> None:
|
||||
allowed = {topic for topic, _qos in CALIBRATION_READ_SUBSCRIPTIONS}
|
||||
if message.topic not in allowed:
|
||||
self._set_callback_error("calibration MQTT received an unreviewed topic")
|
||||
return
|
||||
payload = bytes(message.payload)
|
||||
maximum = (
|
||||
MAX_DEVICE_INFO_RESPONSE_BYTES
|
||||
if message.topic == DEVICE_INFO_RESPONSE_TOPIC
|
||||
else MAX_CALIBRATION_RESPONSE_BYTES
|
||||
)
|
||||
if len(payload) > maximum:
|
||||
self._set_callback_error("calibration MQTT response exceeds bound")
|
||||
return
|
||||
with self._lock:
|
||||
self._messages.append((message.topic, payload))
|
||||
|
||||
def on_disconnect(
|
||||
_callback_client: mqtt.Client,
|
||||
_userdata: object,
|
||||
_flags: mqtt.DisconnectFlags,
|
||||
reason_code: ReasonCode,
|
||||
_properties: Properties | None,
|
||||
) -> None:
|
||||
with self._lock:
|
||||
expected = self._closing
|
||||
self._connected = False
|
||||
if not expected or reason_code.is_failure:
|
||||
self._set_callback_error("calibration MQTT connection ended unexpectedly")
|
||||
|
||||
client.on_connect = on_connect
|
||||
client.on_subscribe = on_subscribe
|
||||
client.on_publish = on_publish
|
||||
client.on_message = on_message
|
||||
client.on_disconnect = on_disconnect
|
||||
|
||||
def _drive_until(
|
||||
self,
|
||||
predicate: Callable[[], bool],
|
||||
deadline: float,
|
||||
*,
|
||||
post_publish: bool,
|
||||
) -> None:
|
||||
while not predicate():
|
||||
with self._lock:
|
||||
callback_error = self._callback_error
|
||||
if callback_error is not None:
|
||||
if post_publish:
|
||||
self._fail_after_publish(callback_error)
|
||||
self._fail_before_publish(callback_error)
|
||||
if self._monotonic() >= deadline:
|
||||
if post_publish:
|
||||
self._fail_after_publish("calibration MQTT response barrier timed out")
|
||||
self._fail_before_publish("calibration MQTT connection/subscription timed out")
|
||||
self._service_once(post_publish=post_publish)
|
||||
|
||||
def _service_once(self, *, post_publish: bool) -> None:
|
||||
client = self._require_client()
|
||||
try:
|
||||
result = client.loop(timeout=CALIBRATION_LOOP_INTERVAL_SECONDS)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
if post_publish:
|
||||
self._fail_after_publish("calibration MQTT network loop failed", exc)
|
||||
self._fail_before_publish("calibration MQTT network loop failed", exc)
|
||||
if result != mqtt.MQTT_ERR_SUCCESS:
|
||||
if post_publish:
|
||||
self._fail_after_publish("calibration MQTT network loop returned an error")
|
||||
self._fail_before_publish("calibration MQTT network loop returned an error")
|
||||
|
||||
def _require_client(self) -> mqtt.Client:
|
||||
if self._client is None:
|
||||
raise CalibrationMqttTransportError("calibration MQTT client is unavailable")
|
||||
return self._client
|
||||
|
||||
def _set_callback_error(self, message: str) -> None:
|
||||
with self._lock:
|
||||
if self._callback_error is None:
|
||||
self._callback_error = message
|
||||
|
||||
def _fail_before_publish(self, message: str, cause: BaseException | None = None) -> None:
|
||||
with self._lock:
|
||||
self._state = "failed"
|
||||
self.close()
|
||||
error = CalibrationMqttTransportError(message)
|
||||
if cause is not None:
|
||||
raise error from cause
|
||||
raise error
|
||||
|
||||
def _fail_after_publish(self, message: str, cause: BaseException | None = None) -> None:
|
||||
with self._lock:
|
||||
self._state = "poisoned"
|
||||
self.close()
|
||||
error = CalibrationReadOutcomeUnknown(message)
|
||||
if cause is not None:
|
||||
raise error from cause
|
||||
raise error
|
||||
|
||||
|
||||
def _build_device_info_discovery(
|
||||
authority: ApplicationControlAuthority,
|
||||
) -> EncodedApplicationRequest:
|
||||
header = ApplicationRequestHeader(
|
||||
message_type="DeviceInfoRequest",
|
||||
authority=authority,
|
||||
)
|
||||
encoded_header = b"".join(
|
||||
(
|
||||
_text_field(5, header.session_id),
|
||||
_text_field(6, authority.openapi_key),
|
||||
)
|
||||
)
|
||||
payload = _bytes_field(1, encoded_header)
|
||||
return EncodedApplicationRequest(
|
||||
ordinal=1,
|
||||
phase="identity-discovery",
|
||||
message_type="DeviceInfoRequest",
|
||||
topic=DEVICE_INFO_REQUEST_TOPIC,
|
||||
response_topic=DEVICE_INFO_RESPONSE_TOPIC,
|
||||
payload=payload,
|
||||
payload_sha256=hashlib.sha256(payload).hexdigest(),
|
||||
payload_bytes=len(payload),
|
||||
mutates_device=False,
|
||||
requires_live_binding=False,
|
||||
response_required=True,
|
||||
session_id=header.session_id,
|
||||
vendor_device_id=None,
|
||||
)
|
||||
|
||||
|
||||
def _varint(value: int) -> bytes:
|
||||
encoded = bytearray()
|
||||
while value > 0x7F:
|
||||
encoded.append((value & 0x7F) | 0x80)
|
||||
value >>= 7
|
||||
encoded.append(value)
|
||||
return bytes(encoded)
|
||||
|
||||
|
||||
def _bytes_field(number: int, value: bytes) -> bytes:
|
||||
key = _varint((number << 3) | 2)
|
||||
return key + _varint(len(value)) + value
|
||||
|
||||
|
||||
def _text_field(number: int, value: str) -> bytes:
|
||||
return _bytes_field(number, value.encode("utf-8", errors="strict"))
|
||||
@@ -0,0 +1,23 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import unicodedata
|
||||
|
||||
QUICK_CONNECT_HOST_PROFILE_PREFIX = "xgrids.lixelkity-k1.quick-connect.fw-3.v2"
|
||||
|
||||
|
||||
def quick_connect_host_profile_id(device_ap_ssid: str) -> str:
|
||||
"""Return an opaque, device-scoped host credential profile identifier.
|
||||
|
||||
LixelGO's reviewed DeviceData model carries WiFiAP_SSID and
|
||||
WiFiAP_Password per device. The SSID is already operator-visible, but the
|
||||
credential-store account stays opaque so neither value is mistaken for a
|
||||
universal K1 factory profile.
|
||||
"""
|
||||
|
||||
normalized = unicodedata.normalize("NFC", device_ap_ssid).strip()
|
||||
encoded = normalized.encode("utf-8")
|
||||
if not 1 <= len(encoded) <= 32:
|
||||
raise ValueError("K1 AP SSID must contain between 1 and 32 UTF-8 bytes")
|
||||
suffix = hashlib.sha256(encoded).hexdigest()[:24]
|
||||
return f"{QUICK_CONNECT_HOST_PROFILE_PREFIX}.{suffix}"
|
||||
@@ -52,6 +52,9 @@ APPLICATION_ID = "nodedc_mission_core_recorded"
|
||||
SESSION_TIMELINE = "session_time"
|
||||
CAPTURE_TIMELINE = "capture_time"
|
||||
JS_MAX_SAFE_INTEGER = (1 << 53) - 1
|
||||
RECORDED_VIEW_POINT_DECIMATION_THRESHOLD = 100_000
|
||||
RECORDED_VIEW_POINT_STRIDE = 4
|
||||
RECORDED_VIEW_POINT_FRAME_STRIDE = 5
|
||||
|
||||
# Rerun keys viewer state by these IDs. Reusing them for every settings-only
|
||||
# blueprint update makes the update overwrite the existing scene instead of
|
||||
@@ -61,8 +64,9 @@ RECORDED_SPATIAL_VIEW_ID = UUID("5f5f11d5-3b0a-4a81-887b-2be767cba1c0")
|
||||
RECORDED_ROOT_CONTAINER_ID = UUID("b02f2aca-8471-4dcb-b786-53df5a320fc8")
|
||||
RECORDED_POINTS_VISUALIZER_ID = UUID("ca037ec0-8761-4417-86ee-846fa2875303")
|
||||
RECORDED_CAMERA_VIEW_ID = UUID("5c1db75b-07cd-479a-903d-f4f2ed554513")
|
||||
RECORDED_PERCEPTION_3D_VIEW_ID = UUID("0496bd2e-2b4d-4a4f-87b8-3ce4f9f7e114")
|
||||
RECORDED_METRICS_VIEW_ID = UUID("f973fc11-0867-4732-ad3c-97008621fab7")
|
||||
RecordedView = Literal["spatial", "perception", "metrics"]
|
||||
RecordedView = Literal["spatial", "perception", "perception3d", "metrics"]
|
||||
|
||||
|
||||
class RrdExportSummary(TypedDict):
|
||||
@@ -191,13 +195,15 @@ def export_k1mqtt_to_rrd(
|
||||
cancel_event: threading.Event | None = None,
|
||||
activity_callback: Callable[[], None] | None = None,
|
||||
) -> RrdExportSummary:
|
||||
"""Losslessly project every decodable K1 data-plane frame into one RRD.
|
||||
"""Project a bounded-rate view of K1 data into one operator RRD.
|
||||
|
||||
The raw capture remains the source of record. The derived RRD uses a
|
||||
recording-local duration timeline whose zero is the durable capture-clock
|
||||
origin for v2 recordings (or the first raw message for legacy captures).
|
||||
It never traverses the bounded live-preview queue, so export throughput
|
||||
cannot drop point or pose frames.
|
||||
It never traverses the bounded live-preview queue. Point-cloud frames and
|
||||
very dense point batches are deterministically sampled for interactive
|
||||
rendering while counters, poses, capture boundaries and the native capture
|
||||
remain complete. AI jobs always read the complete native capture.
|
||||
|
||||
The destination is replaced only after the temporary RRD has been closed,
|
||||
flushed and fsynced. Any decode, timing, sink or rename failure therefore
|
||||
@@ -328,9 +334,10 @@ def export_k1mqtt_to_rrd(
|
||||
)
|
||||
counters.observe_decoded(session_time_ns)
|
||||
if isinstance(decoded, DecodedPointCloudView):
|
||||
_log_points(recording, decoded, settings)
|
||||
counters.point_frames += 1
|
||||
counters.points += decoded.point_count
|
||||
if _should_publish_recorded_point_frame(counters.point_frames):
|
||||
_log_points(recording, decoded, settings)
|
||||
elif isinstance(decoded, DecodedPoseView):
|
||||
position = (
|
||||
float(decoded.position_xyz[0]),
|
||||
@@ -548,6 +555,10 @@ def _recorded_blueprint(
|
||||
"/world/trajectory": rrb.EntityBehavior(
|
||||
visible=settings.show_trajectory,
|
||||
),
|
||||
# Dynamic perception must not inherit the mapping view's
|
||||
# historical accumulation window. It is rendered latest-at in
|
||||
# the dedicated perception view below.
|
||||
"/world/perception": rrb.EntityBehavior(visible=False),
|
||||
},
|
||||
# A positive window accumulates historical frames. With no
|
||||
# window, latest-at deliberately keeps one current LiDAR frame.
|
||||
@@ -560,15 +571,41 @@ def _recorded_blueprint(
|
||||
background=[7, 8, 10, 255],
|
||||
)
|
||||
camera_view.id = RECORDED_CAMERA_VIEW_ID
|
||||
perception_3d_view = rrb.Spatial3DView(
|
||||
# Present derived cuboids in the calibrated world scene instead of an
|
||||
# isolated /world/perception subtree. No visible time range is set on
|
||||
# this view: points and cuboids therefore remain latest-at and do not
|
||||
# accumulate into the overlapping-box failure mode.
|
||||
origin="/world",
|
||||
name="Сегментация и объекты · 3D",
|
||||
background=[7, 8, 10, 255],
|
||||
line_grid=rrb.LineGrid3D(
|
||||
visible=settings.show_grid,
|
||||
color=[86, 91, 99, 110],
|
||||
stroke_width=0.75,
|
||||
),
|
||||
overrides={
|
||||
"/world/points": rrb.EntityBehavior(visible=settings.show_points),
|
||||
"/world/trajectory": rrb.EntityBehavior(
|
||||
visible=settings.show_trajectory,
|
||||
),
|
||||
"/world/perception": rrb.EntityBehavior(visible=True),
|
||||
"/world/perception/lidar": rrb.EntityBehavior(visible=False),
|
||||
},
|
||||
)
|
||||
perception_3d_view.id = RECORDED_PERCEPTION_3D_VIEW_ID
|
||||
metrics_view = rrb.TimeSeriesView(
|
||||
origin="/metrics/device",
|
||||
name="Маршрут и время",
|
||||
)
|
||||
metrics_view.id = RECORDED_METRICS_VIEW_ID
|
||||
active_tab = {"spatial": 0, "perception": 1, "metrics": 2}[active_view]
|
||||
active_tab = {"spatial": 0, "perception": 1, "perception3d": 2, "metrics": 3}[
|
||||
active_view
|
||||
]
|
||||
root_container = rrb.Tabs(
|
||||
spatial_view,
|
||||
camera_view,
|
||||
perception_3d_view,
|
||||
metrics_view,
|
||||
active_tab=active_tab,
|
||||
)
|
||||
@@ -747,6 +784,7 @@ def _log_points(
|
||||
if frame.colors_rgb is None
|
||||
else np.frombuffer(frame.colors_rgb, dtype=np.uint8).reshape((-1, 3))
|
||||
)
|
||||
positions, intensities, rgb = _recorded_view_points(positions, intensities, rgb)
|
||||
recording.log(
|
||||
"/world/points",
|
||||
rr.Points3D(
|
||||
@@ -757,6 +795,35 @@ def _log_points(
|
||||
)
|
||||
|
||||
|
||||
def _recorded_view_points(
|
||||
positions: np.ndarray,
|
||||
intensities: np.ndarray,
|
||||
rgb: np.ndarray | None,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None]:
|
||||
# The native K1 capture remains the complete source of record and all AI
|
||||
# jobs read that source directly. Rerun is the interactive operator
|
||||
# projection: bound the temporal frame rate, but preserve complete normal
|
||||
# K1 scans. The AI composition intentionally uses one latest point frame
|
||||
# so dynamic cuboids do not stack; thinning a normal ~2.4k-point scan here
|
||||
# made that view visibly bald. Keep spatial decimation only as an emergency
|
||||
# guard for unusually large (>100k point) frames from future hardware.
|
||||
if len(positions) <= RECORDED_VIEW_POINT_DECIMATION_THRESHOLD:
|
||||
return positions, intensities, rgb
|
||||
return (
|
||||
positions[::RECORDED_VIEW_POINT_STRIDE],
|
||||
intensities[::RECORDED_VIEW_POINT_STRIDE],
|
||||
None if rgb is None else rgb[::RECORDED_VIEW_POINT_STRIDE],
|
||||
)
|
||||
|
||||
|
||||
def _should_publish_recorded_point_frame(frame_number: int) -> bool:
|
||||
"""Keep the first point frame and then a stable 2 Hz operator cadence."""
|
||||
|
||||
if frame_number < 1:
|
||||
raise ValueError("point frame number must be positive")
|
||||
return frame_number == 1 or (frame_number - 1) % RECORDED_VIEW_POINT_FRAME_STRIDE == 0
|
||||
|
||||
|
||||
def _log_pose(
|
||||
recording: rr.RecordingStream,
|
||||
frame: DecodedPoseView,
|
||||
|
||||
@@ -11,6 +11,10 @@ from pathlib import Path
|
||||
from typing import Literal, Protocol, TypedDict
|
||||
|
||||
from k1link.artifacts import utc_now_iso, write_json_atomic
|
||||
from k1link.compute.live_perception import (
|
||||
LivePerceptionResultFrame,
|
||||
decode_live_perception_result,
|
||||
)
|
||||
from k1link.data_plane import DecodedDataPlaneView, NormalizationError
|
||||
from k1link.device_plugins.xgrids_k1.mqtt import CapturedMqttMessage, CaptureError, capture_mqtt
|
||||
from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage
|
||||
@@ -34,6 +38,7 @@ BridgeFactory = Callable[..., RerunBridge]
|
||||
# bounded pose queue. The compact queue protects acquisition from a slow
|
||||
# visualizer, but under sustained pressure it can still evict pose messages.
|
||||
PREVIEW_QUEUE_SIZE = 4
|
||||
PERCEPTION_PREVIEW_QUEUE_SIZE = 2
|
||||
|
||||
|
||||
class CanonicalNormalizer(Protocol):
|
||||
@@ -92,6 +97,9 @@ class VisualizationRuntime:
|
||||
self._closed = False
|
||||
self._scene_settings = RerunSceneSettings()
|
||||
self._metrics = BridgeMetrics()
|
||||
self._perception_messages: queue.Queue[LivePerceptionResultFrame] = queue.Queue(
|
||||
maxsize=PERCEPTION_PREVIEW_QUEUE_SIZE
|
||||
)
|
||||
|
||||
def snapshot(self) -> RuntimeSnapshot:
|
||||
with self._lock:
|
||||
@@ -113,6 +121,29 @@ class VisualizationRuntime:
|
||||
self._notify()
|
||||
return self.snapshot()
|
||||
|
||||
def publish_perception_result(self, encoded: bytes) -> bool:
|
||||
"""Admit one validated latest-wins AI result without blocking acquisition."""
|
||||
|
||||
frame = decode_live_perception_result(encoded)
|
||||
with self._lock:
|
||||
active = self._source_mode in {"live", "replay"} and not self._closed
|
||||
if not active:
|
||||
return False
|
||||
try:
|
||||
self._perception_messages.put_nowait(frame)
|
||||
return True
|
||||
except queue.Full:
|
||||
pass
|
||||
with suppress(queue.Empty):
|
||||
self._perception_messages.get_nowait()
|
||||
self._perception_messages.task_done()
|
||||
self._metrics.perception_dropped()
|
||||
try:
|
||||
self._perception_messages.put_nowait(frame)
|
||||
return True
|
||||
except queue.Full:
|
||||
return False
|
||||
|
||||
def start_replay(self, path: Path, *, speed: float = 1.0, loop: bool = False) -> None:
|
||||
resolved = path.expanduser().resolve()
|
||||
if not resolved.is_file():
|
||||
@@ -140,10 +171,12 @@ class VisualizationRuntime:
|
||||
host: str,
|
||||
out_dir: Path,
|
||||
*,
|
||||
duration_seconds: float = 3600.0,
|
||||
duration_seconds: float | None = None,
|
||||
project_name: str,
|
||||
) -> None:
|
||||
if not math.isfinite(duration_seconds) or duration_seconds <= 0:
|
||||
if duration_seconds is not None and (
|
||||
not math.isfinite(duration_seconds) or duration_seconds <= 0
|
||||
):
|
||||
raise ValueError("длительность приёма должна быть больше нуля")
|
||||
clock_established = threading.Event()
|
||||
self._start(
|
||||
@@ -311,7 +344,7 @@ class VisualizationRuntime:
|
||||
host: str,
|
||||
out_dir: Path,
|
||||
*,
|
||||
duration_seconds: float,
|
||||
duration_seconds: float | None,
|
||||
project_name: str,
|
||||
clock_established: threading.Event,
|
||||
) -> None:
|
||||
@@ -360,6 +393,12 @@ class VisualizationRuntime:
|
||||
publisher_ready = threading.Event()
|
||||
publisher_aborted = threading.Event()
|
||||
publisher_error: list[BaseException] = []
|
||||
while True:
|
||||
try:
|
||||
self._perception_messages.get_nowait()
|
||||
self._perception_messages.task_done()
|
||||
except queue.Empty:
|
||||
break
|
||||
|
||||
def enqueue(message: StreamMessage) -> None:
|
||||
# A plugin may consume non-visual status before the bounded preview
|
||||
@@ -426,11 +465,30 @@ class VisualizationRuntime:
|
||||
self._notify()
|
||||
if publisher_aborted.is_set():
|
||||
return
|
||||
while not source_done.is_set() or not messages.empty():
|
||||
if self._stop_event.is_set() and source_done.is_set() and messages.empty():
|
||||
while (
|
||||
not source_done.is_set()
|
||||
or not messages.empty()
|
||||
or not self._perception_messages.empty()
|
||||
):
|
||||
if (
|
||||
self._stop_event.is_set()
|
||||
and source_done.is_set()
|
||||
and messages.empty()
|
||||
and self._perception_messages.empty()
|
||||
):
|
||||
break
|
||||
try:
|
||||
message = messages.get(timeout=0.1)
|
||||
perception = self._perception_messages.get_nowait()
|
||||
except queue.Empty:
|
||||
perception = None
|
||||
if perception is not None:
|
||||
try:
|
||||
bridge.process_perception(perception)
|
||||
finally:
|
||||
self._perception_messages.task_done()
|
||||
continue
|
||||
try:
|
||||
message = messages.get(timeout=0.05)
|
||||
except queue.Empty:
|
||||
continue
|
||||
try:
|
||||
@@ -573,7 +631,7 @@ def new_live_session_dir(sessions_root: Path) -> Path:
|
||||
def _write_live_session_preamble(
|
||||
out_dir: Path,
|
||||
host: str,
|
||||
duration_seconds: float,
|
||||
duration_seconds: float | None,
|
||||
project_name: str,
|
||||
) -> None:
|
||||
out_dir.mkdir(parents=True, exist_ok=False)
|
||||
|
||||
Reference in New Issue
Block a user