feat(planning): consolidate recorded-route localization and spatial scene

Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
This commit is contained in:
DCCONSTRUCTIONS
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
+36 -2
View File
@@ -385,8 +385,12 @@ class LivePerceptionIngress:
"control": 4,
"camera-init": 1,
"camera-frame": 2,
"lidar": 8,
"pose": 16,
# Recorded K1 bursts reach 19 spatial receipts in 500 ms despite a
# ~10 Hz mean. Allow one burst plus consumer scheduling headroom.
# Still bounded (64 MiB worst case per spatial modality); overflow is
# observable and receipt timestamps/freshness are never rewritten.
"lidar": 32,
"pose": 32,
}
_MAX_PAYLOAD_BYTES: Final[dict[LiveIngressModality, int]] = {
"control": 16 * 1024,
@@ -406,6 +410,7 @@ class LivePerceptionIngress:
self._session_id: str | None = None
self._session_generation = 0
self._active = False
self._spatial_stop_requested = False
self._closed = False
self._consumer_id: str | None = None
self._results_accepted = 0
@@ -430,6 +435,7 @@ class LivePerceptionIngress:
self._session_id = session_id
self._session_generation += 1
self._active = True
self._spatial_stop_requested = False
self._publish_locked(
modality="control",
source_id="mission-core",
@@ -439,6 +445,33 @@ class LivePerceptionIngress:
payload=b'{"event":"session-start"}',
)
def request_spatial_stop(self, session_id: str, session_generation: int) -> bool:
"""Fence derived localisation after an admitted acquisition STOP.
Not proof of hardware standby: recording and raw-first publications
remain active. Only the lifecycle owner supplies this exact identity.
The latched snapshot survives overflow and wakes a waiting consumer.
"""
with self._condition:
if (
not self._active
or self._closed
or self._session_id != session_id
or self._session_generation != session_generation
):
return False
if not self._spatial_stop_requested:
self._spatial_stop_requested = True
self._publish_locked(
modality="control",
source_id="mission-core",
source_sequence=0,
captured_at_epoch_ns=time.time_ns(),
received_monotonic_ns=time.monotonic_ns(),
payload=b'{"event":"spatial-stop-requested"}',
)
return True
def end_session(self, session_id: str) -> None:
with self._condition:
if not self._active or self._session_id != session_id:
@@ -560,6 +593,7 @@ class LivePerceptionIngress:
"schema_version": LIVE_INGRESS_SCHEMA,
"mode": "shadow-diagnostic-only",
"active": self._active,
"spatial_stop_requested": self._spatial_stop_requested,
"session_id": self._session_id,
"session_generation": self._session_generation,
"consumer_connected": self._consumer_id is not None,
@@ -6,6 +6,7 @@ from missioncore_plugin_sdk.v0alpha2 import RuntimePluginDescriptor
from k1link.web.plugin_runtime import DevicePluginRuntimeContribution, InProcessDevicePluginRuntime
from .planning_live import K1PlanningLiveSource
from .camera import build_xgrids_k1_camera_router
from .facade import (
XGRIDS_K1_PLUGIN_ID,
@@ -57,5 +58,5 @@ def build_xgrids_k1_plugin(repository_root: Path) -> DevicePluginRuntimeContribu
),
),
),
observation=build_xgrids_k1_observation(repository_root),
observation=build_xgrids_k1_observation(repository_root, K1PlanningLiveSource(service.live_perception_ingress)),
)
@@ -22641,6 +22641,7 @@ class XgridsK1CompatibilityService:
def dispatch_admission_deadline_reached() -> bool:
return self._operations.deadline_reached(operation.operation_id)
stop_ingress = self.live_perception_ingress.snapshot()
try:
self._application_control_session.request_stop(
confirmation=request.physical_acceptance.confirmation(),
@@ -22663,6 +22664,14 @@ class XgridsK1CompatibilityService:
expected_session_generation=(request.expected_control_session_generation),
expected_state_revision=request.expected_control_state_revision,
)
# End derived localisation after admission, not after the
# potentially long raw-recording/READY finalisation. A
# rejected synchronous request never reaches this edge.
if prepared_stop_lineage.evidence_session_id is not None:
self.live_perception_ingress.request_spatial_stop(
prepared_stop_lineage.evidence_session_id,
stop_ingress["session_generation"],
)
with self._lock:
# The fresh S1 now owns the durable edge. The retained
# S0 classification owner must not race or survive it.
@@ -0,0 +1,82 @@
"""Extract a bounded, provenance-carrying K1 submap from a saved interval."""
from __future__ import annotations
import bisect
import time
import numpy as np
from .protocol.streams import decode_lio_pcl
from .viewer.replay import iter_replay_messages
EXTRACTION = {'version': 'k1-submap/v1', 'max_frames': 120, 'max_raw_points': 2_000_000,
'max_retained_points': 1_000_000, 'voxel_m': .25,
'radius_m': 20., 'height_relative_m': [-3., 6.]}
def extract_submap(source, planning, start, end):
return _extract(source, planning, start, end, presentation=False)
def extract_scene_submap(source, planning, start, end):
"""Presentation geometry is never an input to numerical localisation."""
return _extract(source, planning, start, end, presentation=True)
def _extract(source, planning, start, end, *, presentation):
profile = ({**EXTRACTION, "version": "k1-scene-submap/v1",
"radius_m": 80.0, "height_relative_m": None} if presentation else EXTRACTION)
poses = planning['poses']
if not 0 <= start < end < len(poses):
raise ValueError('Некорректный интервал записи.')
if poses[end]['distance_m'] - poses[start]['distance_m'] > 40:
raise ValueError('Для первой проверки выберите участок не длиннее 40 м.')
lower, upper = poses[start]['message_index'], poses[end]['message_index']
started = time.monotonic()
def messages():
for ordinal, msg in enumerate(iter_replay_messages(source)):
if ordinal > 2_000_000 or time.monotonic() - started > 90:
raise ValueError('Превышен предел подготовки участка записи.')
if ordinal > upper:
break
if ordinal >= lower and msg.topic.endswith('/lio_pcl'):
yield ordinal, msg
eligible = [ordinal for ordinal, _ in messages()]
if not eligible:
raise ValueError('На выбранном участке нет кадров облака.')
selected = {eligible[int(i)] for i in np.linspace(0, len(eligible)-1, min(120, len(eligible)))}
pose_ordinals = [p['message_index'] for p in poses]
chunks, provenance = [], []
raw_count = retained_count = 0
for ordinal, msg in messages():
if ordinal not in selected:
continue
frame = decode_lio_pcl(msg.payload) # Fail closed on any selected corrupt frame.
xyz = np.array([p.scaled_xyz(frame.header.scaler) for p in frame.points], dtype=np.float64)
if not len(xyz):
continue
if not np.isfinite(xyz).all():
raise ValueError('В облаке обнаружены некорректные координаты.')
pose = poses[bisect.bisect_right(pose_ordinals, ordinal)-1]
delta = xyz - np.asarray(pose['position'])
keep = np.linalg.norm(delta, axis=1) <= profile["radius_m"]
if profile["height_relative_m"] is not None:
low, high = profile["height_relative_m"]
keep &= (delta[:, 2] >= low) & (delta[:, 2] <= high)
raw_count += len(xyz); retained_count += int(keep.sum())
if raw_count > EXTRACTION['max_raw_points'] or retained_count > EXTRACTION['max_retained_points']:
raise ValueError('Облако превышает предел размера проверочного участка.')
chunks.append(xyz[keep]) # K1 publishes map-space points: no second pose transform.
timing_available = pose['elapsed_s'] is not None
provenance.append({'message_index': ordinal, 'sequence': msg.sequence,
'received_monotonic_ns': msg.received_monotonic_ns if timing_available else None,
'received_at_epoch_ns': msg.received_at_epoch_ns if timing_available else None,
'pose_index': pose['index']})
points = np.concatenate(chunks) if chunks else np.empty((0, 3))
_, indices = np.unique(np.floor(points / .25).astype(np.int64), axis=0, return_index=True)
points = points[np.sort(indices)]
if presentation and len(points) > 100_000:
# Display LOD spans the entire extracted volume, never a height slice.
points = points[np.linspace(0, len(points)-1, 100_000, dtype=int)]
return points, {'extraction': profile, 'start_index': start, 'end_index': end,
'message_interval': [lower, upper], 'available_frames': len(eligible),
'frames': provenance, 'raw_points': raw_count, 'retained_points': retained_count,
'voxel_points': len(points), 'extraction_seconds': time.monotonic() - started}
@@ -39,7 +39,7 @@ from k1link.web.camera_archive import recover_incomplete_camera_archives
XGRIDS_K1_PLUGIN_ID = "nodedc.device.xgrids-lixelkity-k1"
def build_xgrids_k1_observation(repository_root: Path) -> ObservationRuntimeContribution:
def build_xgrids_k1_observation(repository_root: Path, live_planning_source=None) -> ObservationRuntimeContribution:
"""Compose every K1 evidence root behind the generic observation ABI."""
configured_legacy_root = os.environ.get(
@@ -58,6 +58,9 @@ def build_xgrids_k1_observation(repository_root: Path) -> ObservationRuntimeCont
),
)
point_colors = RecordedPointColorOverlayStore()
from .session_overview import export_session_overview
from .planning_source import export_planning_source
from .localization_source import extract_submap, extract_scene_submap
return ObservationRuntimeContribution(
archives=tuple(
ObservationArchiveSource(
@@ -71,6 +74,11 @@ def build_xgrids_k1_observation(repository_root: Path) -> ObservationRuntimeCont
),
recording_exporter=_export_recording,
point_color_renderer=point_colors.render,
overview_exporter=export_session_overview,
planning_exporter=export_planning_source,
submap_extractor=extract_submap,
scene_submap_extractor=extract_scene_submap,
live_planning_source=live_planning_source,
)
@@ -0,0 +1,74 @@
"""Planning profile uses the existing exclusive derived-data lease, never MQTT."""
import json
import numpy as np
from k1link.sessions.live_planning import PlanningLiveEvent
from .protocol.streams import (
decode_legacy_pointcloud,
decode_legacy_pose,
decode_lio_pcl,
decode_lio_pose,
)
def decode_planning_event(identity, source_id, modality, payload):
"""One decoder for committed live ingress and receipt-paced archive replay."""
if modality == "pose":
frame = (
decode_legacy_pose(payload) if source_id == "RealtimePath" else decode_lio_pose(payload)
)
return PlanningLiveEvent(
**identity,
kind="pose",
position=frame.position_xyz,
orientation_xyzw=frame.orientation_xyzw,
)
if modality == "lidar":
if source_id == "RealtimePointcloud":
frame = decode_legacy_pointcloud(payload, max_points=100_000)
points = np.array([[p.x, p.y, p.z] for p in frame.points], dtype=float).reshape(-1, 3)
else:
frame = decode_lio_pcl(payload)
# Published points already occupy the K1 map frame.
points = np.array(
[p.scaled_xyz(frame.header.scaler) for p in frame.points], dtype=float
).reshape(-1, 3)
if len(points) > 100_000 or not np.isfinite(points).all():
raise ValueError("Некорректный кадр облака.")
return PlanningLiveEvent(**identity, kind="points", points=points)
if modality == "control":
return PlanningLiveEvent(**identity, kind=json.loads(payload)["event"])
return None
class K1PlanningLiveSource:
def __init__(self, ingress):
self.ingress = ingress
def snapshot(self):
return self.ingress.snapshot()
def open(self, consumer_id):
self.ingress.open_consumer(consumer_id)
def close(self, consumer_id):
self.ingress.close_consumer(consumer_id)
def take(self, consumer_id):
event = self.ingress.take_next(consumer_id, timeout=0.25)
if event is None:
return None
identity = dict(
session_id=event.session_id,
generation=event.session_generation,
sequence=event.ingress_sequence,
monotonic_ns=event.received_monotonic_ns,
epoch_ns=event.captured_at_epoch_ns,
)
decoded = decode_planning_event(identity, event.source_id, event.modality, event.payload)
# Preserve one receipt per turn, including modalities planning ignores.
# None means no receipt: bootstrap may use it to finish a queued prefix.
return decoded if decoded is not None else PlanningLiveEvent(**identity, kind="ignored")
@@ -0,0 +1,39 @@
"""Read-only planning events with original, mandatory host receipt clocks."""
from .planning_live import decode_planning_event
from .viewer.replay import detect_replay_format, iter_replay_messages
def iter_planning_events(source, session_id):
if (
detect_replay_format(source) != "k1mqtt"
or not source.with_name("mqtt.metadata.jsonl").is_file()
):
raise ValueError("Causal replay requires native receipt metadata.")
previous = -1
for message in iter_replay_messages(source):
stamp = message.received_monotonic_ns
if stamp is None or stamp < previous:
raise ValueError("Missing or non-monotonic receipt clock.")
previous = stamp
modality = (
"pose"
if message.topic.endswith("/lio_pose")
else "lidar"
if message.topic.endswith("/lio_pcl")
else None
)
if modality is None:
continue
yield decode_planning_event(
dict(
session_id=session_id,
generation=1,
sequence=message.sequence,
monotonic_ns=stamp,
epoch_ns=message.received_at_epoch_ns,
),
message.topic,
modality,
message.payload,
)
@@ -0,0 +1,58 @@
"""Read the complete recorded scanner trajectory; never reapply K1 poses to its map."""
from __future__ import annotations
import json
import math
import time
from pathlib import Path
from .protocol.streams import decode_lio_pcl, decode_lio_pose
from .viewer.replay import detect_replay_format, iter_replay_messages
def export_planning_source(source: Path, destination: Path, *, cancel_event=None, activity_callback=None) -> dict:
poses = []
distance = 0.0
first_time = None
point_frames = errors = 0
started = time.monotonic()
timing = detect_replay_format(source) != 'k1mqtt' or source.with_name('mqtt.metadata.jsonl').is_file()
for ordinal, message in enumerate(iter_replay_messages(source)):
if ordinal % 100 == 0:
if time.monotonic() - started > 90 or (cancel_event is not None and cancel_event.is_set()):
raise ValueError('Превышено время подготовки траектории.')
if activity_callback:
activity_callback()
if ordinal > 2_000_000:
raise ValueError('Запись превышает размер поддерживаемой зоны.')
try:
if message.topic.endswith('/lio_pcl'):
# Prove a spatial payload once. Route extraction does not decode the
# entire cloud a second time; full cloud diagnostics belong to Overview.
if point_frames:
point_frames += 1
elif decode_lio_pcl(message.payload).points:
point_frames = 1
continue
if not message.topic.endswith('/lio_pose'):
continue
frame = decode_lio_pose(message.payload)
except ValueError:
errors += 1
continue
xyz = list(frame.position_xyz)
if not all(math.isfinite(v) for v in xyz):
raise ValueError('Траектория содержит некорректные координаты.')
if len(poses) >= 100_000:
raise ValueError('Траектория превышает 100 000 положений.')
if poses:
distance += math.dist(poses[-1]['position'], xyz)
timestamp = (message.received_monotonic_ns if message.received_monotonic_ns is not None else message.received_at_epoch_ns) if timing else None
if first_time is None:
first_time = timestamp
poses.append({'index': len(poses), 'message_index': ordinal, 'position': xyz,
'elapsed_s': (timestamp - first_time) / 1e9 if timestamp is not None else None,
'distance_m': distance})
if len(poses) < 2 or not point_frames:
raise ValueError('Для зоны необходимы облако точек и траектория.')
result = {'poses': poses, 'path_m': distance, 'point_frames': point_frames, 'decode_errors': errors}
destination.write_text(json.dumps(result, allow_nan=False))
return {'pose_count': len(poses)}
@@ -0,0 +1,159 @@
"""Bounded display overview of a K1 archive, independent of live preview drops."""
from __future__ import annotations
import math
import threading
from pathlib import Path
from typing import Callable
import numpy as np
import rerun as rr
from rerun import blueprint as rrb
from .protocol.streams import decode_lio_pcl, decode_lio_pose
from .viewer.replay import detect_replay_format, iter_replay_messages
from .protocol.normalizer import normalize_k1_message
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
MAX_SAMPLE = 180_000
MAX_SERIES = 200_000
def export_session_overview(source: Path, destination: Path, *,
cancel_event: threading.Event | None = None,
activity_callback: Callable[[], None] | None = None) -> dict:
point_frames = pose_frames = points_total = errors = 0
samples: list[np.ndarray] = []
sample_count = 0
stride = 64
poses: list[tuple[float, float, float]] = []
pose_stride = 1
intervals: list[float] = []
path_length = 0.0
first_pose = last_pose = None
last_cloud_time = first_cloud_time = None
timing_available = detect_replay_format(source) != 'k1mqtt' or source.with_name('mqtt.metadata.jsonl').is_file()
sequence_previous: dict[str, int] = {}
sequence_gaps = sequence_backwards = 0
arrival_backwards = 0
max_gap = 0.0
gaps_over_second = 0
chart: dict[int, tuple[float, float]] = {}
chart_bucket_seconds = 1
messages = 0
for message in iter_replay_messages(source):
if cancel_event is not None and cancel_event.is_set():
raise RuntimeError('overview cancelled')
messages += 1
if messages % 100 == 0 and activity_callback:
activity_callback()
xyz = pose = None
try:
if message.topic.endswith('/lio_pcl'):
frame = decode_lio_pcl(message.payload)
xyz = np.array(frame.points, dtype=np.float64).reshape(-1, 4)[:, :3] / frame.header.scaler
seq = frame.header.seq
elif message.topic.endswith('/lio_pose'):
frame = decode_lio_pose(message.payload)
pose = frame.position_xyz
seq = frame.header.seq
else:
view = normalize_k1_message(message, processing_started_monotonic_ns=0)
if isinstance(view, DecodedPointCloudView):
xyz = np.asarray(view.positions_xyz, dtype=np.float64).reshape(-1, 3)
elif isinstance(view, DecodedPoseView):
pose = view.position_xyz
else:
continue
seq = None
if seq is not None:
previous = sequence_previous.get(message.topic)
if previous is not None:
sequence_gaps += max(0, seq - previous - 1)
sequence_backwards += int(seq <= previous)
sequence_previous[message.topic] = seq
except (ValueError, OverflowError):
errors += 1
continue
if xyz is not None:
point_frames += 1
points_total += len(xyz)
if len(xyz):
samples.append(xyz[(point_frames % min(stride, len(xyz)))::stride].astype(np.float32))
sample_count += len(samples[-1])
if sample_count > MAX_SAMPLE:
samples = [np.concatenate(samples)[::2]]
sample_count = len(samples[0])
stride *= 2
if timing_available:
t = (message.received_monotonic_ns if message.received_monotonic_ns is not None else message.received_at_epoch_ns) / 1e9
if first_cloud_time is None:
first_cloud_time = t
if last_cloud_time is not None:
gap = t - last_cloud_time
arrival_backwards += int(gap < 0)
if gap >= 0:
max_gap = max(max_gap, gap)
gaps_over_second += int(gap > 1)
if len(intervals) < MAX_SERIES:
intervals.append(gap)
elapsed = t - first_cloud_time
bucket = int(elapsed // chart_bucket_seconds)
if bucket not in chart or gap > chart[bucket][1]:
chart[bucket] = (elapsed, gap)
if len(chart) > 1600:
chart_bucket_seconds *= 2
merged: dict[int, tuple[float, float]] = {}
for item in chart.values():
b = int(item[0] // chart_bucket_seconds)
if b not in merged or item[1] > merged[b][1]:
merged[b] = item
chart = merged
last_cloud_time = t
if pose is not None:
if not all(math.isfinite(v) for v in pose):
errors += 1
continue
pose_frames += 1
if first_pose is None:
first_pose = pose
if last_pose is not None:
path_length += math.dist(pose, last_pose)
last_pose = pose
if pose_frames % pose_stride == 0:
poses.append(pose)
if len(poses) > 20_000:
poses = poses[::2]
pose_stride *= 2
cloud = np.concatenate(samples) if samples else np.empty((0, 3), dtype=np.float32)
recording = rr.RecordingStream('missioncore_session_overview')
recording.save(str(destination))
recording.log('world', rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
if len(cloud):
# Fixed renderer colors encode geometry, not product control states.
height = cloud[:, 2]
low, high = np.quantile(height, [.05, .95])
normalized = np.clip((height-low) / max(high-low, .01), 0, 1)
colors = np.column_stack([70+100*normalized, 135+80*normalized, 220-90*normalized]).astype(np.uint8)
recording.log('world/cloud', rr.Points3D(cloud, colors=colors, radii=rr.Radius.ui_points(1.5)), static=True)
if len(poses) > 1:
recording.log('world/route', rr.LineStrips3D([poses], colors=[180, 240, 90], radii=rr.Radius.ui_points(2)), static=True)
recording.log('world/endpoints', rr.Points3D([first_pose, last_pose], labels=['Старт', 'Финиш'], colors=[245, 248, 240], radii=rr.Radius.ui_points(5)), static=True)
recording.send_blueprint(rrb.Blueprint(rrb.Spatial3DView(name='Облако и траектория', origin='/world', background=[9, 10, 12, 255]), collapse_panels=True), make_active=True)
recording.flush()
recording.disconnect()
span = last_cloud_time-first_cloud_time if first_cloud_time is not None and last_cloud_time is not None else None
return {
'point_frames': point_frames, 'pose_frames': pose_frames, 'point_count': points_total,
'sample_points': len(cloud), 'decode_errors': errors,
'sequence_gaps': sequence_gaps, 'sequence_nonincreasing': sequence_backwards,
'path_m': path_length if pose_frames > 1 else None,
'start_end_m': math.dist(first_pose, last_pose) if pose_frames > 1 else None,
'stream_seconds': span, 'mean_hz': (point_frames-1)/span if span and span > 0 else None,
'interval_p95_s': float(np.quantile(intervals, .95)) if intervals else None,
'interval_statistics_complete': point_frames-1 <= MAX_SERIES,
'interval_max_s': max_gap if intervals else None, 'gaps_over_second': gaps_over_second if intervals else None,
'arrival_backwards': arrival_backwards, 'chart': sorted(chart.values()),
'chart_bucket_seconds': chart_bucket_seconds,
'spatial_available': bool(len(cloud) or poses),
}
+10 -1
View File
@@ -40,6 +40,7 @@ class MissionCoreLaunchAgentPlan:
desired_program_arguments: tuple[str, ...]
local_observatory_worker_enabled: bool
desired_payload: bytes
current_process_type: str
def to_dict(self) -> dict[str, object]:
return {
@@ -57,6 +58,8 @@ class MissionCoreLaunchAgentPlan:
),
"current_program_arguments": list(self.current_program_arguments),
"desired_program_arguments": list(self.desired_program_arguments),
"current_process_type": self.current_process_type,
"desired_process_type": "Interactive",
"changes": {
"repository_migration": self.current_working_directory
!= self.desired_working_directory,
@@ -72,6 +75,7 @@ class MissionCoreLaunchAgentPlan:
"bounded_launchd_exit_timeout_seconds": 20,
"keep_alive": True,
"process_group_owned": True,
"operator_interactive_resources": True,
"local_observatory_worker_enabled": (
self.local_observatory_worker_enabled
),
@@ -184,7 +188,11 @@ def plan_mission_core_launch_agent(
"KeepAlive": True,
"RunAtLoad": True,
"AbandonProcessGroup": False,
"ProcessType": "Background",
# This HTTP service owns operator live camera ingestion and bounded
# localization children. Background throttles CPU AND I/O even while
# the browser is active; HTTP does not promote an Adaptive XPC job.
# Interactive is the ordinary application class, not realtime priority.
"ProcessType": "Interactive",
"ThrottleInterval": 5,
"ExitTimeOut": 20,
"StandardOutPath": str(log_path),
@@ -202,6 +210,7 @@ def plan_mission_core_launch_agent(
desired_program_arguments=desired_program_arguments,
local_observatory_worker_enabled=enable_local_observatory_worker,
desired_payload=desired_payload,
current_process_type=str(current.get("ProcessType", "Standard")),
)
+1
View File
@@ -0,0 +1 @@
"""Recorded-zone mission drafts. No vehicle execution authority."""
+269
View File
@@ -0,0 +1,269 @@
"""Bounded 1x laboratory replay. Original receipt time controls input visibility."""
import hashlib
import json
import time
from concurrent.futures import ThreadPoolExecutor
import numpy as np
from k1link.artifacts import utc_now_iso
from .causal_tracking import TRACKING_POLICY, CausalTracking
from .entry_acquisition import ENTRY_POLICY
from .entry_acquisition_worker import run_entry_acquisition
from .live_buffer import LiveCloudBuffer
from .registration import POLICY, path_hint
from .registration_worker import run_registration
def digest(path):
h = hashlib.sha256()
with path.open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
h.update(chunk)
return h.hexdigest()
def replay(
events,
reference,
reference_path,
directory,
*,
mode="baseline",
max_seconds=120.0,
max_distance=40.0,
calculate=run_registration,
initialize=run_entry_acquisition,
):
if mode not in {"baseline", "tracking", "acquisition"}:
raise ValueError("Unknown replay mode.")
if not 0 < max_seconds <= 120 or not 0 < max_distance <= 40:
raise ValueError("Replay exceeds functional probe bounds.")
directory.mkdir(parents=True, exist_ok=False)
buffer = LiveCloudBuffer(reference_path)
gate = CausalTracking()
steps, transitions, deliveries = [], [], []
iterator = iter(events)
first = next(iterator, None)
if first is None:
raise ValueError("Empty replay.")
origin = first.monotonic_ns
started = time.monotonic_ns()
report = dict(
schema_version="missioncore.causal-planning-replay/v1",
mode=mode,
created_at_utc=utc_now_iso(),
started_monotonic_ns=started,
query_origin_monotonic_ns=origin,
pace=1,
policy=POLICY,
tracking_policy=TRACKING_POLICY,
maximum_seconds=max_seconds,
maximum_distance_m=max_distance,
vehicle_control=False,
localization_confirmed=False,
steps=steps,
transitions=transitions,
first_heading_s=None,
first_candidate_s=None,
first_tracking_s=None,
entry_policy=ENTRY_POLICY if mode == "acquisition" else None,
)
last_fit = -5.0
last_snapshot = -1.0
pending = None
future = None
event = first
last_state = None
wall_origin = time.monotonic()
acquisition_attempts = {}
last_acquisition = -float("inf")
def source_now():
return origin + time.monotonic_ns() - started
def observe_state(now):
nonlocal last_state
value = (gate.state, gate.reason, buffer.segment)
if value != last_state:
transitions.append(
dict(
time_s=(now - origin) / 1e9,
state=gate.state,
reason=gate.reason,
segment=buffer.segment,
)
)
last_state = value
def finish_job(now, *, input_active=True):
nonlocal future, pending
if future is None or not future.done():
return
result = future.result()
future = None
sample, info = pending
temporal = (
gate.accept(result, sample, now, buffer.segment)
if input_active
else dict(
accepted=False, reason="input-ended", age_s=(now - sample["monotonic_ns"]) / 1e9
)
)
info.update(
completed_s=(now - origin) / 1e9,
worker_wall_s=time.monotonic() - info.pop("_start"),
temporal=temporal,
tracking_state=gate.state,
streak=gate.streak,
result={k: v for k, v in result.items() if k != "matched_query_indices"},
)
steps.append(info)
if temporal["accepted"] and report["first_candidate_s"] is None:
report["first_candidate_s"] = info["completed_s"]
if gate.state == "tracking" and report["first_tracking_s"] is None:
report["first_tracking_s"] = info["completed_s"]
observe_state(now)
pool = ThreadPoolExecutor(max_workers=1, thread_name_prefix="causal-replay-fit")
try:
while event is not None:
due = (event.monotonic_ns - origin) / 1e9
if due > max_seconds:
report["end_reason"] = "time-bound"
break
now = source_now()
gate.tick(now, buffer.segment)
finish_job(now)
observe_state(now)
if now < event.monotonic_ns:
time.sleep(min(0.02, (event.monotonic_ns - now) / 1e9))
continue
if time.monotonic() - wall_origin > max_seconds + 35:
raise ValueError("Replay exceeded bounded wall-clock allowance.")
before = time.monotonic()
buffer.ingest(event)
deliveries.append(
dict(
sequence=event.sequence,
kind=event.kind,
time_s=due,
lateness_s=max(0.0, (now - event.monotonic_ns) / 1e9),
ingest_s=time.monotonic() - before,
)
)
gate.tick(source_now(), buffer.segment)
if buffer.distance >= max_distance:
report["end_reason"] = "distance-bound"
break
if event.kind == "points" and due - last_snapshot >= 1:
before = time.monotonic()
sample = buffer.snapshot()
last_snapshot = due
snapshot_s = time.monotonic() - before
if (
future is None
and due - last_fit >= 5
and len(sample["points"]) >= 300
and len(steps) < 24
):
try:
hint = path_hint(reference_path, sample["path"])
except ValueError:
hint = None
if hint is not None:
if report["first_heading_s"] is None:
report["first_heading_s"] = due
seed = "route-entry-and-travel-heading"
acquiring = mode == "acquisition" and gate.matrix is None
if acquiring and (
acquisition_attempts.get(buffer.segment, 0)
>= ENTRY_POLICY["maximum_attempts_per_segment"]
or due - last_acquisition < ENTRY_POLICY["retry_interval_s"]
):
event = next(iterator, None)
continue
if mode in {"tracking", "acquisition"} and gate.matrix is not None:
hint = gate.matrix.copy()
seed = "previous-fresh-candidate"
if acquiring:
seed = "bounded-entry-search"
last_fit = due
step_id = len(steps) + 1
step_dir = directory / f"step-{step_id:03d}"
step_dir.mkdir()
meta = dict(
step=step_id,
requested_s=due,
sample_s=(sample["monotonic_ns"] - origin) / 1e9,
sequence=sample["sequence"],
segment=sample["segment"],
distance_m=sample["distance"],
points=len(sample["points"]),
seed=seed,
snapshot_s=snapshot_s,
source_events=sample["events"],
)
(step_dir / "source.json").write_text(json.dumps(meta))
np.save(step_dir / "query-path.npy", sample["path"], allow_pickle=False)
pending = (sample, {**meta, "_start": time.monotonic()})
if acquiring:
acquisition_attempts[buffer.segment] = (
acquisition_attempts.get(buffer.segment, 0) + 1
)
last_acquisition = due
forward = next(
p - reference_path[0]
for p in reference_path[1:]
if np.linalg.norm((p - reference_path[0])[:2]) >= 3
)
future = pool.submit(
initialize,
step_dir,
reference,
sample["points"],
hint,
sample["path"][0],
forward,
)
else:
future = pool.submit(
calculate, step_dir, reference, sample["points"], hint
)
event = next(iterator, None)
report.setdefault("end_reason", "input-ended")
report["input_end_s"] = (
(event.monotonic_ns - origin) / 1e9
if event is not None
else (source_now() - origin) / 1e9
)
gate.clear("input-ended")
observe_state(source_now())
# Finish numerical evidence, but never let a late result restore live state.
while future is not None:
finish_job(source_now(), input_active=False)
if future is not None:
time.sleep(0.02)
report["state"] = "completed"
except Exception as exc:
report.update(state="error", error=f"{type(exc).__name__}: {exc}")
raise
finally:
pool.shutdown(wait=True, cancel_futures=True)
close = getattr(iterator, "close", None)
if close:
close()
report.update(
finished_at_utc=utc_now_iso(),
elapsed_s=(time.monotonic_ns() - started) / 1e9,
gaps=buffer.gaps,
distance_m=buffer.distance,
)
(directory / "deliveries.json").write_text(json.dumps(deliveries))
report["artifacts"] = {
str(p.relative_to(directory)): digest(p) for p in directory.rglob("*") if p.is_file()
}
(directory / "report.json").write_text(json.dumps(report, allow_nan=False))
return report
+76
View File
@@ -0,0 +1,76 @@
"""Experimental temporal qualification; never grants vehicle authority."""
import numpy as np
from .registration import angle_deg, rigid, transform
TRACKING_POLICY = dict(
version="causal-consistency/v1",
consecutive=3,
maximum_position_change_m=0.5,
maximum_rotation_change_deg=5.0,
maximum_age_s=8.0,
)
class CausalTracking:
def __init__(self):
self.matrix = None
self.sample_ns = 0
self.segment = 0
self.streak = 0
self.state = "acquiring"
self.reason = "initial"
def clear(self, reason):
self.matrix = None
self.sample_ns = 0
self.streak = 0
self.state = "lost"
self.reason = reason
def tick(self, now_ns, segment):
if segment != self.segment:
self.clear("receipt-gap")
self.segment = segment
elif (
self.matrix is not None
and (now_ns - self.sample_ns) / 1e9 > TRACKING_POLICY["maximum_age_s"]
):
self.clear("stale")
def accept(self, result, sample, now_ns, segment):
self.tick(now_ns, segment)
age = (now_ns - sample["monotonic_ns"]) / 1e9
evidence = dict(age_s=age, position_change_m=None, rotation_change_deg=None)
if sample["segment"] != segment:
# An old job must never overwrite new-segment state.
return {**evidence, "accepted": False, "reason": "old-segment"}
if not 0 <= age <= TRACKING_POLICY["maximum_age_s"]:
self.clear("stale-result")
elif result["status"] != "candidate":
self.clear("registration-rejected")
else:
matrix = rigid(result["T_reference_query"])
if self.matrix is not None:
position = sample["path"][-1:]
delta = float(
np.linalg.norm(transform(position, matrix) - transform(position, self.matrix))
)
rotation = angle_deg(matrix[:3, :3] @ self.matrix[:3, :3].T)
evidence.update(position_change_m=delta, rotation_change_deg=rotation)
if (
delta > TRACKING_POLICY["maximum_position_change_m"]
or rotation > TRACKING_POLICY["maximum_rotation_change_deg"]
):
self.clear("inconsistent-candidate")
return {**evidence, "accepted": False, "reason": self.reason}
self.matrix = matrix
self.sample_ns = sample["monotonic_ns"]
self.streak += 1
self.state = (
"tracking" if self.streak >= TRACKING_POLICY["consecutive"] else "acquiring"
)
self.reason = "consistent-candidate"
return {**evidence, "accepted": True, "reason": self.reason}
return {**evidence, "accepted": False, "reason": self.reason}
+98
View File
@@ -0,0 +1,98 @@
"""Server-owned drafts with optimistic revisions and immutable check reports."""
from __future__ import annotations
import json
import math
import sqlite3
from uuid import uuid4
from k1link.artifacts import utc_now_iso
class DraftConflict(ValueError):
pass
def route_from_source(source: dict, start: int, end: int, direction: str) -> dict:
poses = source['poses']
if not 0 <= start < end < len(poses) or direction not in {'forward', 'reverse'}:
raise ValueError('Выберите начало и конец маршрута в пределах записи.')
points = poses[start:end + 1]
if direction == 'reverse':
points = list(reversed(points))
steps = [math.dist(a['position'], b['position']) for a, b in zip(points, points[1:])]
return {'start_index': start, 'end_index': end, 'direction': direction,
'length_m': sum(steps), 'max_step_m': max(steps, default=0),
'points': [{'source_index': p['index'], 'position': p['position']} for p in points]}
class MissionDrafts:
def __init__(self, root, sources):
root.mkdir(parents=True, exist_ok=True)
self.database = root / 'mission-drafts.sqlite3'
self.sources = sources
with self.connect() as db:
db.executescript('''CREATE TABLE IF NOT EXISTS drafts (
id TEXT PRIMARY KEY, revision INTEGER NOT NULL, updated TEXT NOT NULL, body TEXT NOT NULL);
CREATE TABLE IF NOT EXISTS checks (
id TEXT PRIMARY KEY, draft_id TEXT NOT NULL, revision INTEGER NOT NULL, body TEXT NOT NULL);''')
def connect(self):
return sqlite3.connect(self.database, timeout=10)
def list(self):
with self.connect() as db:
return [dict(id=id, revision=rev, updated_at_utc=updated, name=name, zone=json.loads(zone), vehicle_id=None)
for id, rev, updated, name, zone in db.execute(
"SELECT id, revision, updated, json_extract(body, '$.name'), json_extract(body, '$.zone') FROM drafts ORDER BY updated DESC")]
def get(self, id):
with self.connect() as db:
row = db.execute('SELECT revision, updated, body FROM drafts WHERE id=?', (id,)).fetchone()
if row is None:
raise KeyError(id)
return dict(json.loads(row[2]), id=id, revision=row[0], updated_at_utc=row[1])
def save(self, request):
source = self.sources.bound(request.session_id, request.generation)
route = route_from_source(source, request.start_index, request.end_index, request.direction)
id = str(request.id or uuid4())
body = {'schema_version': 'missioncore.mission-draft/v1', 'name': request.name.strip(),
'vehicle_id': None, 'status': 'draft', 'zone': {key: source[key] for key in
('session_id', 'label', 'generation', 'frame_id', 'units', 'source_digests')}, 'route': route}
if not body['name']:
raise ValueError('Укажите название черновика.')
now = utc_now_iso()
with self.connect() as db:
db.execute('BEGIN IMMEDIATE')
current = db.execute('SELECT revision FROM drafts WHERE id=?', (id,)).fetchone()
if (current is None and request.revision != 0) or (current is not None and current[0] != request.revision):
raise DraftConflict('Черновик изменён в другом окне. Откройте сохранённую версию.')
revision = request.revision + 1
db.execute('INSERT INTO drafts VALUES (?, ?, ?, ?) ON CONFLICT(id) DO UPDATE SET revision=excluded.revision, updated=excluded.updated, body=excluded.body',
(id, revision, now, json.dumps(body, allow_nan=False)))
return dict(body, id=id, revision=revision, updated_at_utc=now)
def check(self, id, revision):
draft = self.get(id)
if draft['revision'] != revision:
raise DraftConflict('Черновик изменён. Повторите проверку сохранённой версии.')
source = self.sources.verify(draft['zone']['session_id'], draft['zone']['generation'])
route = route_from_source(source, **{key: draft['route'][key] for key in ('direction',)},
start=draft['route']['start_index'], end=draft['route']['end_index'])
warnings = []
if route['length_m'] < .5:
warnings.append('Маршрут короче 0,5 м: для прохода требуется другой участок.')
if route['max_step_m'] > 3:
warnings.append('Между соседними положениями есть разрыв больше 3 м.')
if source['decode_errors']:
warnings.append('В записи есть ошибки чтения кадров.')
report = {'id': str(uuid4()), 'draft_id': id, 'revision': revision, 'created_at_utc': utc_now_iso(),
'kind': 'recorded-route-check', 'length_m': route['length_m'], 'pose_count': len(route['points']),
'max_step_m': route['max_step_m'], 'source_verified': True, 'warnings': warnings,
'localization': 'not_run', 'vehicle_control': False}
with self.connect() as db:
db.execute('BEGIN IMMEDIATE')
current = db.execute('SELECT revision FROM drafts WHERE id=?', (id,)).fetchone()
if current is None or current[0] != revision:
raise DraftConflict('Черновик изменён во время проверки.')
db.execute('INSERT INTO checks VALUES (?, ?, ?, ?)', (report['id'], id, revision, json.dumps(report)))
return report
+227
View File
@@ -0,0 +1,227 @@
"""Bounded, multi-start entry search; geometric agreement is not vehicle authority."""
import math
import time
from itertools import product
import numpy as np
from .registration import PreparedReference, angle_deg, cloud, rigid, transform
ENTRY_POLICY = dict(
version="entry-multistart/v2",
search_order="centre-first/v1",
offsets_m=[-3.0, 0.0, 3.0],
yaw_degrees=[-15.0, 0.0, 15.0],
maximum_entry_radius_m=5.0,
maximum_entry_height_m=1.0,
maximum_entry_rotation_deg=30.0,
cluster_position_m=0.5,
cluster_rotation_deg=5.0,
minimum_support=3,
minimum_translation_seeds=2,
ambiguity_overlap_margin=0.05,
ambiguity_rmse_margin_m=0.03,
deadline_s=25.0,
maximum_attempts_per_segment=2,
retry_interval_s=10.0,
)
def entry_seeds(initial, query_entry, reference_forward, *, policy=ENTRY_POLICY):
initial = rigid(initial)
anchor = np.asarray(query_entry, dtype=float).reshape(1, 3)
forward = np.asarray(reference_forward, dtype=float)[:2]
if (
not np.isfinite(anchor).all()
or not np.isfinite(forward).all()
or np.linalg.norm(forward) < 1e-9
):
raise ValueError("Invalid entry geometry.")
forward = forward / np.linalg.norm(forward)
across = np.array([-forward[1], forward[0]])
target = transform(anchor, initial)[0]
seeds = list(
enumerate(product(policy["offsets_m"], policy["offsets_m"], policy["yaw_degrees"]))
)
if policy.get("search_order") == "centre-first/v1":
seeds.sort(
key=lambda item: (
item[1][0] ** 2 + item[1][1] ** 2,
abs((item[1][2] + 180) % 360 - 180),
item[0],
)
)
for index, (along, lateral, yaw) in seeds:
a = math.radians(yaw)
rotation = np.array(
[[math.cos(a), -math.sin(a), 0], [math.sin(a), math.cos(a), 0], [0, 0, 1]]
)
seed = np.eye(4)
seed[:3, :3] = rotation @ initial[:3, :3]
offset = np.r_[along * forward + lateral * across, 0.0]
seed[:3, 3] = target + offset - seed[:3, :3] @ anchor[0]
yield dict(index=index, along_m=along, across_m=lateral, yaw_deg=yaw, matrix=seed)
def _distance(first, second, query_entry):
a, b = np.asarray(first), np.asarray(second)
position = float(
np.linalg.norm(
transform(np.asarray(query_entry).reshape(1, 3), a)
- transform(np.asarray(query_entry).reshape(1, 3), b)
)
)
return position, angle_deg(a[:3, :3] @ b[:3, :3].T)
def choose_entry(attempts, initial, query_entry, *, complete=True, policy=ENTRY_POLICY):
"""Pure decision, tested independently on competing repeated-place solutions."""
eligible = []
diagnostics = []
for attempt in attempts:
result = attempt["result"]
item = {k: v for k, v in attempt.items() if k != "result"}
item["result"] = {k: v for k, v in result.items() if k != "matched_query_indices"}
item["entry_admitted"] = False
if result["status"] == "candidate":
matrix = rigid(result["T_reference_query"])
delta = (
transform(np.asarray(query_entry).reshape(1, 3), matrix)[0]
- transform(np.asarray(query_entry).reshape(1, 3), initial)[0]
)
angle = angle_deg(matrix[:3, :3] @ initial[:3, :3].T)
item.update(
entry_xy_m=float(np.linalg.norm(delta[:2])),
entry_z_m=float(abs(delta[2])),
entry_rotation_deg=angle,
)
if (
item["entry_xy_m"] <= policy["maximum_entry_radius_m"]
and item["entry_z_m"] <= policy["maximum_entry_height_m"]
and angle <= policy["maximum_entry_rotation_deg"]
):
eligible.append(attempt)
item["entry_admitted"] = True
diagnostics.append(item)
eligible.sort(key=lambda a: (-a["result"]["overlap"], a["result"]["inlier_rmse_m"], a["index"]))
clusters = []
for attempt in eligible:
for cluster in clusters:
distances = [
_distance(
attempt["result"]["T_reference_query"],
x["result"]["T_reference_query"],
query_entry,
)
for x in cluster
]
if all(
p <= policy["cluster_position_m"] and r <= policy["cluster_rotation_deg"]
for p, r in distances
):
cluster.append(attempt)
break
else:
clusters.append([attempt])
reason = None
expected = len(policy["offsets_m"]) ** 2 * len(policy["yaw_degrees"])
if not complete or len(attempts) != expected:
reason = "incomplete-search"
elif not clusters:
reason = "no-admissible-entry"
else:
best = clusters[0][0]["result"]
if any(
c[0]["result"]["overlap"] >= best["overlap"] - policy["ambiguity_overlap_margin"]
and c[0]["result"]["inlier_rmse_m"]
<= best["inlier_rmse_m"] + policy["ambiguity_rmse_margin_m"]
for c in clusters[1:]
):
reason = "ambiguous-entry"
elif (
len(clusters[0]) < policy["minimum_support"]
or len({(x["along_m"], x["across_m"]) for x in clusters[0]})
< policy["minimum_translation_seeds"]
):
reason = "insufficient-multistart-support"
if clusters:
selected = dict(clusters[0][0]["result"])
else:
selected = dict(
status="rejected",
T_reference_query=initial.tolist(),
initial_T_reference_query=initial.tolist(),
overlap=0.0,
inlier_rmse_m=None,
matched_query_indices=[],
localization_confirmed=False,
vehicle_control=False,
)
selected.update(
status="rejected" if reason else "candidate", reasons=[reason] if reason else []
)
if reason:
selected["matched_query_indices"] = []
selected["initialization"] = dict(
policy=policy,
complete=complete,
reason=reason,
attempts=diagnostics,
selected_index=clusters[0][0]["index"] if clusters else None,
clusters=[
dict(
indices=[a["index"] for a in c],
support=len(c),
overlap=c[0]["result"]["overlap"],
rmse_m=c[0]["result"]["inlier_rmse_m"],
)
for c in clusters
],
)
selected["registration_seconds"] = sum(
a["result"].get("registration_seconds", 0.0) for a in attempts
)
return selected
def acquire_entry(
reference,
query,
initial,
query_entry,
reference_forward,
*,
fitter=None,
clock=time.monotonic,
policy=ENTRY_POLICY,
):
reference, query, initial = cloud(reference), cloud(query), rigid(initial)
started = clock()
cpu_started = time.process_time()
if fitter is None:
prepared = PreparedReference(reference)
def fitter(reference, query, initial):
return prepared.register(query, initial)
attempts = []
for seed in entry_seeds(initial, query_entry, reference_forward, policy=policy):
if clock() - started >= policy["deadline_s"]:
break
matrix = seed.pop("matrix")
result = fitter(reference, query, matrix)
attempts.append({**seed, "result": result})
elapsed = clock() - started
result = choose_entry(
attempts,
initial,
query_entry,
complete=elapsed <= policy["deadline_s"],
policy=policy,
)
result["initialization"]["elapsed_s"] = elapsed
# Diagnostic only: never replace the wall deadline with a CPU budget.
# Background scheduling can slow a child even with no competing scanner I/O.
result["initialization"]["process_cpu_s"] = time.process_time() - cpu_started
return result
@@ -0,0 +1,77 @@
"""Single isolated CPU child for one complete bounded initial search."""
import json
import os
import subprocess
import sys
from pathlib import Path
import numpy as np
def run_entry_acquisition(
directory, reference, query, initial, query_entry, reference_forward, *, mode="travel"
):
if mode not in {"travel", "stationary"}:
raise ValueError("Unknown acquisition mode.")
source = directory / "registration-input.npz"
destination = directory / "registration-result.json"
np.savez_compressed(
source,
reference=reference,
query=query,
initial=initial,
query_entry=query_entry,
reference_forward=reference_forward,
)
environment = {
**os.environ,
"OMP_NUM_THREADS": "1",
"OPENBLAS_NUM_THREADS": "1",
"VECLIB_MAXIMUM_THREADS": "1",
}
with (directory / "calculation.log").open("wb") as log:
try:
subprocess.run(
[
sys.executable,
"-m",
"k1link.missions.entry_acquisition_worker",
str(source),
str(destination),
mode,
],
env=environment,
stdout=log,
stderr=log,
timeout=30,
check=True,
)
except subprocess.TimeoutExpired as exc:
raise ValueError(
"Entry acquisition exceeded its deadline; no result accepted."
) from exc
return json.loads(destination.read_text())
def main():
from .entry_acquisition import ENTRY_POLICY, acquire_entry
from .stationary_entry import STATIONARY_POLICY
mode = sys.argv[3] if len(sys.argv) > 3 else "travel"
if mode not in {"travel", "stationary"}:
raise ValueError("Unknown acquisition mode.")
with np.load(Path(sys.argv[1]), allow_pickle=False) as data:
result = acquire_entry(
data["reference"],
data["query"],
data["initial"],
data["query_entry"],
data["reference_forward"],
policy=STATIONARY_POLICY if mode == "stationary" else ENTRY_POLICY,
)
Path(sys.argv[2]).write_text(json.dumps(result, allow_nan=False))
if __name__ == "__main__":
main()
+95
View File
@@ -0,0 +1,95 @@
"""Bounded causal map-frame accumulator for one independent walk."""
import math
from collections import deque
import numpy as np
from .registration import path_hint
class PoseDiscontinuity(ValueError):
"""A new coordinate segment requires fresh localisation, not capture shutdown."""
class LiveCloudBuffer:
def __init__(self, reference_path, *, point_radius_m=20.0):
if not math.isfinite(point_radius_m) or point_radius_m <= 0:
raise ValueError("Радиус облака для совмещения должен быть конечным и положительным.")
self.reference_path = np.asarray(reference_path)
# This is an explicit calculation profile, not a limit on the walk or
# on the scanner. Presentation has its own independent 80-m envelope.
self.point_radius_m = float(point_radius_m)
self.path = []
self.distance = 0.
self.pose_ns = 0
self.sample_ns = 0
self.sequence = 0
self.chunks = deque(maxlen=40)
self.events = deque(maxlen=40)
self.segment = 0
self.gaps = []
def ingest(self, event):
if event.kind == 'pose':
p = np.asarray(event.position, dtype=float)
if p.shape != (3,) or not np.isfinite(p).all():
raise ValueError('Некорректное положение сканера.')
if event.monotonic_ns <= self.pose_ns: return
if self.path:
step = float(np.linalg.norm(p - self.path[-1]))
elapsed = (event.monotonic_ns - self.pose_ns) / 1e9
if step > max(3., min(elapsed, 30.) * 3.):
raise PoseDiscontinuity('Разрыв координат сканера. Требуется новая привязка; запись продолжается.')
if elapsed > 2:
# A receipt gap is not an instantaneous coordinate jump.
# Never fit clouds across a gap or retain its old green result.
self.segment += 1
self.gaps.append({'seconds': elapsed, 'displacement_m': step,
'sequence': event.sequence})
self.chunks.clear(); self.events.clear(); self.sample_ns = 0
if step < .05:
self.pose_ns = event.monotonic_ns
return
self.distance += step
if len(self.path) >= 2000:
# Keep the first and latest pose; display history may decimate,
# but distance and jump checks must never use an old frozen tail.
self.path = self.path[::2] + [self.path[-1]]
self.path.append(p)
self.pose_ns = event.monotonic_ns
elif event.kind == 'points' and self.path:
if not 0 <= event.monotonic_ns - self.pose_ns <= 500_000_000: return
if event.monotonic_ns - self.sample_ns < 500_000_000: return
points = np.asarray(event.points)
d = points - self.path[-1]
keep = (
(np.linalg.norm(d, axis=1) <= self.point_radius_m)
& (d[:,2] >= -3)
& (d[:,2] <= 6)
)
points = points[keep]
_, ix = np.unique(np.floor(points/.25).astype(np.int64), axis=0, return_index=True)
points = points[np.sort(ix)]
# Fixed preview budget; raw capture is retained by the existing recorder.
if len(points) > 4000: points = points[np.linspace(0,len(points)-1,4000,dtype=int)]
self.chunks.append(points)
self.sample_ns = event.monotonic_ns
self.sequence = event.sequence
self.events.append({'sequence': event.sequence, 'monotonic_ns': event.monotonic_ns, 'epoch_ns': event.epoch_ns})
def snapshot(self):
points = np.concatenate(self.chunks) if self.chunks else np.empty((0,3))
if len(points):
_, ix = np.unique(np.floor(points/.25).astype(np.int64), axis=0, return_index=True)
points = points[np.sort(ix)]
if len(points) > 40_000: points = points[np.linspace(0,len(points)-1,40_000,dtype=int)]
path = np.asarray(self.path).reshape(-1,3)
hint = None
if len(path):
hint = np.eye(4)
hint[:3,3] = self.reference_path[0] - path[0]
try: hint = path_hint(self.reference_path, path)
except ValueError: pass
return dict(points=points, path=path, hint=hint, distance=self.distance,
point_radius_m=self.point_radius_m,
sequence=self.sequence, monotonic_ns=self.sample_ns, events=list(self.events),
segment=self.segment, gaps=self.gaps[-30:])
+140
View File
@@ -0,0 +1,140 @@
"""Presentation-only receipt buffer. Never supplies an input to registration.
The newest scanner receipt stays separately addressable at native source cadence.
Older half-second chunks freeze into bounded history, so the renderer never has
to replay a whole map to show a live point-cloud head.
"""
from collections import OrderedDict
from uuid import uuid4
import numpy as np
from .observation_profiles import SCENE_INPUT
DISPLAY_POLICY = dict(
version="planning-fast-display/v2",
scene=SCENE_INPUT,
poll_s=0.1,
chunk_s=0.5,
slots=40,
chunk_points=2000,
total_points=40_000,
voxel_m=0.25,
)
def bounded_points(points):
if not len(points):
return np.empty((0, 3), dtype=np.float32)
_, indices = np.unique(np.floor(points / DISPLAY_POLICY["voxel_m"]), axis=0, return_index=True)
points = points[np.sort(indices)]
if len(points) > DISPLAY_POLICY["chunk_points"]:
points = points[np.linspace(0, len(points) - 1, DISPLAY_POLICY["chunk_points"], dtype=int)]
return np.asarray(points, dtype=np.float32)
class LiveDisplayBuffer:
def __init__(self):
self.epoch = str(uuid4())
self.revision = 0
self.segment = None
self.bucket = None
self.slot = -1
self.chunks = OrderedDict()
self.versions = [0] * DISPLAY_POLICY["slots"]
self.active_points = np.empty((0, 3), dtype=np.float32)
self.current_points = np.empty((0, 3), dtype=np.float32)
self.pose = None
self.path = np.empty((0, 3))
self.packet = None
self.frames = 0
def ingest(self, event, buffer):
# The numerical buffer has already validated pose continuity/identity.
if event.kind == "pose":
if event.monotonic_ns != buffer.pose_ns:
return
self.pose = dict(
sequence=event.sequence,
monotonic_ns=event.monotonic_ns,
position=list(event.position),
)
self.path = np.asarray(buffer.path).reshape(-1, 3)
return
if event.kind != "points" or self.pose is None:
return
if not 0 <= event.monotonic_ns - self.pose["monotonic_ns"] <= 500_000_000:
return
if self.packet and event.monotonic_ns <= self.packet["monotonic_ns"]:
return
points = np.asarray(event.points)
delta = points - self.pose["position"]
points = points[
np.isfinite(points).all(axis=1)
& (np.linalg.norm(delta, axis=1) <= SCENE_INPUT["radius_m"])
]
points = bounded_points(points)
if not len(points):
return
self.revision += 1
if buffer.segment != self.segment:
self.segment = buffer.segment
self.chunks.clear()
self.versions = [self.revision] * DISPLAY_POLICY["slots"]
self.bucket = None
self.slot = -1
self.active_points = np.empty((0, 3), dtype=np.float32)
bucket = event.monotonic_ns // int(DISPLAY_POLICY["chunk_s"] * 1_000_000_000)
if bucket != self.bucket:
if self.bucket is not None and len(self.active_points):
self.slot = (self.slot + 1) % DISPLAY_POLICY["slots"]
self.chunks.pop(self.slot, None)
self.chunks[self.slot] = self.active_points
self.versions[self.slot] = self.revision
self.bucket = bucket
self.active_points = points
else:
self.active_points = bounded_points(np.concatenate((self.active_points, points)))
self.current_points = points
while (
sum(len(p) for p in self.chunks.values()) + len(self.active_points)
> DISPLAY_POLICY["total_points"]
):
removed, _ = self.chunks.popitem(last=False)
self.versions[removed] = self.revision
self.frames += 1
self.packet = dict(
sequence=event.sequence,
monotonic_ns=event.monotonic_ns,
segment=buffer.segment,
events=[],
)
def snapshot(self):
if self.packet is None:
return None
history = list(self.chunks.values())
points = np.concatenate((*history, self.active_points)) if history else self.active_points
return dict(
**self.packet,
points=points,
current_points=self.current_points,
path=self.path,
cloud_revision=self.revision,
# Include tombstones: even a slow reader must remove evicted slots.
chunks=tuple(
(slot, revision, self.chunks.get(slot))
for slot, revision in enumerate(self.versions)
),
pose=self.pose,
)
def diagnostics(self):
return dict(
policy=DISPLAY_POLICY,
frames=self.frames,
revision=self.revision,
chunks=len(self.chunks),
points=sum(len(p) for p in self.chunks.values()) + len(self.active_points),
)
+21
View File
@@ -0,0 +1,21 @@
"""One admission and termination policy for a selected live route."""
import math
LIVE_ROUTE_POLICY = dict(
version="selected-live-route/v1",
minimum_m=3.0,
maximum_m=None,
maximum_seconds=None,
)
def live_route_limits(length_m):
length = float(length_m)
if not math.isfinite(length) or length < LIVE_ROUTE_POLICY["minimum_m"]:
raise ValueError("Для привязки выберите участок длиной не менее 3 м.")
return dict(
route_policy=LIVE_ROUTE_POLICY.copy(),
maximum_distance_m=length,
maximum_seconds=None,
)
+101
View File
@@ -0,0 +1,101 @@
"""Accepted alignment and latest display data; never travel-heading fallback."""
import hashlib
import json
import numpy as np
from .registration import rigid
PRESENTATION_POLICY = dict(version="accepted-alignment-view/v1", cloud_age_s=2.0, snapshot_s=0.5)
class LivePresentation:
def __init__(self):
self.sample = None
self.result = None
self.source_sequence = None
def accept(self, result, sample):
# Correspondence indices belong to the fitted window, not a newer cloud.
self.result = {**result, "matched_query_indices": []}
self.sample = sample
self.source_sequence = sample["sequence"]
def advance(self, sample, accepted, now_ns):
if (
self.result is not None
and accepted is not None
and sample["segment"] == accepted["segment"]
and sample["monotonic_ns"] >= accepted["monotonic_ns"]
and 0 <= now_ns - sample["monotonic_ns"] <= 2_000_000_000
and 0 <= now_ns - accepted["monotonic_ns"] <= 8_000_000_000
):
self.sample = sample
def save(self, directory):
if self.sample is None or self.result is None:
return None
np.savez_compressed(
directory / "aligned-preview.npz",
points=self.sample["points"],
path=self.sample["path"],
transform=rigid(self.result["T_reference_query"]),
)
return dict(
policy=PRESENTATION_POLICY,
alignment_source_sequence=self.source_sequence,
sample_sequence=self.sample["sequence"],
segment=self.sample["segment"],
sample_monotonic_ns=self.sample["monotonic_ns"],
historical=True,
)
def stored_alignment(directory, doc):
"""Hash-verified historical projection, including old runs without new artifacts.
New runs freeze the last aligned display. Older runs expose their last
temporally accepted fit window. A rejected terminal result is never promoted.
"""
def verified(name):
payload = (directory / name).read_bytes()
if hashlib.sha256(payload).hexdigest() != doc.get("artifacts", {}).get(name):
raise ValueError("Данные результата не прошли проверку целостности.")
return payload
if doc.get("presentation") is not None:
verified("aligned-preview.npz")
verified("reference.npy")
with np.load(directory / "aligned-preview.npz", allow_pickle=False) as data:
matrix = rigid(data["transform"])
return (
np.load(directory / "reference.npy", allow_pickle=False),
dict(points=data["points"], path=data["path"]),
dict(
status="candidate", T_reference_query=matrix.tolist(), matched_query_indices=[]
),
)
for path in sorted(directory.glob("step-*/source.json"), reverse=True):
prefix = path.parent.name + "/"
source = json.loads(verified(prefix + "source.json"))
decision_name = prefix + "decision.json"
if decision_name in doc.get("artifacts", {}):
decision = json.loads(verified(decision_name))
if not decision.get("temporal", {}).get("accepted"):
continue
elif source.get("sequence") != doc.get("result_source_sequence"):
continue
result = json.loads(verified(prefix + "registration-result.json"))
if result.get("status") != "candidate":
continue
rigid(result["T_reference_query"])
verified(prefix + "registration-input.npz")
with np.load(path.parent / "registration-input.npz", allow_pickle=False) as data:
return (
data["reference"],
dict(points=data["query"], path=np.array(source["query_path"])),
{**result, "matched_query_indices": []},
)
return None
+172
View File
@@ -0,0 +1,172 @@
"""Incremental Rerun entities for the planning profile, independent of capture."""
import numpy as np
import rerun as rr
from rerun import blueprint as rrb
from .registration import transform
from .registration_colors import query_colors
def clip_reference(points, ceiling_m):
if ceiling_m is None:
return points
return points[points[:, 2] <= ceiling_m]
def clip_query(points, matrix, ceiling_m):
if ceiling_m is None or not len(points):
return points
transformed = transform(points, matrix)
return points[np.isfinite(transformed).all(axis=1) & (transformed[:, 2] <= ceiling_m)]
def log_base(recording, reference, reference_path, options):
size = options.get("point_size", 1.8)
recording.log("world", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
visible_reference = clip_reference(reference, options.get("ceiling_m"))
recording.log(
"world/reference",
rr.Points3D(
visible_reference if options.get("reference", True) else np.empty((0, 3)),
colors=[140, 140, 140],
radii=rr.Radius.ui_points(size),
),
static=True,
)
recording.log(
"world/reference_path",
rr.LineStrips3D(
[reference_path] if options.get("trajectory", True) else [], colors=[185, 185, 185]
),
static=True,
)
# The grid is a display entity, not a replacement blueprint. Changing its
# visibility must not replace the native viewer's operator-owned camera.
recording.log(
"world/grid",
rr.LineStrips3D(
grid_lines(reference) if options.get("grid", True) else [],
colors=[128, 128, 128, 60], radii=rr.Radius.ui_points(0.5),
),
static=True,
)
def grid_lines(reference):
"""Reference-bound XY guide, unchanged by clipping or layer visibility."""
xy = reference[:, :2] if len(reference) else np.array([[-1., -1.], [1., 1.]])
low, high = xy.min(axis=0) - 80, xy.max(axis=0) + 80
# Keep guides legible and bounded for kilometre-scale references. This is
# presentation density only, never a source or localization limit.
spacing = max(1., float(10 ** np.ceil(np.log10(max(high - low) / 400))))
low, high = np.floor(low / spacing) * spacing, np.ceil(high / spacing) * spacing
return [
[[x, low[1], 0], [x, high[1], 0]]
for x in np.arange(low[0], high[0] + spacing / 2, spacing)
] + [
[[low[0], y, 0], [high[0], y, 0]]
for y in np.arange(low[1], high[1] + spacing / 2, spacing)
]
def log_view(recording, reference, options):
"""Only initial admission and explicit camera intents may send a blueprint."""
from k1link.sessions.overview_spatial import _camera_eye
eye = _camera_eye(reference, options.get("mode", "3d"), 1.5)
recording.send_blueprint(
rrb.Blueprint(
rrb.Spatial3DView(
name="Планирование · эталон и новый проход",
origin="/world",
contents=["/world/**"],
line_grid=rrb.LineGrid3D(visible=False),
eye_controls=rrb.EyeControls3D(
kind=rrb.Eye3DKind.Orbital,
position=eye["position"],
look_target=eye["lookTarget"],
eye_up=eye["eyeUp"],
),
background=[9, 10, 12, 255],
),
auto_layout=False,
auto_views=False,
collapse_panels=True,
)
)
def log_evidence(recording, evidence, size, ceiling_m=None):
fitted, accepted = evidence
indices = np.asarray(accepted.get("matched_query_indices", []), dtype=int)
indices = indices[(indices >= 0) & (indices < len(fitted["points"]))]
if accepted["status"] == "candidate" and len(indices):
points = transform(fitted["points"][indices], np.array(accepted["T_reference_query"]))
if ceiling_m is not None:
points = points[np.isfinite(points).all(axis=1) & (points[:, 2] <= ceiling_m)]
recording.log(
"world/validated_query",
rr.Points3D(
points,
colors=query_colors(
points, {"status": "candidate", "matched_query_indices": np.arange(len(points))}
),
radii=rr.Radius.ui_points(size),
),
static=True,
)
def scene_bytes(
run_id,
reference,
reference_path,
sample,
result=None,
*,
base=False,
options=None,
evidence=None,
):
options = options or {}
size = options.get("point_size", 1.8)
recording = rr.RecordingStream("missioncore-planning-live", recording_id=run_id)
sink = recording.binary_stream()
try:
if base:
log_base(recording, reference, reference_path, options)
log_view(recording, reference, options)
recording.log("world/query", rr.Clear(recursive=True), static=True)
recording.log("world/query_path", rr.Clear(recursive=True), static=True)
recording.log("world/validated_query", rr.Clear(recursive=True), static=True)
if (
sample
and result
and result["status"] == "candidate"
and len(sample["points"])
and options.get("query", True)
):
t = np.array(result["T_reference_query"])
points = clip_query(sample["points"], t, options.get("ceiling_m"))
recording.log(
"world/query",
rr.Points3D(
transform(points, t),
colors=query_colors(points),
radii=rr.Radius.ui_points(size),
),
static=True,
)
if len(sample["path"]) > 1 and options.get("trajectory", True):
recording.log(
"world/query_path",
rr.LineStrips3D([transform(sample["path"], t)], colors=[255, 175, 65]),
static=True,
)
if evidence:
log_evidence(recording, evidence, size, options.get("ceiling_m"))
recording.flush()
return sink.read()
finally:
recording.disconnect()
+144
View File
@@ -0,0 +1,144 @@
"""Stateless latest-only scene deltas. A cursor is not localization authority."""
import base64
import hashlib
import json
import numpy as np
import rerun as rr
from .live_scene import clip_query, log_base, log_evidence, log_view
from .registration_colors import query_colors
def encode_cursor(value):
return base64.urlsafe_b64encode(json.dumps(value, separators=(",", ":")).encode()).decode()
def decode_cursor(value):
try:
decoded = json.loads(base64.urlsafe_b64decode(value))
return decoded if isinstance(decoded, dict) else {}
except (ValueError, TypeError):
return {}
def scene_delta(
run_id,
epoch,
reference,
reference_path,
sample,
result,
evidence,
live,
*,
cursor="",
base=False,
options=None,
):
options = options or {}
previous = decode_cursor(cursor)
visible = bool(sample and result and result["status"] == "candidate")
pose = sample.get("pose") if visible and live else None
current = dict(
epoch=epoch,
cloud=sample.get("cloud_revision", 0) if visible else 0,
alignment=result["T_reference_query"] if visible else None,
pose=pose["sequence"] if pose else None,
evidence=evidence[0]["sequence"] if evidence else None,
live=live,
camera=[options.get("mode", "3d"), options.get("reset", 0)],
options=hashlib.sha256(json.dumps(options, sort_keys=True).encode()).hexdigest()[:16],
)
full = (
base
or previous.get("epoch") != epoch
or previous.get("options") != current["options"]
or (previous.get("alignment") is None) != (current["alignment"] is None)
or type(previous.get("cloud")) is not int
or not 0 <= previous.get("cloud", -1) <= current["cloud"]
)
token = encode_cursor(current)
if not full and previous == current:
return b"", token
recording = rr.RecordingStream("missioncore-planning-live", recording_id=run_id)
sink = recording.binary_stream()
size = options.get("point_size", 1.8)
try:
if previous.get("camera") != current["camera"]:
log_view(recording, reference, options)
if full:
log_base(recording, reference, reference_path, options)
for name in ("query", "query_path", "live", "validated_query"):
recording.log("world/" + name, rr.Clear(recursive=True), static=True)
if visible and options.get("query", True):
matrix = np.asarray(current["alignment"])
if full or previous.get("alignment") != current["alignment"]:
recording.log(
"world/query",
rr.Transform3D(translation=matrix[:3, 3], mat3x3=matrix[:3, :3]),
static=True,
)
chunks = sample.get("chunks", ((0, 0, sample["points"]),))
for slot, revision, points in chunks:
if not full and revision <= previous["cloud"]:
continue
name = f"world/query/cloud/{slot}"
# Empty Points3D replaces every component; no temporal history.
points = np.empty((0, 3)) if points is None else points
points = clip_query(points, matrix, options.get("ceiling_m"))
recording.log(
name,
rr.Points3D(
points, colors=query_colors(points), radii=rr.Radius.ui_points(size)
),
static=True,
)
current_points = clip_query(
sample.get("current_points", np.empty((0, 3))), matrix, options.get("ceiling_m")
)
if live:
# Temporal head follows the scanner receipt sequence. Frozen chunks
# remain the bounded trail; this is the only entity updated per frame.
recording.set_time("planning_source_sequence", sequence=int(sample["sequence"]))
recording.log(
"world/query/live",
rr.Points3D(
current_points,
colors=query_colors(current_points),
radii=rr.Radius.ui_points(size),
),
)
if (
full
or previous.get("pose") != current["pose"]
or previous.get("cloud") != current["cloud"]
):
recording.log(
"world/query/path",
rr.LineStrips3D(
[sample["path"]]
if options.get("trajectory", True) and len(sample["path"]) > 1
else [],
colors=[255, 175, 65],
),
static=True,
)
recording.log(
"world/query/scanner",
rr.Points3D(
[pose["position"]] if pose else [],
colors=[255, 175, 65],
radii=rr.Radius.ui_points(6),
),
static=True,
)
if full or previous.get("evidence") != current["evidence"]:
recording.log("world/validated_query", rr.Clear(recursive=True), static=True)
if evidence and options.get("query", True):
log_evidence(recording, evidence, size, options.get("ceiling_m"))
recording.flush()
return sink.read(), token
finally:
recording.disconnect()
+703
View File
@@ -0,0 +1,703 @@
"""One diagnostic planning profile; no device command or vehicle authority."""
from __future__ import annotations
import hashlib
import json
import logging
import os
import threading
import time
from concurrent.futures import ThreadPoolExecutor
from contextlib import ExitStack
from uuid import UUID, uuid4
import numpy as np
from k1link.artifacts import utc_now_iso
from .causal_tracking import TRACKING_POLICY
from .drafts import DraftConflict
from .live_display_buffer import LiveDisplayBuffer
from .live_limits import live_route_limits
from .live_presentation import PRESENTATION_POLICY, LivePresentation, stored_alignment
from .live_scene import scene_bytes
from .live_scene_delta import scene_delta
from .observation_profiles import SCENE_INPUT
from .planning_browser_presentation import BrowserPresentationTelemetry
from .registration_worker import run_registration
from .route_relocalization import ROUTE_RELOCALIZATION_POLICY
from .route_relocalization_worker import run_route_relocalization
from .stationary_bootstrap import BOOTSTRAP_POLICY
from .stationary_entry import STATIONARY_POLICY
from .stationary_live import run_stationary_live
TERMINAL = {"completed", "cancelled", "error", "interrupted"}
logger = logging.getLogger(__name__)
class PlanningLiveTests:
def __init__(self, drafts, sources, compute_lock):
self.drafts, self.sources, self.compute_lock = drafts, sources, compute_lock
self.root = drafts.database.parent / "live-tests"
self.root.mkdir(exist_ok=True)
self.lock = threading.RLock()
self.run = None
self.sample = None
self.accepted_sample = None
self.reference = None
self.scene_reference = None
self._height_reference = None
self._height_bounds = (None, None)
self.reference_path = None
self.cancel = threading.Event()
self.thread = None
self.revision = 0
self.last_frame_ns = 0
self.last_result_ns = 0
self.source = None
self.presentation = LivePresentation()
self.display = LiveDisplayBuffer()
self.browser_presentation = BrowserPresentationTelemetry()
self.latest_pose = None
self.presentation_error = None
self.reinitialization_requested = False
active = self.root / "active.json"
if active.is_file():
self.run = json.loads(active.read_text())
if self.run["state"] not in TERMINAL:
self.run.update(
state="interrupted",
message=(
"Исследование прервано перезапуском сервера. Запись сохранена отдельно."
),
)
self.persist()
self.restore_scene()
def restore_scene(self):
"""Restore only hash-bound derived geometry, never restart capture or fitting."""
directory = self.directory(self.run["id"])
artifacts = self.run.get("artifacts", {})
path = directory / "reference.npy"
if path.is_file() and hashlib.sha256(path.read_bytes()).hexdigest() == artifacts.get(
"reference.npy"
):
self.reference = np.load(path, allow_pickle=False)
self.reference_path = np.array(
[p["position"] for p in self.run["draft"]["route"]["points"]]
)
path = directory / "preview.npz"
if path.is_file() and hashlib.sha256(path.read_bytes()).hexdigest() == artifacts.get(
"preview.npz"
):
with np.load(path, allow_pickle=False) as data:
self.sample = {k: data[k] for k in ("points", "path", "hint")}
try:
frozen = stored_alignment(directory, self.run) if artifacts else None
except (ValueError, OSError) as exc:
self.presentation_error = str(exc)
frozen = None
if frozen is not None:
self.reference, self.presentation.sample, self.presentation.result = frozen
scene_path = directory / "scene-reference.npy"
if scene_path.is_file() and hashlib.sha256(
scene_path.read_bytes()
).hexdigest() == artifacts.get("scene-reference.npy"):
self.scene_reference = np.load(scene_path, allow_pickle=False)
elif self.reference is not None and hasattr(self.drafts.sources, "scene_reference_map"):
# A separate presentation derivative; historical numerical evidence
# and its hashes are never rewritten to improve the viewer.
route, zone = self.run["draft"]["route"], self.run["draft"]["zone"]
try:
self.scene_reference, _ = self.drafts.sources.scene_reference_map(
zone["session_id"], zone["generation"], route["start_index"], route["end_index"]
)
except (ValueError, OSError):
logger.exception("Could not prepare historical presentation geometry")
def directory(self, run_id):
return self.root / str(UUID(run_id))
def history(self):
paths = sorted(
self.root.glob("*/report.json"), key=lambda p: p.stat().st_mtime, reverse=True
)[:100]
items = []
for path in paths:
doc = json.loads(path.read_text())
if doc.get("query_session_id") or doc["state"] not in TERMINAL:
items.append(
{k: doc.get(k) for k in ("id", "state", "created_at_utc", "query_session_id")}
| {"name": doc["draft"]["name"]}
)
return items
def select(self, run_id):
with self.lock:
if self.run and self.run["id"] == run_id:
return self.get()
if self.thread and self.thread.is_alive():
raise ValueError("Сначала завершите текущее исследование.")
path = self.directory(run_id) / "report.json"
if not path.is_file():
raise KeyError(run_id)
doc = json.loads(path.read_text())
if doc["state"] not in TERMINAL:
raise ValueError("Незавершённое исследование нельзя восстановить как живое.")
self.run = doc
self.reference = self.scene_reference = self.sample = self.accepted_sample = (
self.source
) = None
self.presentation = LivePresentation()
self.display = LiveDisplayBuffer()
self.browser_presentation = BrowserPresentationTelemetry()
self.latest_pose = None
self.presentation_error = None
self.last_frame_ns = self.last_result_ns = 0
self.restore_scene()
self.revision += 1
target = self.root / "active.json"
tmp = target.with_suffix(".tmp")
tmp.write_text(json.dumps(doc, allow_nan=False))
os.replace(tmp, target)
return self.get()
def persist(self):
doc = self.run
if doc is None:
return
directory = self.directory(doc["id"])
directory.mkdir(exist_ok=True)
payload = json.dumps(doc, allow_nan=False)
for path in [directory / "report.json", self.root / "active.json"]:
tmp = path.with_suffix(".tmp")
tmp.write_text(payload)
os.replace(tmp, path)
def update(self, **values):
with self.lock:
self.run.update(values)
self.revision += 1
self.persist()
def get(self):
with self.lock:
if self.run is None:
return None
result = json.loads(json.dumps(self.run))
now = time.monotonic_ns()
result["frame_age_s"] = (now - self.last_frame_ns) / 1e9 if self.last_frame_ns else None
result["result_age_s"] = (
(now - self.last_result_ns) / 1e9 if self.last_result_ns else None
)
result["stale"] = not self.last_frame_ns or now - self.last_frame_ns > 8_000_000_000
result["scene_available"] = self.reference is not None
result["scene_height_min_m"], result["scene_height_max_m"] = self.scene_height_bounds()
result["scene_revision"] = self.revision
result["presentation_state"] = (
"live"
if self._view_live(now)
else "historical"
if self.presentation.result
else "unlocalized"
)
result["pose_age_s"] = (
(now - self.latest_pose["monotonic_ns"]) / 1e9 if self.latest_pose else None
)
result["scanner_pose"] = self.latest_pose
if self.display.packet is not None or "display" not in result:
result["display"] = self.display.diagnostics()
result["browser_presentation"] = self.browser_presentation.diagnostics()
return result
def scene_height_bounds(self):
# Reference arrays are frozen and replaced as a unit. Do not rescan
# and copy a route-wide map under the consumer lock on every UI poll.
reference = self.scene_reference if self.scene_reference is not None else self.reference
if reference is not self._height_reference:
finite = reference[np.isfinite(reference).all(axis=1)] if reference is not None else []
self._height_bounds = (
(float(finite[:, 2].min()), SCENE_INPUT["ceiling_m"])
if len(finite) else (None, None)
)
self._height_reference = reference
return self._height_bounds
def _view_live(self, now_ns):
snapshot = self.source.snapshot() if self.source else {}
sample = self.presentation.sample
return bool(
self.run["state"] == "running"
and snapshot.get("active")
and not snapshot.get("spatial_stop_requested", False)
and (snapshot.get("session_id"), snapshot.get("session_generation"))
== (self.run["query_session_id"], self.run["query_generation"])
and self.accepted_sample
and sample
and sample["segment"] == self.accepted_sample["segment"]
and 0 <= now_ns - sample["monotonic_ns"] <= 2_000_000_000
and 0 <= now_ns - self.accepted_sample["monotonic_ns"] <= 8_000_000_000
)
def observe_pose(self, event):
# Scanner-frame telemetry only: not a chassis pose or control contract.
with self.lock:
self.latest_pose = dict(
session_id=event.session_id,
generation=event.generation,
sequence=event.sequence,
monotonic_ns=event.monotonic_ns,
epoch_ns=event.epoch_ns,
position=list(event.position),
orientation_xyzw=list(event.orientation_xyzw)
if event.orientation_xyzw is not None
else None,
frame_id="session/" + event.session_id,
)
def update_sample(self, sample, now_ns):
with self.lock:
self.sample, self.last_frame_ns = sample, sample["monotonic_ns"]
if self.display.packet is None:
self.presentation.advance(sample, self.accepted_sample, now_ns)
def observe_display(self, event, buffer, now_ns):
with self.lock:
# Identity and continuity are checked before this presentation tap.
if (
not self.run
or (event.session_id, event.generation)
!= (
self.run.get("query_session_id"),
self.run.get("query_generation"),
)
or self.run.get("state") != "running"
):
return
self.display.ingest(event, buffer)
latest = self.display.snapshot()
if latest is not None:
self.presentation.advance(latest, self.accepted_sample, now_ns)
def start(self, draft_id, revision):
with self.lock:
if self.thread and self.thread.is_alive():
raise ValueError("Предыдущее исследование ещё выполняется.")
draft = self.drafts.get(draft_id)
if draft["revision"] != revision:
raise DraftConflict("Черновик изменён. Откройте сохранённую версию.")
limits = live_route_limits(draft["route"]["length_m"])
detail = self.drafts.sources.store.get_session(draft["zone"]["session_id"])
source = self.sources.get(detail.plugin_id)
if source is None:
raise ValueError("Для этой модели нет профиля исследования в реальном времени.")
initial = source.snapshot()
if initial["active"]:
raise ValueError(
"Сначала завершите текущую запись. "
"Тест требует нового проекта и отдельного прохода."
)
run_id = str(uuid4())
try:
# Own rollback until a worker actually starts. In particular,
# persistence and thread creation must not strand either lease.
with ExitStack() as startup:
if not self.compute_lock.acquire(blocking=False):
raise ValueError("Другой расчёт совмещения ещё выполняется.")
startup.callback(self.compute_lock.release)
try:
source.open("planning-" + run_id)
except RuntimeError as exc:
raise ValueError(
"Поток занят другим исследованием. "
"Завершите его перед выбором профиля планирования."
) from exc
startup.callback(source.close, "planning-" + run_id)
self.cancel = threading.Event()
self.source = source
self.reference = self.reference_path = self.sample = self.accepted_sample = None
self.scene_reference = None
self.presentation = LivePresentation()
self.display = LiveDisplayBuffer()
self.browser_presentation = BrowserPresentationTelemetry()
self.latest_pose = None
self.presentation_error = None
self.reinitialization_requested = False
self.last_frame_ns = self.last_result_ns = 0
self.revision += 1
self.run = dict(
schema_version="missioncore.planning-live-test/v1",
id=run_id,
profile="planning",
draft=draft,
plugin_id=detail.plugin_id,
state="preparing",
created_at_utc=utc_now_iso(),
query_session_id=None,
query_generation=None,
result=None,
distance_m=0.0,
baseline_generation=initial["session_generation"],
baseline_session_id=initial["session_id"],
ingress_before=initial.get("queues", {}),
localization_confirmed=False,
vehicle_control=False,
slam_reset_verified=False,
hint=BOOTSTRAP_POLICY["version"],
entry_policy=STATIONARY_POLICY,
bootstrap_policy=BOOTSTRAP_POLICY,
route_relocalization_policy=ROUTE_RELOCALIZATION_POLICY,
planning_phase="preparing",
tracking_policy=TRACKING_POLICY,
tracking_state="acquiring",
tracking_established=False,
initialization_attempt=1,
reinitialization_count=0,
**limits,
update_interval_seconds=5,
presentation_policy=PRESENTATION_POLICY,
browser_presentation=self.browser_presentation.diagnostics(),
maximum_query_points=40_000,
message="Подготовка эталонного участка",
)
self.persist()
self.thread = threading.Thread(
target=self.work, args=(source, run_id), name="planning-live", daemon=True
)
response = self.get()
self.thread.start()
startup.pop_all() # The worker now owns both resources.
return response
except Exception as exc:
if self.run is not None and self.run["id"] == run_id:
self.cancel.set()
self.thread = self.source = None
self.record_failure(exc)
raise
def record_failure(self, exc):
"""Revoke live authority even when the report store itself is failing."""
with self.lock:
self.accepted_sample = None
self.last_result_ns = 0
self.run.update(
state="error",
planning_phase="ended",
tracking_state="lost",
planning_reason="execution-error",
tracking_reason="execution-error",
termination_reason="execution-error",
finished_at_utc=utc_now_iso(),
message=str(exc)
if isinstance(exc, ValueError)
else "Исследование не завершено. Исходная запись сохраняется отдельно.",
)
self.revision += 1
try:
self.persist()
(self.directory(self.run["id"]) / "failure.txt").write_text(
f"{type(exc).__name__}: {exc}"
)
except Exception:
# A second storage error must not prevent ownership cleanup or
# leave the in-memory run looking alive. Keep it observable.
logger.exception("Could not persist failed planning run %s", self.run["id"])
def stop(self, run_id):
with self.lock:
if self.run is None or self.run["id"] != run_id:
raise KeyError(run_id)
self.cancel.set()
return self.get()
def request_reinitialization(self, run_id):
"""Start a new stationary location attempt without touching capture.
This is intentionally available only after an unconfirmed route-search
failure. It cannot replace an in-flight computation, a tracked pose,
or the scanner's own stop/start authority.
"""
with self.lock:
if self.run is None or self.run["id"] != run_id:
raise KeyError(run_id)
if self.run["state"] != "running":
raise ValueError("Переинициализация доступна только пока идёт текущая запись.")
if self.reinitialization_requested:
raise ValueError("Переинициализация уже запрошена; дождитесь нового облака.")
if self.run.get("planning_phase") != "lost" or self.run.get("tracking_established"):
raise ValueError(
"Переинициализация доступна после неподтверждённой идентификации маршрута."
)
attempt = int(self.run.get("initialization_attempt", 1)) + 1
self.reinitialization_requested = True
self.accepted_sample = None
self.last_result_ns = 0
self.sample = None
self.presentation = LivePresentation()
self.display = LiveDisplayBuffer()
self.latest_pose = None
self.run.update(
initialization_attempt=attempt,
reinitialization_count=int(self.run.get("reinitialization_count", 0)) + 1,
initialization_result=None,
initialization_temporal=None,
result=None,
temporal=None,
result_source_sequence=None,
result_source_events=None,
result_received_monotonic_ns=None,
planning_phase="waiting-cloud",
planning_reason="operator-reinitialize",
tracking_state="acquiring",
tracking_established=False,
tracking_reason="operator-reinitialize",
message=(
"Переинициализация запрошена. Переместите сканер в новую точку и "
"оставьте неподвижно до начала накопления данных."
),
)
self.revision += 1
self.persist()
return self.get()
def consume_reinitialization(self, run_id):
"""Hand one operator retry to the live worker, exactly once."""
with self.lock:
if self.run is None or (self.run.get("id") is not None and self.run["id"] != run_id):
raise KeyError(run_id)
if not self.reinitialization_requested:
return None
self.reinitialization_requested = False
return self.run["initialization_attempt"]
def work(self, source, run_id):
executor = None
try:
executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="planning-fit")
draft = self.run["draft"]
route, zone = draft["route"], draft["zone"]
points, provenance = self.drafts.sources.reference_map(
zone["session_id"],
zone["generation"],
route["start_index"],
route["end_index"],
cancel_event=self.cancel,
)
if self.cancel.is_set():
raise InterruptedError("Reference preparation cancelled.")
scene_points, scene_provenance = points, None
if hasattr(self.drafts.sources, "scene_reference_map"):
scene_points, scene_provenance = self.drafts.sources.scene_reference_map(
zone["session_id"],
zone["generation"],
route["start_index"],
route["end_index"],
cancel_event=self.cancel,
)
with self.lock:
self.reference = points
self.scene_reference = scene_points
self.reference_path = np.array([p["position"] for p in route["points"]])
directory = self.directory(run_id)
np.save(directory / "reference.npy", points, allow_pickle=False)
np.save(directory / "scene-reference.npy", scene_points, allow_pickle=False)
self.update(
state="waiting",
reference=provenance,
scene_reference=scene_provenance,
planning_phase="waiting-cloud",
message=(
"Эталон готов. Сначала выполняется точная привязка у стартовой зоны; "
"при честном отказе включается поиск по выбранному маршруту. "
"После запуска требуется ожидание на месте."
),
)
run_stationary_live(
self, source, run_id, executor, time, run_route_relocalization, run_registration
)
except InterruptedError:
self.update(
state="cancelled",
planning_phase="ended",
tracking_state="lost",
termination_reason="cancelled",
finished_at_utc=utc_now_iso(),
message="Подготовка отменена. Исходные записи сохранены.",
)
except Exception as exc:
self.record_failure(exc)
finally:
try:
try:
with self.lock:
self.accepted_sample = None
self.last_result_ns = 0
self.run["tracking_state"] = "lost"
# The bounded route-search child may run longer than a local
# tracking fit; never overlap it with a replacement run.
if executor is not None:
executor.shutdown(wait=True, cancel_futures=True)
finally:
source.close("planning-" + run_id)
if self.sample is not None and self.sample["hint"] is not None:
np.savez_compressed(
self.directory(run_id) / "preview.npz",
**{k: self.sample[k] for k in ("points", "path", "hint")},
)
hashes = {}
presentation = self.presentation.save(self.directory(run_id))
for path in [
*self.directory(run_id).glob("step-*/*"),
*self.directory(run_id).glob("reference.npy"),
*self.directory(run_id).glob("scene-reference.npy"),
*self.directory(run_id).glob("preview.npz"),
*self.directory(run_id).glob("aligned-preview.npz"),
]:
hashes[str(path.relative_to(self.directory(run_id)))] = hashlib.sha256(
path.read_bytes()
).hexdigest()
self.update(
artifacts=hashes,
presentation=presentation,
display=self.display.diagnostics(),
browser_presentation=self.browser_presentation.diagnostics(),
ingress_after=source.snapshot().get("queues", {}),
)
except Exception as exc:
self.record_failure(exc)
finally:
self.compute_lock.release()
def commit_result(
self, result, sample, temporal, tracking_state, *, phase, message, tracking_established
):
with self.lock:
self.accepted_sample = sample if temporal["accepted"] else None
self.last_result_ns = sample["monotonic_ns"] if temporal["accepted"] else 0
if temporal["accepted"]:
self.presentation.accept(result, sample)
latest = self.display.snapshot() or self.sample
if latest is not None:
self.presentation.advance(latest, sample, time.monotonic_ns())
# Candidate geometry can be inspected while acquiring. Green requires
# the three-window temporal decision as well as geometric proximity.
self._scene_result = (
result if tracking_state == "tracking" else {**result, "matched_query_indices": []}
)
self.update(
result={k: v for k, v in result.items() if k != "matched_query_indices"},
temporal=temporal,
tracking_state=tracking_state,
result_source_sequence=sample["sequence"],
result_source_events=sample["events"],
result_received_monotonic_ns=sample["monotonic_ns"],
planning_phase=phase,
tracking_established=tracking_established,
message=message,
)
def scene(self, run_id, base=False, options=None):
with self.lock:
if self.run is None or self.run["id"] != run_id:
raise KeyError(run_id)
if self.reference is None:
raise ValueError("Эталон ещё не готов.")
if self.presentation_error:
raise ValueError(self.presentation_error)
sample, result = self.presentation.sample, self.presentation.result
evidence = (
(self.accepted_sample, self._scene_result)
if self._view_live(time.monotonic_ns())
else None
)
reference = self.scene_reference if self.scene_reference is not None else self.reference
reference_path = self.reference_path
return scene_bytes(
run_id,
reference,
reference_path,
sample,
result,
base=base,
options=options,
evidence=evidence,
)
def scene_update(self, run_id, cursor="", base=False, options=None):
with self.lock:
if self.run is None or self.run["id"] != run_id:
raise KeyError(run_id)
if self.reference is None:
raise ValueError("Эталон ещё не готов.")
if self.presentation_error:
raise ValueError(self.presentation_error)
now = time.monotonic_ns()
live = self._view_live(now)
sample, result = self.presentation.sample, self.presentation.result
evidence = (self.accepted_sample, self._scene_result) if live else None
reference = self.scene_reference if self.scene_reference is not None else self.reference
path, epoch = self.reference_path, self.display.epoch
age = (now - sample["monotonic_ns"]) / 1e9 if sample and live else None
fit_age = (now - self.accepted_sample["monotonic_ns"]) / 1e9 if live else None
cloud_revision = sample.get("cloud_revision") if sample and live else None
cloud_sequence = sample.get("sequence") if sample and live else None
display_epoch = self.display.epoch if sample and live else None
height_min_m, height_max_m = self.scene_height_bounds()
# Immutable snapshots; serialization cannot hold the receipt/fit lock.
payload, next_cursor = scene_delta(
run_id,
epoch,
reference,
path,
sample,
result,
evidence,
live,
cursor=cursor,
base=base,
options=options,
)
return (
payload,
next_cursor,
dict(
live=live,
cloud_age_s=age,
fit_age_s=fit_age,
cloud_revision=cloud_revision,
cloud_sequence=cloud_sequence,
display_epoch=display_epoch,
height_min_m=height_min_m,
height_max_m=height_max_m,
),
)
def record_browser_presentation(self, run_id, observations):
"""Keep browser timing as review evidence, outside every live decision."""
with self.lock:
if self.run is None or self.run["id"] != run_id:
raise KeyError(run_id)
current = self.display.snapshot()
if self.run["state"] != "running" or current is None:
return {"accepted": 0, "rejected": 0, "ignored": len(observations)}
accepted = rejected = 0
for observation in observations:
# The browser may lag; it may not claim a future or another-run
# display packet. This is identity fencing, not trust elevation.
if (
observation["display_epoch"] != self.display.epoch
or observation["cloud_revision"] > current["cloud_revision"]
or observation["cloud_sequence"] > current["sequence"]
):
self.browser_presentation.reject()
rejected += 1
continue
self.browser_presentation.record(observation)
accepted += 1
return {"accepted": accepted, "rejected": rejected, "ignored": 0}
def close(self):
self.cancel.set()
if self.thread:
self.thread.join(timeout=125)
@@ -0,0 +1,4 @@
"""Independent numerical and presentation footprints; neither limits a route."""
TRACKING_INPUT = dict(version="local-tracking-input/v1", radius_m=20.0)
SCENE_INPUT = dict(version="planning-scene-input/v2", radius_m=80.0, ceiling_m=80.0)
@@ -0,0 +1,83 @@
"""Bounded browser-side presentation observations for planning live scenes.
The browser can report native Rerun-channel admission and browser animation
frames, but neither signal is a GPU paint receipt. This module deliberately
keeps that distinction in the retained report and has no control authority.
"""
from __future__ import annotations
import math
from collections import deque
from typing import Any
BROWSER_PRESENTATION_POLICY = {
"version": "missioncore.planning-browser-presentation/v1",
"producer": "browser-rerun-admission-two-animation-frames/v1",
"sample_boundary": "browser-reported after native Rerun channel admission and up to two browser animation-frame opportunities",
"source_to_presentation_measurement": "conservative upper bound: server cloud age plus full browser request plus post-admission animation-frame delay",
"not_proved": [
"GPU canvas paint receipt",
"physical scanner-to-pixel clock synchronization",
"registration, route-following, navigation, or safety authority",
],
"maximum_retained_samples": 4096,
"maximum_batch_samples": 8,
}
def _quantile(values: list[float], fraction: float) -> float | None:
if not values:
return None
values = sorted(values)
index = min(len(values) - 1, max(0, math.ceil(len(values) * fraction) - 1))
return values[index]
def _summary(samples: list[dict[str, Any]], name: str) -> dict[str, float | int | None]:
values = [float(sample[name]) for sample in samples if sample.get(name) is not None]
if not values:
return {"count": 0, "min": None, "p50": None, "p95": None, "max": None}
return {
"count": len(values),
"min": min(values),
"p50": _quantile(values, 0.5),
"p95": _quantile(values, 0.95),
"max": max(values),
}
class BrowserPresentationTelemetry:
"""Run-bounded aggregate only; received samples never alter scene state."""
def __init__(self):
self.samples: deque[dict[str, Any]] = deque(
maxlen=BROWSER_PRESENTATION_POLICY["maximum_retained_samples"]
)
self.accepted = 0
self.rejected = 0
def record(self, sample: dict[str, Any]) -> None:
self.samples.append(sample)
self.accepted += 1
def reject(self) -> None:
self.rejected += 1
def diagnostics(self) -> dict[str, Any]:
samples = list(self.samples)
return {
"policy": BROWSER_PRESENTATION_POLICY,
"reported_sample_count": self.accepted,
"retained_sample_count": len(samples),
"rejected_sample_count": self.rejected,
"frame_timeout_count": sum(bool(sample["frame_timeout"]) for sample in samples),
"request_ms": _summary(samples, "request_ms"),
"rerun_admission_ms": _summary(samples, "rerun_admission_ms"),
"first_animation_frame_ms": _summary(samples, "first_animation_frame_ms"),
"second_animation_frame_ms": _summary(samples, "second_animation_frame_ms"),
"source_to_second_animation_frame_upper_bound_ms": _summary(
samples, "source_to_second_animation_frame_upper_bound_ms"
),
}
+151
View File
@@ -0,0 +1,151 @@
"""Project catalog over frozen experiments and unstarted drafts.
Each run keeps its own identity. Browsing never selects a live acquisition,
changes a draft, or reruns registration.
Catalog deletion is a durable tombstone, never destruction of source evidence.
"""
import hashlib
import json
import os
import sqlite3
import threading
from uuid import UUID
from k1link.artifacts import utc_now_iso
class PlanningProjects:
def __init__(self, runs, live):
self.runs, self.live = runs, live
self.scene_lock = threading.Lock()
self.catalog_database = runs.root / 'catalog-deletions.sqlite3'
def _deleted(self):
if not self.catalog_database.is_file():
return set()
with sqlite3.connect(self.catalog_database, timeout=10) as db:
db.execute('CREATE TABLE IF NOT EXISTS deleted_projects '
'(key TEXT PRIMARY KEY, deleted_at TEXT NOT NULL)')
return {row[0] for row in db.execute('SELECT key FROM deleted_projects')}
def remove(self, kind, identity, revision):
"""Remove exactly one catalog entry; retain reports, drafts and captures."""
identity = str(UUID(identity))
if kind not in {'recorded', 'live', 'draft'}:
raise KeyError(identity)
key = f'{kind}:{identity}'
if key in self._deleted():
return {'key': key, 'deleted': True}
doc = self._document(kind, identity)
if self._summary(kind, doc)['revision'] != revision:
raise ValueError('Проект изменён. Обновите список и повторите удаление.')
allowed = {'recorded': {'ready', 'error'},
'live': {'completed', 'cancelled', 'error', 'interrupted'}}
if kind != 'draft' and doc.get('state') not in allowed[kind]:
raise ValueError('Сначала завершите исследование. Выполняющийся проект удалить нельзя.')
if kind == 'draft' and not any(item['key'] == key for item in self.list()):
raise ValueError('Черновик уже связан с исследованием. Обновите список проектов.')
with sqlite3.connect(self.catalog_database, timeout=10) as db:
db.execute('CREATE TABLE IF NOT EXISTS deleted_projects '
'(key TEXT PRIMARY KEY, deleted_at TEXT NOT NULL)')
db.execute('INSERT OR IGNORE INTO deleted_projects VALUES (?, ?)', (key, utc_now_iso()))
return {'key': key, 'deleted': True}
def _document(self, kind, identity):
identity = str(UUID(identity))
if kind == 'recorded':
return self.runs.get(identity)
if kind == 'live' and self.live:
path = self.live.directory(identity) / 'report.json'
if path.is_file():
return json.loads(path.read_text())
if kind == 'draft':
return self.runs.drafts.get(identity)
raise KeyError(identity)
def _summary(self, kind, doc):
draft = doc if kind == 'draft' else doc['draft']
return dict(key=kind+':'+doc['id'], kind=kind, id=doc['id'], name=draft['name'],
created_at_utc=doc.get('created_at_utc', doc.get('updated_at_utc')),
state=doc.get('state', 'draft'), result_status=(doc.get('result') or {}).get('status'),
reference_label=draft['zone']['label'],
query_label=(doc.get('query') or {}).get('label'),
draft_id=draft['id'], revision=draft['revision'])
def list(self):
items, used = [], set()
for kind, owner in [('recorded', self.runs), ('live', self.live)]:
if owner is None:
continue
for path in owner.root.glob('*/report.json'):
doc = json.loads(path.read_text())
# Preparation-only probes have no independent passage evidence.
if kind == 'live' and not doc.get('query_session_id') and doc['state'] in {'completed', 'cancelled', 'error', 'interrupted'}:
continue
items.append(self._summary(kind, doc))
used.add(doc['draft']['id'])
for draft in self.runs.drafts.list():
if draft['id'] not in used:
items.append(self._summary('draft', draft))
deleted = self._deleted()
return sorted((item for item in items if item['key'] not in deleted),
key=lambda item: item['created_at_utc'], reverse=True)
def get(self, kind, identity):
identity = str(UUID(identity))
if f'{kind}:{identity}' in self._deleted():
raise KeyError(identity)
doc = self._document(kind, identity)
summary = self._summary(kind, doc)
result = doc.get('result')
# Keep large correspondence arrays out of the UI report.
result = {k: v for k, v in result.items() if k != 'matched_query_indices'} if result else None
return dict(**summary, draft=doc if kind == 'draft' else doc['draft'], result=result,
message=doc.get('message'), evidence_relation=doc.get('evidence_relation'),
scene_note=('Историческая сцена использует последнюю принятую привязку. '
'Показатели относятся к последнему расчёту.' if kind == 'live' else None),
elapsed_seconds=doc.get('elapsed_seconds'), request=doc.get('request'),
reference=doc.get('reference'), query=doc.get('query'),
scene_url=(doc.get('scene_url') if kind == 'recorded' and doc['state'] == 'ready' else
f'/api/v1/mission-planner/projects/live/{identity}/scene.rrd'
if kind == 'live' and result and doc['state'] in {'completed', 'cancelled', 'error', 'interrupted'} else None),
localization_confirmed=False, vehicle_control=False)
def live_scene(self, identity):
"""Present the committed causal fit, never the terminal unregistered preview.
Derived view cache is separate from immutable inputs/reports. All its
inputs are checked on every open; no fitting or live selection occurs.
"""
import numpy as np
from .live_presentation import stored_alignment
from .registration_scene import write_scene
doc = self._document('live', identity)
if not doc.get('result') or doc['state'] not in {'completed', 'cancelled', 'error', 'interrupted'}:
raise ValueError('В этом исследовании нет сохранённого результата совмещения.')
directory = self.live.directory(identity)
selected = stored_alignment(directory, doc)
if selected is None:
raise ValueError('В этом исследовании нет принятой привязки для сохранённой сцены.')
reference, sample, result = selected
digest = hashlib.sha256(('accepted-alignment-view/v1'+json.dumps(doc, sort_keys=True)).encode()).hexdigest()
cache = directory / 'views'; cache.mkdir(exist_ok=True)
target = cache / f'{digest}.rrd'
with self.scene_lock:
if not target.is_file():
temporary = cache / f'{digest}.tmp'
write_scene(temporary, identity, reference, sample['points'], result,
np.array([p['position'] for p in doc['draft']['route']['points']]),
sample['path'])
os.replace(temporary, target)
return target
def verified_scene(self, identity):
doc = self.runs.get(identity)
if doc['state'] != 'ready':
raise ValueError('Совмещение ещё не завершено.')
path = self.runs.directory(identity) / 'scene.rrd'
if hashlib.sha256(path.read_bytes()).hexdigest() != doc.get('artifacts', {}).get('scene.rrd'):
raise ValueError('Сохранённое облако не прошло проверку целостности.')
return path
+90
View File
@@ -0,0 +1,90 @@
"""Bounded route context, independent of the selected path to follow."""
from contextlib import nullcontext
import numpy as np
REFERENCE_POLICY = dict(
version="route-context-map/v2",
margin_m=20.0,
tile_length_m=40.0,
voxel_m=0.25,
)
def reference_intervals(poses, start, end):
if not 0 <= start < end < len(poses):
raise ValueError("Некорректный интервал эталона.")
distances = np.array([p["distance_m"] for p in poses], dtype=float)
if not np.isfinite(distances).all() or (np.diff(distances) < 0).any():
raise ValueError("Некорректная дистанция эталонной записи.")
lower = int(
np.searchsorted(distances, distances[start] - REFERENCE_POLICY["margin_m"], side="left")
)
upper = min(
len(poses) - 1,
int(np.searchsorted(distances, distances[end] + REFERENCE_POLICY["margin_m"], side="right"))
- 1,
)
tiles = []
cursor = lower
while cursor < upper:
stop = min(
upper,
int(
np.searchsorted(
distances, distances[cursor] + REFERENCE_POLICY["tile_length_m"], side="right"
)
)
- 1,
)
if stop <= cursor:
raise ValueError("Недостаточная непрерывность эталонной карты.")
tiles.append((cursor, stop))
cursor = stop
return tiles
def build_reference_map(
sources, session_id, generation, start, end, *, cancel_event=None, presentation=False
):
planning = sources.bound(session_id, generation)
tiles = reference_intervals(planning["poses"], start, end)
points, evidence = np.empty((0, 3)), []
options = {"presentation": True} if presentation else {}
prepared = (
sources.prepared_submaps(session_id, generation, cancel_event=cancel_event, **options)
if hasattr(sources, "prepared_submaps")
else nullcontext(
lambda first, last: sources.submap(session_id, generation, first, last, **options)
)
)
with prepared as extract:
for first, last in tiles:
if cancel_event is not None and cancel_event.is_set():
raise InterruptedError("Reference preparation cancelled.")
chunk, provenance = extract(first, last)
# Keep one deduplicated map plus one tile, not every overlapping tile.
points = np.concatenate([points, chunk])
_, indices = np.unique(
np.floor(points / REFERENCE_POLICY["voxel_m"]).astype(np.int64),
axis=0,
return_index=True,
)
points = points[np.sort(indices)]
evidence.append(provenance)
if cancel_event is not None and cancel_event.is_set():
raise InterruptedError("Reference preparation cancelled.")
return points, dict(
**{
k: v
for k, v in evidence[0].items()
if k in {"session_id", "generation", "label", "frame_id", "units", "source_digests"}
},
policy=REFERENCE_POLICY,
purpose="presentation" if presentation else "registration",
route_interval=[start, end],
map_interval=[tiles[0][0], tiles[-1][1]],
tiles=evidence,
voxel_points=len(points),
)
+103
View File
@@ -0,0 +1,103 @@
"""Bound a numerical target without thinning the route-wide reference map."""
import hashlib
from itertools import product
import numpy as np
from .registration import transform
WINDOW_POLICY = dict(version="local-reference-window/v2", maximum_points=None, margin_m=10.0)
class ReferenceCoverageError(ValueError):
"""No usable local target; this is not a terminal recording failure."""
class ReferenceWindowIndex:
"""One run-owned spatial index; exact source order and density are retained.
The reference is immutable for the lifetime of a planning run. Only source
indices are stored, not another point-cloud copy or a reduced map. This
index must never be reused with a replacement reference array.
"""
def __init__(self, reference, *, cell_m=10.0):
if not np.isfinite(cell_m) or cell_m <= 0:
raise ValueError("Invalid reference index cell size.")
if reference.ndim != 2 or reference.shape[1] != 3 or not np.isfinite(reference).all():
raise ValueError("Invalid reference index geometry.")
self.reference = reference
self.cell_m = cell_m
cells = np.floor(reference / cell_m).astype(np.int64)
keys, inverse, counts = np.unique(cells, axis=0, return_inverse=True, return_counts=True)
self.order = np.argsort(inverse, kind="stable")
boundaries = np.r_[0, np.cumsum(counts)]
self.slices = {
tuple(key): (int(boundaries[i]), int(boundaries[i + 1])) for i, key in enumerate(keys)
}
def crop(self, center, radius):
if not np.isfinite(center).all() or not np.isfinite(radius) or radius <= 0:
raise ValueError("Invalid reference window.")
lower = np.floor((center - radius) / self.cell_m).astype(np.int64)
upper = np.floor((center + radius) / self.cell_m).astype(np.int64)
pieces = []
ranges = tuple(range(int(a), int(b) + 1) for a, b in zip(lower, upper, strict=True))
# Very broad diagnostic crops should not enumerate empty space.
keys = (
product(*ranges)
if np.prod([len(r) for r in ranges]) <= len(self.slices)
else (
key
for key in self.slices
if all(a <= v <= b for v, a, b in zip(key, lower, upper, strict=True))
)
)
for key in keys:
bounds = self.slices.get(key)
if bounds is not None:
pieces.append(self.order[slice(*bounds)])
indices = (
np.sort(np.concatenate(pieces), kind="stable")
if pieces
else np.empty(0, dtype=np.int64)
)
candidates = self.reference[indices]
mask = np.linalg.norm(candidates - center, axis=1) <= radius
selected = candidates[mask]
if len(selected) == len(self.reference):
selected = self.reference
return selected, len(candidates)
def reference_window(reference, sample, hint, *, initializing=False, index=None):
anchor = sample["path"][0 if initializing else -1]
center = transform(np.asarray(anchor)[None, :], hint)[0]
# The local observation footprint bounds work spatially, not by treating
# an arbitrary point count as evidence that localisation was lost.
radius = (
float(np.linalg.norm(sample["points"] - anchor, axis=1).max()) + WINDOW_POLICY["margin_m"]
)
if index is None:
mask = np.linalg.norm(reference - center, axis=1) <= radius
points = reference if mask.all() else reference[mask]
examined = len(reference)
else:
if index.reference is not reference:
raise ValueError("Reference index belongs to another map.")
points, examined = index.crop(center, radius)
if len(points) < 300:
raise ReferenceCoverageError(
"Недостаточное покрытие локальной области эталона для проверки привязки."
)
return points, dict(
policy=WINDOW_POLICY,
map_points=len(reference),
target_points=len(points),
center=center.tolist() if center is not None else None,
radius_m=radius,
lookup="spatial-index/v1" if index is not None else "full-scan",
examined_points=examined,
target_sha256=hashlib.sha256(np.ascontiguousarray(points).tobytes()).hexdigest(),
)
+211
View File
@@ -0,0 +1,211 @@
"""Bounded CPU registration. A geometric candidate never grants vehicle authority."""
from __future__ import annotations
import math
import time
from importlib.metadata import version
import numpy as np
POLICY = {
"version": "local-gicp-candidate/v1",
"voxel_m": 0.25,
"threads": 1,
"iterations": 40,
"correspondence_m": 1.5,
"evaluation_m": 0.5,
"minimum_overlap": 0.55,
"maximum_rmse_m": 0.25,
"maximum_correction_m": 3.0,
"maximum_correction_deg": 30.0,
"minimum_shape_ratio": 0.002,
"minimum_information_ratio": 0.0001,
}
def rigid(value):
t = np.asarray(value, dtype=np.float64)
if (
t.shape != (4, 4)
or not np.isfinite(t).all()
or not np.allclose(t[3], [0, 0, 0, 1], atol=1e-7)
or not np.allclose(t[:3, :3].T @ t[:3, :3], np.eye(3), atol=1e-6)
or not np.isclose(np.linalg.det(t[:3, :3]), 1.0, atol=1e-6)
):
raise ValueError("Начальная привязка должна быть жёстким преобразованием.")
return t
def transform(points, t):
return np.asarray(points) @ t[:3, :3].T + t[:3, 3]
def cloud(value):
p = np.ascontiguousarray(value, dtype=np.float64)
if (
p.ndim != 2
or p.shape[1] != 3
or len(p) < 300
or not np.isfinite(p).all()
or np.abs(p).max() > 100_000
):
raise ValueError("Для совмещения требуется не менее 300 конечных точек в метрах.")
return p
def angle_deg(rotation):
return math.degrees(math.acos(float(np.clip((np.trace(rotation) - 1) / 2, -1, 1))))
def path_hint(reference_path, query_path):
"""Explicit hypothesis: first query pose is at route entry, headings agree.
Translation/heading come from the operator-selected paths, not recognition.
Roll/pitch start at zero and are refined by full 6-DoF registration.
"""
def heading(path):
p = np.asarray(path, dtype=float)
for point in p[1:]:
d = point - p[0]
if np.linalg.norm(d[:2]) >= 3:
return math.atan2(d[1], d[0])
raise ValueError("Для начального направления нужен участок длиной не менее 3 м.")
yaw = heading(reference_path) - heading(query_path)
c, s = math.cos(yaw), math.sin(yaw)
t = np.eye(4)
t[:3, :3] = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
t[:3, 3] = np.asarray(reference_path[0]) - t[:3, :3] @ np.asarray(query_path[0])
return t
class PreparedReference:
"""One immutable target/tree shared by sequential seeds in a single worker."""
def __init__(self, reference):
try:
import small_gicp as gicp
except ImportError as exc:
raise ValueError("Модуль совмещения не установлен на сервере.") from exc
target = cloud(reference)
self.origin = np.median(target, axis=0)
self.target, self.tree = gicp.preprocess_points(
target - self.origin, downsampling_resolution=0.25, num_threads=1
)
self.gicp = gicp
def register(self, query, initial, *, policy=POLICY):
"""Fit a query against this immutable target under an explicit policy.
The default remains the short-range tracking policy. Route-wide
relocalisation supplies a separate, recorded policy: it may start from
a less exact hypothesis, but it never changes the acceptance criteria
of a normal tracking update by accident.
"""
return _register(self, query, initial, policy=policy)
def register(reference, query, initial, *, policy=POLICY):
started = time.monotonic()
result = PreparedReference(reference).register(query, initial, policy=policy)
result["registration_seconds"] = time.monotonic() - started
return result
def _register(prepared, query, initial, *, policy=POLICY):
source, initial = cloud(query), rigid(initial)
started = time.monotonic()
# Keep seed-dependent voxelization and patch-centre correction identical to
# the single-fit path. Only invariant target preprocessing is shared.
gicp, origin = prepared.gicp, prepared.origin
tgt, tree = prepared.target, prepared.tree
seeded = transform(source, initial) - origin
src, _ = gicp.preprocess_points(seeded, downsampling_resolution=0.25, num_threads=1)
if min(tgt.size(), src.size()) < 300:
raise ValueError("После прореживания недостаточно точек для совмещения.")
result = gicp.align(
tgt,
src,
tree,
registration_type="GICP",
num_threads=1,
max_iterations=40,
max_correspondence_distance=policy["correspondence_m"],
)
delta = rigid(result.T_target_source)
transformed = transform(src.points()[:, :3], delta)
_, sq = tree.batch_nearest_neighbor_search(transformed, num_threads=1)
distances = np.sqrt(np.asarray(sq))
inside = distances <= policy["evaluation_m"]
overlap = float(np.mean(inside))
rmse = float(np.sqrt(np.mean(distances[inside] ** 2))) if inside.any() else None
shape = np.linalg.eigvalsh(np.cov(src.points()[:, :3].T))
shape_ratio = float(max(0, shape[0]) / max(shape[-1], 1e-12))
# Scale the information matrix by its diagonal: otherwise metres/radians and
# point count dominate a raw Hessian condition number.
h = np.asarray(result.H)
scale = np.sqrt(np.maximum(np.diag(h), 1e-12))
eigen = np.linalg.eigvalsh(h / np.outer(scale, scale))
information_ratio = float(max(0, eigen[0]) / max(eigen[-1], 1e-12))
center = np.median(seeded, axis=0)
correction = float(np.linalg.norm(transform(center[None, :], delta)[0] - center))
rotation = angle_deg(delta[:3, :3])
reasons = []
for failed, reason in [
(not result.converged, "Расчёт не сошёлся."),
(overlap < policy["minimum_overlap"], "Недостаточное совпадение поверхностей."),
(
rmse is None or rmse > policy["maximum_rmse_m"],
"Большое расстояние между поверхностями.",
),
(
correction > policy["maximum_correction_m"]
or rotation > policy["maximum_correction_deg"],
"Уточнение вышло за пределы начальной подсказки.",
),
(
shape_ratio < policy["minimum_shape_ratio"]
or information_ratio < policy["minimum_information_ratio"],
"Недостаточно пространственных ориентиров для устойчивой привязки.",
),
]:
if failed:
reasons.append(reason)
c = np.eye(4)
c[:3, 3] = origin
final = c @ delta @ np.linalg.inv(c) @ initial
_, display_sq = tree.batch_nearest_neighbor_search(
transform(source, final) - origin, num_threads=1
)
matched = (
np.flatnonzero(np.asarray(display_sq) <= policy["evaluation_m"] ** 2).tolist()
if not reasons
else []
)
return {
"matched_query_indices": matched,
"correspondence_colors": "accepted-distance-v1",
"status": "rejected" if reasons else "candidate",
"reasons": reasons,
"policy": policy,
"algorithm": "small_gicp/GICP",
"algorithm_version": version("small-gicp"),
"T_reference_query": rigid(final).tolist(),
"initial_T_reference_query": initial.tolist(),
"overlap": overlap,
"inlier_rmse_m": rmse,
"evaluation_points": len(distances),
"reference_points": tgt.size(),
"query_points": src.size(),
"converged": bool(result.converged),
"iterations": int(result.iterations),
"correction_m": correction,
"correction_deg": rotation,
"shape_ratio": shape_ratio,
"information_ratio": information_ratio,
"registration_seconds": time.monotonic() - started,
"localization_confirmed": False,
"vehicle_control": False,
}
@@ -0,0 +1,14 @@
"""Green encodes accepted per-point geometric proximity, never all query points."""
import numpy as np
def query_colors(points, result=None):
points = np.asarray(points)
if not len(points): return np.empty((0,3), dtype=np.uint8)
z = points[:, 2]
v = np.clip((z-np.min(z))/max(float(np.ptp(z)), .01), 0, 1)
colors = np.column_stack((70+185*v, 120-60*v, 255-65*v)).astype(np.uint8)
if result and result.get('status') == 'candidate':
indices = np.asarray(result.get('matched_query_indices', []), dtype=int)
indices = indices[(indices >= 0) & (indices < len(points))]
colors[indices] = [154, 235, 75]
return colors
+129
View File
@@ -0,0 +1,129 @@
"""One bounded background calculation at a time, persisted with source provenance."""
from __future__ import annotations
import hashlib
import json
import os
import platform
import threading
import time
from concurrent.futures import ThreadPoolExecutor
from uuid import UUID, uuid4
import numpy as np
from k1link.artifacts import utc_now_iso
from .drafts import DraftConflict
from .registration import path_hint
from .registration_worker import run_registration
from .registration_scene import write_scene
class RegistrationRuns:
def __init__(self, drafts):
self.drafts = drafts
self.root = drafts.database.parent / 'registration-runs'
self.root.mkdir(parents=True, exist_ok=True)
self.lock = threading.Lock()
self.executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix='registration')
for path in self.root.glob('*/report.json'):
doc = json.loads(path.read_text())
if doc['state'] in {'queued', 'running'}:
self.write({**doc, 'state': 'error', 'message': 'Расчёт прерван перезапуском сервера.'})
def directory(self, run_id):
return self.root / str(UUID(run_id))
def write(self, doc):
directory = self.directory(doc['id']); directory.mkdir(exist_ok=True)
candidate = directory / 'report.tmp'
candidate.write_text(json.dumps(doc, allow_nan=False))
os.replace(candidate, directory / 'report.json')
def get(self, run_id):
path = self.directory(run_id) / 'report.json'
if not path.is_file():
raise KeyError(run_id)
return json.loads(path.read_text())
def list(self, draft_id):
self.drafts.get(draft_id)
items = [json.loads(p.read_text()) for p in self.root.glob('*/report.json')]
return sorted([{'id': d['id'], 'created_at_utc': d['created_at_utc'], 'state': d['state'],
'revision': d['revision'], 'query_session_id': d['request']['session_id']}
for d in items if d['draft_id'] == draft_id], key=lambda d: d['created_at_utc'], reverse=True)[:50]
def start(self, draft_id, request):
if not self.lock.acquire(blocking=False):
raise ValueError('Другой расчёт совмещения ещё выполняется.')
try:
draft = self.drafts.get(draft_id)
if draft['revision'] != request['revision']:
raise DraftConflict('Черновик изменён. Откройте сохранённую версию.')
route = draft['route']
if not 3 <= route['length_m'] <= 40:
raise ValueError('Для совмещения выберите маршрут длиной от 3 до 40 м.')
query = self.drafts.sources.bound(request['session_id'], request['generation'])
start, end = request['start_index'], request['end_index']
if not 0 <= start < end < len(query['poses']):
raise ValueError('Некорректный интервал повторной записи.')
distance = query['poses'][end]['distance_m'] - query['poses'][start]['distance_m']
if not 3 <= distance <= 40:
raise ValueError('Для повторного прохода выберите участок длиной от 3 до 40 м.')
same = draft['zone']['session_id'] == request['session_id']
if same and max(start, route['start_index']) <= min(end, route['end_index']):
raise ValueError('Эталонный и проверочный участки одной записи не должны пересекаться.')
doc = {'schema_version': 'missioncore.registration-run/v1', 'id': str(uuid4()),
'draft_id': draft_id, 'revision': draft['revision'], 'draft': draft,
'request': request, 'state': 'queued', 'created_at_utc': utc_now_iso(),
'evidence_relation': 'same_recording' if same else 'different_recordings',
'localization_confirmed': False, 'vehicle_control': False}
self.write(doc)
self.executor.submit(self.calculate, doc)
return doc
except Exception:
self.lock.release()
raise
def calculate(self, doc):
started = time.monotonic_ns()
directory = self.directory(doc['id'])
try:
doc = {**doc, 'state': 'running', 'started_at_utc': utc_now_iso(), 'started_monotonic_ns': started}
self.write(doc)
draft, request = doc['draft'], doc['request']
route, zone = draft['route'], draft['zone']
sources = self.drafts.sources
doc['progress_label'] = 'Подготовка эталонного участка'; self.write(doc)
reference, ref_meta = sources.submap(zone['session_id'], zone['generation'], route['start_index'], route['end_index'])
doc['progress_label'] = 'Подготовка повторного прохода'; self.write(doc)
query, query_meta = sources.submap(request['session_id'], request['generation'], request['start_index'], request['end_index'])
qdoc = sources.bound(request['session_id'], request['generation'])
ref_path = np.array([p['position'] for p in route['points']])
query_path = np.array([p['position'] for p in qdoc['poses'][request['start_index']:request['end_index']+1]])
initial = path_hint(ref_path, query_path)
doc['progress_label'] = 'Расчёт совмещения'; self.write(doc)
result = run_registration(directory, reference, query, initial)
doc['progress_label'] = 'Сохранение результата'; self.write(doc)
np.savez_compressed(directory / 'clouds.npz', reference=reference, query=query,
reference_path=ref_path, query_path=query_path)
write_scene(directory / 'scene.rrd', doc['id'], reference, query, result, ref_path, query_path)
artifacts = {name: hashlib.sha256((directory / name).read_bytes()).hexdigest()
for name in ['clouds.npz', 'scene.rrd', 'registration-input.npz', 'registration-result.json']}
doc.update(state='ready', result=result, reference=ref_meta, query=query_meta,
hint='route-entry-and-travel-heading', artifacts=artifacts,
scene_url='/api/v1/mission-planner/registration-runs/'+doc['id']+'/scene.rrd',
runtime={'system': platform.system(), 'machine': platform.machine(), 'python': platform.python_version()})
except Exception as exc:
# Details stay in private evidence; paths and native errors do not enter the UI.
(directory / 'failure.txt').write_text(f'{type(exc).__name__}: {exc}')
doc.update(state='error', message=str(exc) if isinstance(exc, ValueError)
else 'Не удалось завершить совмещение. Исходные записи сохранены.')
finally:
doc.update(finished_at_utc=utc_now_iso(), elapsed_seconds=(time.monotonic_ns()-started)/1e9)
try:
self.write(doc)
finally:
self.lock.release()
def close(self):
self.executor.shutdown(wait=True, cancel_futures=True)
+26
View File
@@ -0,0 +1,26 @@
"""Static spatial evidence for one immutable registration run."""
import numpy as np
import rerun as rr
from rerun import blueprint as rrb
from .registration import transform
from .registration_colors import query_colors
def write_scene(path, run_id, reference, query, result, reference_path, query_path):
recording = rr.RecordingStream('missioncore-registration', recording_id=run_id)
recording.save(path)
try:
recording.log('world', rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
for name, xyz, color in [('reference', reference, [140, 140, 140]),
('query', transform(query, np.array(result['T_reference_query'])), query_colors(query, result))]:
recording.log('world/'+name, rr.Points3D(xyz, colors=color, radii=rr.Radius.ui_points(1.5)), static=True)
for name, xyz, color in [('reference_path', reference_path, [120, 160, 255]),
('query_path', transform(query_path, np.array(result['T_reference_query'])), [255, 190, 70])]:
recording.log('world/'+name, rr.LineStrips3D([xyz], colors=color), static=True)
recording.send_blueprint(rrb.Blueprint(
rrb.Spatial3DView(name='Совмещение проходов', origin='/world', contents=['/world/**'],
background=[9, 10, 12, 255]),
auto_layout=False, auto_views=False, collapse_panels=True))
recording.flush()
finally:
recording.disconnect()
@@ -0,0 +1,36 @@
"""Short-lived numeric worker: native library lifetime is separate from the API."""
from __future__ import annotations
import json
import os
import subprocess
import sys
from pathlib import Path
import numpy as np
def run_registration(directory, reference, query, initial):
source, destination = directory / 'registration-input.npz', directory / 'registration-result.json'
np.savez_compressed(source, reference=reference, query=query, initial=initial)
environment = {**os.environ, 'OMP_NUM_THREADS': '1', 'OPENBLAS_NUM_THREADS': '1',
'VECLIB_MAXIMUM_THREADS': '1'}
with (directory / 'calculation.log').open('wb') as log:
try:
subprocess.run([sys.executable, '-m', 'k1link.missions.registration_worker',
str(source), str(destination)], env=environment,
stdout=log, stderr=log, timeout=30, check=True)
except subprocess.TimeoutExpired as exc:
raise ValueError('Превышено время совмещения. Выберите более короткий участок.') from exc
except subprocess.CalledProcessError as exc:
raise ValueError('Расчёт совмещения завершился с ошибкой. Исходные записи сохранены.') from exc
return json.loads(destination.read_text())
def main():
from .registration import register
with np.load(Path(sys.argv[1]), allow_pickle=False) as data:
result = register(data['reference'], data['query'], data['initial'])
Path(sys.argv[2]).write_text(json.dumps(result, allow_nan=False))
if __name__ == '__main__':
main()
+25
View File
@@ -0,0 +1,25 @@
"""Explicit laboratory receipt loss; surviving events are never retimed."""
import math
def drop_receipts(events, start_s, end_s, audit):
if not all(math.isfinite(x) for x in (start_s, end_s)) or not 0 < start_s < end_s <= 120:
raise ValueError("Invalid bounded receipt-loss interval.")
audit.update(version="receipt-drop/v1", interval_s=[start_s, end_s], dropped=[])
origin = None
for event in events:
if origin is None:
origin = event.monotonic_ns
elapsed = (event.monotonic_ns - origin) / 1e9
if start_s <= elapsed < end_s:
audit["dropped"].append(
dict(
sequence=event.sequence,
kind=event.kind,
time_s=elapsed,
monotonic_ns=event.monotonic_ns,
)
)
else:
yield event
+721
View File
@@ -0,0 +1,721 @@
"""Staged stationary localisation before the normal fresh-data tracking gate.
A known start is the reliable laboratory path, so it first receives a dense
multi-start fit. Only its honest rejection permits retrieval over the entire
selected route. That preserves a repeatable start while retaining an auditable
recovery path for a restarted rover that must look for *where it is*.
Neither stage grants tracking or vehicle authority: both only produce a
provisional hypothesis for the separate, disjoint fresh-data gate.
"""
from __future__ import annotations
import math
import time
from copy import deepcopy
from dataclasses import dataclass
from itertools import product
import numpy as np
from .entry_acquisition import acquire_entry
from .observation_profiles import TRACKING_INPUT
from .reference_window import reference_window
from .registration import POLICY as TRACKING_POLICY
from .registration import PreparedReference, angle_deg, cloud, rigid, transform
from .stationary_entry import STATIONARY_POLICY
ROUTE_RELOCALIZATION_POLICY = dict(
version="route-relocalization/v6",
scope="selected-route",
strategy="dense-start-first-then-route-recovery/v1",
# Local geometry is independent of the 80-m presentation envelope.
query_radius_m=TRACKING_INPUT["radius_m"],
anchor_spacing_m=5.0,
spatial_cell_m=10.0,
# Candidate retrieval stays local and distinctive. The chosen candidate is
# then matched against the high-resolution local tracking footprint.
descriptor_context_m=28.0,
descriptor_radial_bins=7,
descriptor_height_bins=6,
descriptor_height_low_m=-4.0,
descriptor_height_high_m=8.0,
polar_angle_bins=24,
# A batch controls scheduling, never eligibility. Ranking must not discard
# the real place merely because a coarse descriptor prefers an endpoint.
candidate_batch_size=6,
yaw_candidates_per_place=3,
yaw_step_deg=30.0,
target_context_margin_m=12.0,
target_maximum_points=None,
descriptor_voxel_m=0.5,
cluster_position_m=0.75,
cluster_rotation_deg=8.0,
ambiguity_overlap_margin=0.05,
ambiguity_rmse_margin_m=0.03,
# Keep the stationary prefix younger than the bootstrap's 40-s source-age
# fence. A late exhaustive calculation is an explicit incomplete search,
# never a stale provisional position.
deadline_s=30.0,
maximum_search_wall_s=35.0,
# This is a numerical convergence envelope, not an operator start-radius
# admission rule. Reaching its wall deadline is reported as incomplete.
registration_policy={
**TRACKING_POLICY,
"version": "route-relocalization-gicp/v1",
"maximum_correction_m": 25.0,
"maximum_correction_deg": 180.0,
},
)
def _valid_path(path):
path = np.asarray(path, dtype=float)
if path.ndim != 2 or path.shape[1] != 3 or len(path) < 2 or not np.isfinite(path).all():
raise ValueError("Для поиска по маршруту нужен конечный маршрут минимум из двух точек.")
lengths = np.linalg.norm(np.diff(path, axis=0), axis=1)
if not np.isfinite(lengths).all() or float(lengths.sum()) <= 0:
raise ValueError("Маршрут не содержит достаточной геометрии для поиска.")
return path, lengths
def route_reference_cloud(value):
"""Validate the complete route atlas without applying GICP's target cap.
A selected kilometre route is not one target: it is indexed here and only a
local, separately checked target is handed to GICP later.
"""
points = np.ascontiguousarray(value, dtype=np.float64)
if (
points.ndim != 2
or points.shape[1] != 3
or len(points) < 300
or not np.isfinite(points).all()
or np.abs(points).max() > 100_000
):
raise ValueError("Полная карта маршрута содержит недостаточно конечных точек в метрах.")
return points
def route_anchors(path, *, spacing_m=ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]):
"""Resample the complete path; no endpoint or intermediate segment is skipped."""
if not 0 < spacing_m <= 25:
raise ValueError("Некорректный шаг индекса маршрута.")
path, lengths = _valid_path(path)
cumulative = np.r_[0.0, np.cumsum(lengths)]
distances = np.r_[np.arange(0.0, cumulative[-1], spacing_m), cumulative[-1]]
positions = []
for distance in distances:
segment = min(
int(np.searchsorted(cumulative, distance, side="right") - 1), len(lengths) - 1
)
fraction = (distance - cumulative[segment]) / lengths[segment]
positions.append(path[segment] + fraction * (path[segment + 1] - path[segment]))
return np.asarray(positions), distances
def _voxel(points, *, voxel_m):
if len(points) == 0:
return points
_, index = np.unique(np.floor(points / voxel_m).astype(np.int64), axis=0, return_index=True)
return points[np.sort(index)]
class ReferenceGrid:
"""Read-only spatial index for a full route map.
It prevents every atlas anchor from scanning every point in a kilometre
route. The grid is local to one isolated search process and is never
reused as a mutable tracking map.
"""
def __init__(self, reference, *, cell_m=ROUTE_RELOCALIZATION_POLICY["spatial_cell_m"]):
if not 1.0 <= cell_m <= 25.0:
raise ValueError("Некорректный размер ячейки карты маршрута.")
self.reference = route_reference_cloud(reference)
self.cell_m = float(cell_m)
cells = np.floor(self.reference / self.cell_m).astype(np.int64)
keys, inverse = np.unique(cells, axis=0, return_inverse=True)
order = np.argsort(inverse, kind="stable")
counts = np.bincount(inverse, minlength=len(keys))
boundaries = np.r_[0, np.cumsum(counts)]
self.ordered = self.reference[order]
self.slices = {
tuple(key): (int(boundaries[index]), int(boundaries[index + 1]))
for index, key in enumerate(keys)
}
def crop(self, center, radius_m):
center = np.asarray(center, dtype=float).reshape(3)
if not np.isfinite(center).all() or not 0 < radius_m <= 100:
raise ValueError("Некорректная локальная область маршрута.")
lower = np.floor((center - radius_m) / self.cell_m).astype(int)
upper = np.floor((center + radius_m) / self.cell_m).astype(int)
pieces = []
ranges = tuple(range(first, last + 1) for first, last in zip(lower, upper, strict=True))
for key in product(*ranges):
bounds = self.slices.get(key)
if bounds is not None:
pieces.append(self.ordered[slice(*bounds)])
if not pieces:
return np.empty((0, 3), dtype=float)
points = np.concatenate(pieces)
return points[np.linalg.norm(points - center, axis=1) <= radius_m]
def local_submap(reference, center, radius_m, *, maximum_points):
"""Radial crop. Production verification preserves the source resolution.
An explicit point budget is available only to descriptor/test callers.
It is never a density threshold for declaring tracking lost.
"""
if isinstance(reference, ReferenceGrid):
points = reference.crop(center, radius_m)
else:
full = route_reference_cloud(reference)
points = full[np.linalg.norm(full - center, axis=1) <= radius_m]
if maximum_points is not None and len(points) > maximum_points:
original = points
voxel_m = ROUTE_RELOCALIZATION_POLICY["descriptor_voxel_m"]
while len(points) > maximum_points:
reduced = _voxel(original, voxel_m=voxel_m)
if voxel_m > radius_m * 2.0:
return np.empty((0, 3), dtype=float)
points = reduced
voxel_m *= 2.0
if len(points) < 300:
return np.empty((0, 3), dtype=float)
return points
def radial_height_descriptor(points, center, *, policy=ROUTE_RELOCALIZATION_POLICY):
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
radial = np.linalg.norm(relative[:, :2], axis=1)
histogram, _ = np.histogramdd(
np.column_stack([radial, relative[:, 2]]),
bins=(
policy["descriptor_radial_bins"],
policy["descriptor_height_bins"],
),
range=(
(0.0, policy["descriptor_context_m"]),
(policy["descriptor_height_low_m"], policy["descriptor_height_high_m"]),
),
)
flat = histogram.reshape(-1)
norm = float(np.linalg.norm(flat))
return flat / norm if norm else flat
def polar_descriptor(
points,
center,
*,
bins=ROUTE_RELOCALIZATION_POLICY["polar_angle_bins"],
context_m=ROUTE_RELOCALIZATION_POLICY["descriptor_context_m"],
):
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
angle = np.mod(np.arctan2(relative[:, 1], relative[:, 0]), 2 * math.pi)
radial = np.linalg.norm(relative[:, :2], axis=1)
# Four equally sized radial rings prevent one distant, unrelated wall
# from deciding yaw while retaining the full declared context.
rings = np.minimum((radial / (context_m / 4.0)).astype(int), 3)
output = np.zeros((4, bins), dtype=float)
angles = np.minimum((angle / (2 * math.pi) * bins).astype(int), bins - 1)
np.add.at(output, (rings, angles), 1.0)
norm = float(np.linalg.norm(output))
return output / norm if norm else output
def _yaw_candidates(query, target, query_center, target_center, *, policy):
q = polar_descriptor(
query,
query_center,
bins=policy["polar_angle_bins"],
context_m=policy["descriptor_context_m"],
)
t = polar_descriptor(
target,
target_center,
bins=policy["polar_angle_bins"],
context_m=policy["descriptor_context_m"],
)
candidates = []
for yaw in np.arange(0.0, 360.0, policy["yaw_step_deg"]):
shift = int(round(yaw / 360.0 * policy["polar_angle_bins"]))
candidates.append((float(np.linalg.norm(t - np.roll(q, shift, axis=1))), float(yaw)))
return [yaw for _, yaw in sorted(candidates)[: policy["yaw_candidates_per_place"]]]
@dataclass(frozen=True)
class RouteCandidate:
index: int
position: np.ndarray
progress_m: float
descriptor_distance: float
def rank_route_candidates(
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None
):
"""Rank every resampled route position against the stationary query cloud."""
reference, query = route_reference_cloud(reference), cloud(query)
grid = grid or ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
anchors, progress = route_anchors(reference_path, spacing_m=policy["anchor_spacing_m"])
query_center = np.median(query, axis=0)
query_descriptor = radial_height_descriptor(query, query_center, policy=policy)
ranked = []
for index, (position, distance) in enumerate(zip(anchors, progress, strict=True)):
target = local_submap(
grid,
position,
policy["descriptor_context_m"],
maximum_points=policy["target_maximum_points"],
)
if len(target) < 300:
continue
descriptor = radial_height_descriptor(target, np.median(target, axis=0), policy=policy)
ranked.append(
RouteCandidate(
index=index,
position=position,
progress_m=float(distance),
descriptor_distance=float(np.linalg.norm(query_descriptor - descriptor)),
)
)
ranked.sort(key=lambda candidate: (candidate.descriptor_distance, candidate.index))
return ranked, dict(
route_anchor_count=len(anchors),
descriptor_covered_anchor_count=len(ranked),
descriptor_candidate_count=len(ranked),
descriptor_scope="entire-selected-route",
)
def _seed(query_center, target_center, yaw_deg):
angle = math.radians(yaw_deg)
rotation = np.array(
[
[math.cos(angle), -math.sin(angle), 0.0],
[math.sin(angle), math.cos(angle), 0.0],
[0, 0, 1],
],
dtype=float,
)
matrix = np.eye(4)
matrix[:3, :3] = rotation
matrix[:3, 3] = np.asarray(target_center) - rotation @ np.asarray(query_center)
return matrix
def _rejected_attempt(message, initial):
return dict(
status="rejected",
reasons=[message],
T_reference_query=rigid(initial).tolist(),
initial_T_reference_query=rigid(initial).tolist(),
overlap=0.0,
inlier_rmse_m=None,
matched_query_indices=[],
localization_confirmed=False,
vehicle_control=False,
registration_seconds=0.0,
)
def _distance(first, second, query_entry):
a, b = np.asarray(first), np.asarray(second)
position = float(
np.linalg.norm(
transform(np.asarray(query_entry).reshape(1, 3), a)
- transform(np.asarray(query_entry).reshape(1, 3), b)
)
)
return position, angle_deg(a[:3, :3] @ b[:3, :3].T)
def choose_route_location(attempts, query_entry, *, complete, policy=ROUTE_RELOCALIZATION_POLICY):
"""Accept one well-separated route location, or expose why we did not."""
candidates, diagnostics = [], []
for attempt in attempts:
result = attempt["result"]
diagnostic = {k: v for k, v in attempt.items() if k != "result"}
diagnostic["result"] = {k: v for k, v in result.items() if k != "matched_query_indices"}
diagnostics.append(diagnostic)
if result["status"] == "candidate":
candidates.append(attempt)
candidates.sort(
key=lambda attempt: (
-attempt["result"]["overlap"],
attempt["result"]["inlier_rmse_m"],
attempt["candidate"]["index"],
attempt["yaw_deg"],
)
)
clusters = []
for attempt in candidates:
for cluster in clusters:
if all(
_distance(
attempt["result"]["T_reference_query"],
other["result"]["T_reference_query"],
query_entry,
)[0]
<= policy["cluster_position_m"]
and _distance(
attempt["result"]["T_reference_query"],
other["result"]["T_reference_query"],
query_entry,
)[1]
<= policy["cluster_rotation_deg"]
for other in cluster
):
cluster.append(attempt)
break
else:
clusters.append([attempt])
# Keep the remaining distinct hypotheses for disjoint fresh confirmation.
# Their ambiguity is evaluated again relative to the remaining queue, not
# inherited from the best hypothesis after it has been rejected.
queue = []
for index, cluster in enumerate(clusters):
best = cluster[0]
ambiguous = any(
alternative[0]["result"]["overlap"]
>= best["result"]["overlap"] - policy["ambiguity_overlap_margin"]
and alternative[0]["result"]["inlier_rmse_m"]
<= best["result"]["inlier_rmse_m"] + policy["ambiguity_rmse_margin_m"]
for alternative in clusters[index + 1 :]
)
queue.append(dict(
candidate_index=best["candidate"]["index"],
route_progress_m=best["candidate"]["progress_m"],
T_reference_query=best["result"]["T_reference_query"],
overlap=best["result"]["overlap"],
inlier_rmse_m=best["result"]["inlier_rmse_m"],
ambiguous=ambiguous,
))
reason = None
if not complete:
reason = "incomplete-route-search"
elif not clusters:
reason = "no-route-location"
elif queue[0]["ambiguous"]:
# Distinctness comes from fitted SE(3), not the retrieval anchor: two
# seeds at one anchor can converge to different places or directions.
reason = "ambiguous-route-location"
selected = (
dict(clusters[0][0]["result"])
if clusters
else _rejected_attempt(
"Ни один кандидат маршрута не прошёл геометрическую проверку.", np.eye(4)
)
)
selected.update(
status="rejected" if reason else "candidate",
reasons=[reason] if reason else [],
matched_query_indices=[] if reason else selected.get("matched_query_indices", []),
localization_confirmed=False,
vehicle_control=False,
)
selected["initialization"] = dict(
policy=policy,
scope=policy["scope"],
complete=complete,
reason=reason,
expected_attempts=len(attempts),
attempts=diagnostics,
candidate_queue=queue if complete else [],
selected_candidate_index=clusters[0][0]["candidate"]["index"] if clusters else None,
selected_route_progress_m=(clusters[0][0]["candidate"]["progress_m"] if clusters else None),
clusters=[
dict(
candidate_indices=sorted({item["candidate"]["index"] for item in cluster}),
route_progress_m=cluster[0]["candidate"]["progress_m"],
support=len(cluster),
overlap=cluster[0]["result"]["overlap"],
rmse_m=cluster[0]["result"]["inlier_rmse_m"],
)
for cluster in clusters
],
)
selected["registration_seconds"] = sum(
item["result"].get("registration_seconds", 0.0) for item in attempts
)
return selected
def relocalize_route(
reference,
reference_path,
query,
query_entry,
*,
clock=time.monotonic,
policy=ROUTE_RELOCALIZATION_POLICY,
):
"""Run complete candidate retrieval and qualification against a selected route."""
started = clock()
reference, query = route_reference_cloud(reference), cloud(query)
query_entry = np.asarray(query_entry, dtype=float).reshape(3)
grid = ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
ranked, coverage = rank_route_candidates(
reference, reference_path, query, policy=policy, grid=grid
)
attempts, evaluated, batches = [], [], []
query_center = np.median(query, axis=0)
radius = max(
policy["descriptor_context_m"],
float(np.linalg.norm(query - query_center, axis=1).max())
+ policy["target_context_margin_m"],
)
expected = len(ranked) * policy["yaw_candidates_per_place"]
batch_size = policy["candidate_batch_size"]
for candidate in ranked:
if clock() - started > policy["deadline_s"]:
break
if len(evaluated) % batch_size == 0:
batches.append([])
target = local_submap(
grid, candidate.position, radius, maximum_points=policy["target_maximum_points"]
)
if len(target) < 300:
# Descriptor-admitted geometry unexpectedly disappeared. Do not
# call this a complete negative search or silently skip the place.
break
target_center = np.median(target, axis=0)
count_before = len(attempts)
prepared = None
for yaw_deg in _yaw_candidates(
query, target, query_center, target_center, policy=policy
):
if clock() - started > policy["deadline_s"]:
break
initial = _seed(query_center, target_center, yaw_deg)
try:
# Target preprocessing is independent of yaw. Keep one tree
# per place; all seeds and all eligibility checks stay intact.
if prepared is None:
prepared = PreparedReference(target)
result = prepared.register(
query, initial, policy=policy["registration_policy"]
)
except ValueError as exc:
result = _rejected_attempt(str(exc), initial)
attempts.append(
dict(
candidate=dict(
index=candidate.index,
position=candidate.position.tolist(),
progress_m=candidate.progress_m,
descriptor_distance=candidate.descriptor_distance,
),
yaw_deg=yaw_deg,
result=result,
)
)
if len(attempts) - count_before != policy["yaw_candidates_per_place"]:
break
evaluated.append(candidate.index)
batches[-1].append(candidate.index)
complete = len(evaluated) == len(ranked) and clock() - started <= policy["deadline_s"]
result = choose_route_location(attempts, query_entry, complete=complete, policy=policy)
result["initialization"].update(
coverage,
elapsed_s=clock() - started,
expected_attempts=expected,
evaluated_candidate_indices=evaluated,
remaining_candidate_indices=[c.index for c in ranked if c.index not in evaluated],
candidate_batches=batches,
candidate_queue_exhausted=complete,
)
return result
def _route_start_context(reference, reference_path, query, query_entry, reference_position=None):
"""Prepare the established dense start target without shrinking the scene.
The selected route's first point is still a valuable, explicitly chosen
laboratory datum. After loss, the last confirmed place takes its role.
This target preserves the precise local map representation used
by the successful start-area runs instead of voxelising a broad whole-route
crop before GICP has a chance to converge.
"""
reference, query = route_reference_cloud(reference), cloud(query)
path, _lengths = _valid_path(reference_path)
entry = np.asarray(query_entry, dtype=float).reshape(3)
initial = np.eye(4)
anchor = path[0] if reference_position is None else np.asarray(reference_position, dtype=float)
if anchor.shape != (3,) or not np.isfinite(anchor).all():
raise ValueError("Некорректная область восстановления привязки.")
initial[:3, 3] = anchor - entry
forward = next(
(point - path[0] for point in path[1:] if np.linalg.norm((point - path[0])[:2]) >= 3),
None,
)
if forward is None:
raise ValueError("Reference lacks a usable route basis.")
target, window = reference_window(
reference,
dict(points=query, path=np.asarray([entry])),
initial,
initializing=True,
)
return target, query, initial, entry, forward, window
def _stage_attempts(stage, initialization):
"""Keep every fit auditable while retaining its stage of the hybrid search."""
return [dict(stage=stage, **attempt) for attempt in initialization.get("attempts", [])]
def _hybrid_initialization(policy, start_result, route_result=None):
"""Normalize two numerical stages for StationaryBootstrap's strict gate."""
start = start_result["initialization"]
attempts = _stage_attempts("dense-start", start)
expected = start.get("expected_attempts", len(attempts))
stages = [
dict(
name="dense-start",
status=start_result["status"],
reason=start.get("reason"),
complete=start.get("complete", False),
elapsed_s=start.get("elapsed_s"),
expected_attempts=expected,
target_window=start.get("target_window"),
reference_position=start.get("reference_position"),
)
]
selected = dict(
selected_candidate_index=0 if start_result["status"] == "candidate" else None,
selected_route_progress_m=start.get("route_progress_m", 0.0)
if start_result["status"] == "candidate"
else None,
)
reason = start.get("reason")
complete = bool(start.get("complete"))
if route_result is not None:
route = route_result["initialization"]
attempts.extend(_stage_attempts("route-recovery", route))
expected += route.get("expected_attempts", len(route.get("attempts", [])))
stages.append(
dict(
name="route-recovery",
status=route_result["status"],
reason=route.get("reason"),
complete=route.get("complete", False),
elapsed_s=route.get("elapsed_s"),
expected_attempts=len(route.get("attempts", [])),
descriptor_scope=route.get("descriptor_scope"),
expected_attempts_total=route.get("expected_attempts"),
evaluated_candidate_indices=route.get("evaluated_candidate_indices"),
remaining_candidate_indices=route.get("remaining_candidate_indices"),
candidate_batches=route.get("candidate_batches"),
)
)
selected = dict(
selected_candidate_index=route.get("selected_candidate_index"),
selected_route_progress_m=route.get("selected_route_progress_m"),
candidate_queue=route.get("candidate_queue", []),
)
reason = route.get("reason")
complete = bool(route.get("complete"))
return dict(
policy=policy,
scope=policy["scope"],
strategy=policy["strategy"],
complete=complete,
reason=reason,
expected_attempts=expected,
attempts=attempts,
stages=stages,
**selected,
)
def relocalize_start_then_route(
reference,
reference_path,
query,
query_entry,
*,
clock=time.monotonic,
policy=ROUTE_RELOCALIZATION_POLICY,
reference_position=None,
route_only=False,
):
"""Use the proven start-area fit first, then a bounded route fallback.
This is deliberately not a looser acceptance rule. The dense start fit
runs every stationary multi-start seed against its high-resolution local
target. Only an honest rejection enters whole-route retrieval, whose
result remains provisional until the existing fresh-data gate confirms it.
"""
started = clock()
if route_only:
# A dense-start prior failed fresh confirmation. Recollect first, then
# search the route without repeatedly retrying that unconfirmed start.
return relocalize_route(reference, reference_path, query, query_entry,
clock=clock, policy=policy)
target, query, initial, entry, forward, window = _route_start_context(
reference, reference_path, query, query_entry, reference_position
)
start_result = acquire_entry(
target,
query,
initial,
entry,
forward,
clock=clock,
policy=STATIONARY_POLICY,
)
start_result["initialization"].update(
scope=policy["scope"],
target_window=window,
query_radius_m=policy["query_radius_m"],
reference_position=(np.asarray(query_entry) + initial[:3, 3]).tolist(),
route_progress_m=float(
np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(reference_path, axis=0), axis=1))][
np.argmin(
np.linalg.norm(
np.asarray(reference_path) - (np.asarray(query_entry) + initial[:3, 3]),
axis=1,
)
)
]
),
)
if start_result["status"] == "candidate":
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
if not start_result["initialization"].get("complete"):
# Compute exhaustion is not evidence that this place did not match.
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
# A failed standard start may still be a valid mid-route or recovery
# position. Give retrieval only the fresh-prefix time remaining: it must
# never turn a late calculation into an apparently usable prior.
remaining = policy["maximum_search_wall_s"] - (clock() - started)
if remaining <= 0:
route_result = choose_route_location([], entry, complete=False, policy=policy)
route_result["initialization"].update(
elapsed_s=0.0, worker_timeout_reason="start-stage-timeout"
)
else:
recovery_policy = deepcopy(policy)
recovery_policy["deadline_s"] = min(policy["deadline_s"], remaining)
route_result = relocalize_route(
reference,
reference_path,
query,
entry,
clock=clock,
policy=recovery_policy,
)
route_result["initialization"] = _hybrid_initialization(policy, start_result, route_result)
route_result["registration_seconds"] = start_result.get(
"registration_seconds", 0.0
) + route_result.get("registration_seconds", 0.0)
return route_result
@@ -0,0 +1,99 @@
"""Isolated CPU child for complete selected-route relocalisation."""
import json
import os
import subprocess
import sys
from pathlib import Path
import numpy as np
from .route_relocalization import ROUTE_RELOCALIZATION_POLICY
def incomplete_result(reason):
identity = np.eye(4).tolist()
return dict(
status="rejected",
reasons=[reason],
T_reference_query=identity,
initial_T_reference_query=identity,
matched_query_indices=[],
overlap=0.0,
inlier_rmse_m=None,
localization_confirmed=False,
vehicle_control=False,
initialization=dict(
policy=ROUTE_RELOCALIZATION_POLICY,
scope="selected-route",
complete=False,
reason="incomplete-route-search",
expected_attempts=0,
attempts=[],
worker_timeout_reason=reason,
),
)
def run_route_relocalization(
directory, reference, reference_path, query, query_entry, *, reference_position=None,
route_only=False,
):
source = directory / "route-relocalization-input.npz"
destination = directory / "route-relocalization-result.json"
np.savez_compressed(
source,
reference=reference,
reference_path=reference_path,
query=query,
query_entry=query_entry,
route_only=route_only,
**({"reference_position": reference_position} if reference_position is not None else {}),
)
environment = {
**os.environ,
"OMP_NUM_THREADS": "1",
"OPENBLAS_NUM_THREADS": "1",
"VECLIB_MAXIMUM_THREADS": "1",
}
with (directory / "calculation.log").open("wb") as log:
try:
subprocess.run(
[
sys.executable,
"-m",
"k1link.missions.route_relocalization_worker",
str(source),
str(destination),
],
env=environment,
stdout=log,
stderr=log,
timeout=ROUTE_RELOCALIZATION_POLICY["maximum_search_wall_s"] + 5,
check=True,
)
except subprocess.TimeoutExpired:
# A process timeout says nothing about whether the scanner is at a
# known place. Return a normal, persisted incomplete-search result
# so the UI can distinguish it from a geometric rejection.
destination.write_text(json.dumps(incomplete_result("worker-timeout"), allow_nan=False))
return json.loads(destination.read_text())
def main():
from .route_relocalization import relocalize_start_then_route
with np.load(Path(sys.argv[1]), allow_pickle=False) as data:
result = relocalize_start_then_route(
data["reference"],
data["reference_path"],
data["query"],
data["query_entry"],
reference_position=data.get("reference_position"),
route_only=bool(data.get("route_only", False)),
)
Path(sys.argv[2]).write_text(json.dumps(result, allow_nan=False))
if __name__ == "__main__":
main()
+171
View File
@@ -0,0 +1,171 @@
"""Bounded, immutable planning-source cache contributed by device plugins."""
from __future__ import annotations
import hashlib
import json
import os
import shutil
import threading
from contextlib import contextmanager
from uuid import uuid4
from k1link.sessions.recording import (
RecordingMaterializationCancelled,
_stage_replay_prefix,
_validate_source,
_validate_source_state,
_validated_artifact_digests,
)
SCHEMA = "missioncore.planning-source/v1"
class PlanningSources:
def __init__(self, store, exporters, submap_extractors=None, scene_submap_extractors=None):
self.store = store
self.exporters = exporters
self.submap_extractors = submap_extractors or {}
self.scene_submap_extractors = scene_submap_extractors or {}
self.root = store.data_dir / "planning-sources"
self.root.mkdir(parents=True, exist_ok=True)
self.lock = threading.Lock()
self._scene_cache = None
def get(self, session_id: str) -> dict:
detail = self.store.get_session(session_id)
exporter = self.exporters.get(detail.plugin_id)
if not detail.summary.replayable or detail.summary.lab is not None or exporter is None:
raise ValueError("В этой записи нет поддерживаемой пространственной зоны.")
source = _validate_source(self.store.prepare_replay(session_id))
generation = hashlib.sha256(
json.dumps([SCHEMA, session_id, source.identity], default=str).encode()
).hexdigest()
with self.lock:
path = self.root / (generation + ".json")
if path.is_file() and path.stat().st_size < 32 * 1024 * 1024:
try:
doc = json.loads(path.read_text())
if doc["schema_version"] == SCHEMA and doc["generation"] == generation:
return doc
except (ValueError, KeyError):
pass
stage = None
candidate = self.root / ("." + uuid4().hex + ".json")
try:
digests = _validated_artifact_digests(source)
stage, primary, _ = _stage_replay_prefix(self.root, source)
exporter(primary, candidate)
if candidate.stat().st_size > 30 * 1024 * 1024:
raise ValueError("Траектория превышает размер поддерживаемой зоны.")
result = json.loads(candidate.read_text())
if source.identity != _validate_source_state(
source
).identity or digests != _validated_artifact_digests(source):
raise ValueError("Исходная запись изменилась во время подготовки.")
doc = {
**result,
"schema_version": SCHEMA,
"session_id": session_id,
"label": detail.as_dict()["display_name"],
"generation": generation,
"units": "m",
"frame_id": "session/" + session_id,
"source_digests": digests,
}
candidate.write_text(json.dumps(doc, allow_nan=False))
os.replace(candidate, path)
return doc
finally:
candidate.unlink(missing_ok=True)
if stage is not None:
shutil.rmtree(stage, ignore_errors=True)
def bound(self, session_id: str, generation: str) -> dict:
doc = self.get(session_id)
if doc["generation"] != generation:
raise ValueError("Исходная запись изменилась. Требуется повторный выбор зоны.")
return doc
def verify(self, session_id: str, generation: str) -> dict:
doc = self.bound(session_id, generation)
source = _validate_source(self.store.prepare_replay(session_id))
if _validated_artifact_digests(source) != doc["source_digests"]:
raise ValueError("Контрольные суммы исходной записи изменились.")
return doc
def submap(self, session_id, generation, start, end, *, presentation=False):
with self.prepared_submaps(session_id, generation, presentation=presentation) as extract:
return extract(start, end)
@contextmanager
def prepared_submaps(self, session_id, generation, *, presentation=False, cancel_event=None):
"""One verified private snapshot for all tiles of one map build.
Callers may assemble tiles inside this context, but must not publish
the map until exit has revalidated the original source. Cancellation
and failures discard staging without publishing a partial atlas.
"""
if cancel_event is not None and cancel_event.is_set():
raise InterruptedError("Reference preparation cancelled.")
doc = self.verify(session_id, generation)
detail = self.store.get_session(session_id)
extractors = self.scene_submap_extractors if presentation else self.submap_extractors
extractor = extractors.get(detail.plugin_id)
if extractor is None:
raise ValueError("Эта запись не поддерживает подготовку облака для совмещения.")
source = _validate_source(self.store.prepare_replay(session_id))
stage = None
try:
stage, primary, _ = _stage_replay_prefix(self.root, source, cancel_event=cancel_event)
def extract(start, end):
if cancel_event is not None and cancel_event.is_set():
raise InterruptedError("Reference preparation cancelled.")
points, evidence = extractor(primary, doc, start, end)
return points, {
**evidence,
**{
k: doc[k]
for k in (
"session_id",
"generation",
"label",
"frame_id",
"units",
"source_digests",
)
},
}
yield extract
if cancel_event is not None and cancel_event.is_set():
raise InterruptedError("Reference preparation cancelled.")
if source.identity != _validate_source_state(source).identity:
raise ValueError("Запись изменилась во время подготовки облака.")
self.verify(session_id, generation)
except RecordingMaterializationCancelled as exc:
raise InterruptedError("Reference preparation cancelled.") from exc
finally:
if stage is not None:
shutil.rmtree(stage, ignore_errors=True)
def reference_map(self, session_id, generation, start, end, *, cancel_event=None):
from .reference_map import build_reference_map
return build_reference_map(
self, session_id, generation, start, end, cancel_event=cancel_event
)
def scene_reference_map(self, session_id, generation, start, end, *, cancel_event=None):
from .reference_map import build_reference_map
self.verify(session_id, generation)
key = (session_id, generation, start, end)
if self._scene_cache is not None and self._scene_cache[0] == key:
return self._scene_cache[1]
result = build_reference_map(
self, session_id, generation, start, end, cancel_event=cancel_event, presentation=True
)
self._scene_cache = (key, result) # One display atlas, not an unbounded route cache.
return result
+294
View File
@@ -0,0 +1,294 @@
"""Ranked stationary hypotheses followed by disjoint, fresh geometric checks.
A prior is a hypothesis, never a CausalTracking result. Receipt continuity does
not prove SLAM frame continuity; this remains a laboratory-only protocol.
"""
import numpy as np
from .causal_tracking import CausalTracking
from .live_buffer import LiveCloudBuffer
from .registration import rigid
from .stationary_entry import STATIONARY_POLICY, StationaryPrefix
BOOTSTRAP_POLICY = dict(
version="stationary-fresh-bootstrap/v3",
prefix_seconds=10.0,
maximum_motion_m=0.10,
maximum_search_wall_s=30.0,
maximum_prior_source_age_s=40.0,
maximum_pre_ready_gaps=1,
maximum_pre_validation_gaps=1,
prior_lifetime_s=10.0,
minimum_fresh_span_s=2.0,
check_interval_s=5.0,
maximum_initializations=1,
trials_per_hypothesis=1,
allow_travel_heading_fallback=False,
)
class StationaryBootstrap:
def __init__(
self, reference_path, *, initialization_policy=STATIONARY_POLICY, point_radius_m=20.0
):
self.reference_path = np.asarray(reference_path)
self.point_radius_m = point_radius_m
self.prefix = StationaryPrefix(reference_path, point_radius_m=point_radius_m)
self.initialization_policy = initialization_policy
self.gate = CausalTracking()
self.phase = "collecting"
self.reason = "prefix"
self.identity = None
self.last_event_ns = None
self.last_sequence = -1
self.origin = None
self.segment = 0
self.initialization_sample = None
self.search_started_ns = None
self.ready_ns = None
self.prior = None
self.fresh = None
self.floor_ns = None
self.last_check_ns = None
self.validation_pending = False
self.tracking_established = False
self.candidate_queue = []
self.candidate_trial = 0
self.candidate_index = None
self.dense_start_prior = False
self.retry_route_search = False
def stop(self, reason):
self.prior = None
self.fresh = None
self.validation_pending = False
self.candidate_queue = []
self.retry_route_search = False
self.gate.clear(reason)
self.phase = "lost"
self.reason = reason
def ingest(self, event, segment):
identity = (event.session_id, event.generation)
if self.identity is None:
self.identity, self.origin = identity, event.monotonic_ns
if identity != self.identity:
self.stop("identity-changed")
raise ValueError("Session or generation changed during stationary bootstrap.")
if self.last_event_ns is not None and (
event.monotonic_ns < self.last_event_ns or event.sequence <= self.last_sequence
):
self.stop("source-order-changed")
raise ValueError("Source sequence or receipt clock regressed.")
self.last_event_ns, self.last_sequence = event.monotonic_ns, event.sequence
if segment != self.segment:
# Worker completion is not a data receipt. A gap straddling that
# instant may finish before the first fresh cloud. No validation has
# used the prior yet: start a new continuous window without extending
# its lifetime or allowing more gaps than the original prefix budget.
awaiting_first_cloud = (
self.phase == "refreshing"
and self.prior is not None
and not self.validation_pending
and not self.fresh.events
and 0
<= segment - self.initialization_sample["segment"]
<= BOOTSTRAP_POLICY["maximum_pre_validation_gaps"]
)
if awaiting_first_cloud:
self.segment = segment
self._reset_fresh(self.floor_ns)
elif self.phase in {"refreshing", "validating", "tracking"}:
self.stop("receipt-gap")
self.segment = segment
if self.phase == "collecting":
self.prefix.ingest(event)
elif self.fresh is not None and event.monotonic_ns > self.floor_ns:
self.fresh.ingest(event)
def tick(self, now_ns, segment):
self.gate.tick(now_ns, segment)
if self.phase in {"validating", "tracking"} and self.gate.reason == "stale":
self.stop("stale")
if self.phase == "refreshing" and (
now_ns - self.ready_ns > BOOTSTRAP_POLICY["prior_lifetime_s"] * 1e9
):
self.stop("prior-expired")
def start_search(self, now_ns):
if self.phase != "collecting" or self.origin is None:
return None
if (now_ns - self.origin) / 1e9 < BOOTSTRAP_POLICY["prefix_seconds"]:
return None
sample, initial, forward, meta = self.prefix.freeze()
sample["segment"] = self.segment
self.initialization_sample = sample
self.search_started_ns = now_ns
self.phase, self.reason = "searching", "bounded-entry-search"
return sample, initial, forward, meta
def offer_prior(self, result, now_ns, segment):
if self.phase != "searching":
return dict(accepted=False, reason="inactive-initialization", provisional=False)
sample = self.initialization_sample
age = (now_ns - sample["monotonic_ns"]) / 1e9
initialization = result.get("initialization", {})
reason = None
if result["status"] != "candidate":
reason = {
"incomplete-search": "initialization-incomplete",
"incomplete-route-search": "initialization-incomplete",
"ambiguous-route-location": "initialization-ambiguous",
"no-route-location": "initialization-no-route-location",
}.get(initialization.get("reason"), "initialization-rejected")
elif (
not initialization.get("complete")
or initialization.get("policy") != self.initialization_policy
):
reason = "initialization-incomplete"
elif self.initialization_policy is STATIONARY_POLICY and len(
initialization.get("attempts", [])
) != 108:
# Preserve the strict evidence count for the existing local-start
# protocol and use an explicit dynamic count for route retrieval.
reason = "initialization-incomplete"
elif self.initialization_policy.get("scope") == "selected-route" and (
initialization.get("scope") != "selected-route"
or initialization.get("expected_attempts") != len(initialization.get("attempts", []))
):
reason = "initialization-incomplete"
elif not 0 <= (now_ns - self.search_started_ns) / 1e9 <= self.initialization_policy.get(
"maximum_search_wall_s", BOOTSTRAP_POLICY["maximum_search_wall_s"]
):
reason = "initialization-expired"
elif not 0 <= age <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]:
reason = "prior-source-expired"
elif not 0 <= segment - sample["segment"] <= BOOTSTRAP_POLICY["maximum_pre_ready_gaps"]:
reason = "too-many-receipt-gaps"
if reason:
self.stop(reason)
return dict(accepted=False, reason=reason, age_s=age, provisional=False)
queue = initialization.get("candidate_queue", [])
if queue and queue[0].get("ambiguous", True):
self.stop("initialization-ambiguous")
return dict(accepted=False, reason=self.reason, provisional=False, age_s=age)
if queue and not np.allclose(rigid(queue[0]["T_reference_query"]),
rigid(result["T_reference_query"])):
self.stop("initialization-incomplete")
return dict(accepted=False, reason=self.reason, provisional=False, age_s=age)
self.candidate_queue = [dict(item) for item in queue[1:]]
self.candidate_trial = 1
self.candidate_index = initialization.get("selected_candidate_index")
stages = initialization.get("stages", [])
self.dense_start_prior = bool(stages and len(stages) == 1
and stages[0]["name"] == "dense-start")
self.prior = rigid(result["T_reference_query"]).copy()
self.ready_ns = now_ns
self.segment = segment
self._reset_fresh(now_ns)
self.phase, self.reason = "refreshing", "provisional-prior"
# Deliberately never call gate.accept with the old initialization sample.
return dict(
accepted=False,
reason=self.reason,
age_s=age,
provisional=True,
source_segment=sample["segment"],
validation_segment=segment,
)
def _reset_fresh(self, floor_ns):
self.floor_ns = floor_ns
self.fresh = LiveCloudBuffer(self.reference_path, point_radius_m=self.point_radius_m)
self.fresh.segment = self.segment
def _advance_candidate(self, now_ns):
"""A different hypothesis gets a new receipt fence, never a reused fit.
This is available only before tracking. Once tracking is established,
loss must use the ordinary last-confirmed-place recovery instead.
The original prefix's source-age fence is never extended by retries.
"""
age = (now_ns - self.initialization_sample["monotonic_ns"]) / 1e9
if not 0 <= age <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]:
self.stop("prior-source-expired")
return
candidate = self.candidate_queue.pop(0)
if candidate["ambiguous"]:
self.stop("initialization-ambiguous")
return
self.prior = rigid(candidate["T_reference_query"]).copy()
self.candidate_trial += 1
self.candidate_index = candidate["candidate_index"]
self.ready_ns = now_ns
self.last_check_ns = None
self.validation_pending = False
self.gate.clear("next-candidate")
self._reset_fresh(now_ns)
self.phase, self.reason = "refreshing", "provisional-prior"
def validation(self, now_ns, distance):
self.tick(now_ns, self.segment)
if self.phase not in {"refreshing", "validating", "tracking"} or self.validation_pending:
return None
if self.last_check_ns is not None and (
now_ns - self.last_check_ns < BOOTSTRAP_POLICY["check_interval_s"] * 1e9
):
return None
if not self.fresh.events or (
self.fresh.sample_ns - self.fresh.events[0]["monotonic_ns"]
< BOOTSTRAP_POLICY["minimum_fresh_span_s"] * 1e9
):
return None
sample = self.fresh.snapshot()
if len(sample["points"]) < 300:
return None
seed = self.prior if self.phase == "refreshing" else self.gate.matrix
if seed is None:
self.stop("missing-fresh-seed")
return None
sample["distance"] = distance
sample["fresh_floor_ns"] = self.floor_ns
self.validation_pending = True
self.last_check_ns = now_ns
self.prior = None # One trial per hypothesis; never reseed the failed one.
self._reset_fresh(sample["monotonic_ns"])
return sample, seed.copy()
def accept_fresh(self, result, sample, now_ns, segment):
self.tick(now_ns, segment)
if self.phase not in {"refreshing", "validating", "tracking"}:
return dict(accepted=False, reason=self.reason)
self.validation_pending = False
if not sample["events"] or any(
e["monotonic_ns"] <= sample["fresh_floor_ns"] for e in sample["events"]
):
self.stop("pre-validation-data")
return dict(accepted=False, reason=self.reason)
temporal = self.gate.accept(result, sample, now_ns, segment)
if not temporal["accepted"]:
geometric_rejection = temporal["reason"] in {
"registration-rejected", "inconsistent-candidate"
}
if not self.tracking_established and geometric_rejection and self.candidate_queue:
failed_trial = self.candidate_trial
self._advance_candidate(now_ns)
temporal.update(
rejected_candidate_trial=failed_trial,
next_candidate_trial=self.candidate_trial if self.prior is not None else None,
continuation_reason=self.reason,
)
else:
retry_route_search = (
not self.tracking_established and geometric_rejection and self.dense_start_prior
)
self.stop(temporal["reason"])
self.retry_route_search = retry_route_search
else:
self.phase = "tracking" if self.gate.state == "tracking" else "validating"
self.reason = self.gate.reason
self.tracking_established |= self.phase == "tracking"
if self.tracking_established:
self.candidate_queue = []
return temporal
+94
View File
@@ -0,0 +1,94 @@
"""Prefix-only stationary acquisition shared by live planning and archive probes."""
import numpy as np
from .entry_acquisition import ENTRY_POLICY
from .live_buffer import LiveCloudBuffer
STATIONARY_POLICY = {
**ENTRY_POLICY,
"version": "stationary-entry/v2",
"yaw_degrees": list(range(0, 360, 30)),
"maximum_entry_rotation_deg": 180.0,
}
class StationaryPrefix:
"""Incremental prefix collector: no future events or raw-cloud retention."""
def __init__(
self, reference_path, *, seconds=10.0, maximum_motion_m=0.10, point_radius_m=20.0
):
if not 0 < seconds <= 30 or not 0 < maximum_motion_m <= 0.10:
raise ValueError("Stationary probe exceeds fixed bounds.")
self.buffer = LiveCloudBuffer(reference_path, point_radius_m=point_radius_m)
self.seconds = seconds
self.maximum_motion_m = maximum_motion_m
self.origin = None
self.identity = None
self.first_position = None
self.maximum_motion = 0.0
self.last_elapsed = 0.0
self.source_events = []
def ingest(self, event):
if self.origin is None:
self.origin = event.monotonic_ns
self.identity = (event.session_id, event.generation)
elapsed = (event.monotonic_ns - self.origin) / 1e9
if elapsed > self.seconds:
return False
if elapsed < self.last_elapsed or self.identity != (event.session_id, event.generation):
raise ValueError("Stationary prefix identity or clock changed.")
if len(self.source_events) >= 2048:
raise ValueError("Stationary prefix exceeds event budget.")
if event.kind == "pose":
position = np.asarray(event.position, dtype=float)
if self.first_position is None:
self.first_position = position.copy()
self.maximum_motion = max(
self.maximum_motion, float(np.linalg.norm(position - self.first_position))
)
if self.maximum_motion > self.maximum_motion_m:
raise ValueError("Prefix is not stationary within the declared motion limit.")
self.buffer.ingest(event)
if self.buffer.gaps:
raise ValueError("Stationary prefix contains a receipt gap.")
self.last_elapsed = elapsed
self.source_events.append(dict(sequence=event.sequence, kind=event.kind, time_s=elapsed))
return True
def freeze(self):
if self.first_position is None or self.last_elapsed < self.seconds - 0.5:
raise ValueError("Incomplete stationary prefix.")
sample = self.buffer.snapshot()
if len(sample["points"]) < 300:
raise ValueError("Insufficient stationary geometry.")
path = self.buffer.reference_path
initial = np.eye(4)
initial[:3, 3] = path[0] - self.first_position
forward = next(
(p - path[0] for p in path[1:] if np.linalg.norm((p - path[0])[:2]) >= 3), None
)
if forward is None:
raise ValueError("Reference lacks a usable route basis.")
return (
sample,
initial,
forward,
dict(
seconds=self.seconds,
last_elapsed_s=self.last_elapsed,
maximum_motion_m=self.maximum_motion,
event_count=len(self.source_events),
source_events=list(self.source_events),
),
)
def stationary_prefix(events, reference_path, *, seconds=10.0, maximum_motion_m=0.10):
collector = StationaryPrefix(reference_path, seconds=seconds, maximum_motion_m=maximum_motion_m)
for event in events:
if not collector.ingest(event):
break
return collector.freeze()
+629
View File
@@ -0,0 +1,629 @@
"""Stationary bootstrap on the existing exclusive, read-only planning ingress."""
import json
import subprocess
import numpy as np
from k1link.artifacts import utc_now_iso
from .live_buffer import LiveCloudBuffer, PoseDiscontinuity
from .live_presentation import PRESENTATION_POLICY
from .reference_window import ReferenceCoverageError, ReferenceWindowIndex, reference_window
from .route_relocalization import ROUTE_RELOCALIZATION_POLICY
from .stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap
PHASE_MESSAGE = {
"waiting-cloud": "Ожидание облака точек после подготовки сканера.",
"collecting": "Накопление данных. Сканер должен оставаться неподвижным.",
"searching": (
"Точная привязка у стартовой зоны; при честном отказе — поиск по выбранному "
"маршруту. Ожидание на месте."
),
"refreshing": "Подтверждение привязки по новым кадрам. Ожидание на месте.",
"validating": "Подтверждение привязки по новым кадрам. Ожидание на месте.",
"tracking": "Привязка подтверждена. Можно начинать проверочный проход.",
"lost": "Привязка потеряна. Остановитесь; поиск по новым данным продолжается.",
}
def phase_message(boot):
if boot.phase == "lost" and not boot.tracking_established:
if boot.reason == "initialization-ambiguous":
return (
"Синхронизация маршрута не выполнена: найдены несколько похожих участков. "
"Останьтесь на месте, измените обзор сцены или выберите более различимый участок."
)
if boot.reason == "initialization-no-route-location":
return (
"Синхронизация маршрута не выполнена: облако не дало устойчивого совпадения "
"с выбранным маршрутом. Можно переместить сканер в другую точку, остановить "
"его там и затем нажать «Переинициализировать»."
)
if boot.reason in {"initialization-incomplete", "initialization-expired"}:
return (
"Синхронизация маршрута не завершилась. Остановите устройство и запись, "
"затем начните новое исследование и дождитесь неподвижной калибровки."
)
return (
"Синхронизация маршрута не выполнена. Убедитесь, что сканер находится "
"у исследованного участка; можно выбрать другую различимую точку, остановиться "
"и затем нажать «Переинициализировать»."
)
return PHASE_MESSAGE[boot.phase]
def run_stationary_live(service, source, run_id, executor, clock, initialize, calculate):
"""Own calculations only. Device start/stop and capture remain plugin-owned."""
directory = service.directory(run_id)
query_radius_m = ROUTE_RELOCALIZATION_POLICY["query_radius_m"]
buffer = LiveCloudBuffer(service.reference_path, point_radius_m=query_radius_m)
reference_index = ReferenceWindowIndex(service.reference)
boot = None
latest_pose = None
query_key = None
future = pending = None
sequence = 0
last_snapshot = 0.0
last_phase = None
transitions = []
end_reason = "cancelled"
ever_tracking = False
recovering = False
waiting_retry = False
recovery_attempt = 0
recovery_position = None
route_search_only = False
def begin_recovery(reason):
# Retain only the last confirmed place as a SEARCH HINT. Neither the
# old matrix nor old receipts may grant tracking in this new attempt.
nonlocal boot, latest_pose, last_phase, recovering, recovery_attempt
nonlocal route_search_only
route_search_only = False
boot = None
latest_pose = None
last_phase = None
recovering = True
recovery_attempt += 1
with service.lock:
service.accepted_sample = None
service.last_result_ns = 0
service.update(
planning_phase="recovering",
planning_reason=reason,
tracking_state="lost",
tracking_established=ever_tracking,
tracking_reason=reason,
recovery_attempt=recovery_attempt,
recovery_reference_position=recovery_position,
message="Остановитесь. Восстанавливаем привязку по новым данным; запись продолжается.",
)
def initialization_attempt():
# The worker owns a stable run snapshot. Do not invoke the projected
# UI view merely to stamp internal evidence for one calculation.
return int(service.run.get("initialization_attempt", 1))
def reset_initialization(attempt):
"""Discard only derived evidence after an operator-directed retry.
The source session remains open and keeps recording. The following
usable pose/cloud pair starts a completely new stationary prefix, so a
failed location hypothesis can never leak into the next attempt.
"""
nonlocal buffer, boot, latest_pose, last_snapshot, last_phase, waiting_retry
nonlocal route_search_only
route_search_only = False
waiting_retry = False
buffer = LiveCloudBuffer(service.reference_path, point_radius_m=query_radius_m)
boot = None
latest_pose = None
last_snapshot = 0.0
last_phase = None
transitions.append(
dict(
phase="waiting-cloud",
reason="operator-reinitialize",
tracking_state="acquiring",
streak=0,
segment=0,
initialization_attempt=attempt,
monotonic_ns=clock.monotonic_ns(),
at_utc=utc_now_iso(),
)
)
service.update(
state="running",
planning_phase="waiting-cloud",
planning_reason="operator-reinitialize",
tracking_state="acquiring",
tracking_established=False,
tracking_reason="operator-reinitialize",
phase_transitions=transitions[-100:],
message=(
"Предыдущая попытка привязки отброшена. Переинициализация начинается "
"в выбранной точке: оставьте сканер неподвижно до окончания накопления."
),
)
def retry_prefix_interrupted(exc):
"""Return a moved retry to the actionable lost state without stopping capture."""
nonlocal buffer, boot, latest_pose, last_snapshot, last_phase, waiting_retry
reason = str(exc)
attempt = initialization_attempt()
transitions.append(
dict(
phase="lost",
reason="retry-prefix-interrupted",
tracking_state="lost",
streak=0,
segment=buffer.segment,
initialization_attempt=attempt,
diagnostic=reason,
monotonic_ns=clock.monotonic_ns(),
at_utc=utc_now_iso(),
)
)
buffer = LiveCloudBuffer(service.reference_path, point_radius_m=query_radius_m)
boot = None
latest_pose = None
last_snapshot = 0.0
last_phase = None
waiting_retry = True
service.update(
state="running",
planning_phase="lost",
planning_reason="retry-prefix-interrupted",
tracking_state="lost",
tracking_established=False,
tracking_reason="retry-prefix-interrupted",
reinitialization_diagnostic=reason,
phase_transitions=transitions[-100:],
message=(
"Переинициализация не началась: сканер сдвинулся или поток прервался во время "
"накопления. Остановите его в выбранной точке "
"и нажмите «Переинициализировать» ещё раз."
),
)
def publish_phase(*, force=False):
nonlocal last_phase, ever_tracking, recovering
if boot is None:
return
ever_tracking |= boot.tracking_established
if boot.phase == "tracking":
recovering = False
phase = "recovering" if recovering else boot.phase
state = (phase, boot.reason, boot.gate.state, boot.gate.reason)
if state == last_phase and not force:
return
last_phase = state
transitions.append(
dict(
phase=phase,
recovery_stage=boot.phase if recovering else None,
recovery_attempt=recovery_attempt,
reason=boot.reason,
tracking_state=boot.gate.state,
streak=boot.gate.streak,
candidate_trial=boot.candidate_trial,
candidate_index=boot.candidate_index,
segment=buffer.segment,
monotonic_ns=clock.monotonic_ns(),
at_utc=utc_now_iso(),
)
)
if boot.gate.matrix is None:
with service.lock:
service.accepted_sample = None
service.last_result_ns = 0
service.update(
planning_phase=phase,
planning_reason=boot.reason,
tracking_state=boot.gate.state,
tracking_established=ever_tracking,
tracking_reason=boot.gate.reason,
phase_transitions=transitions[-100:],
message=(
"Остановитесь. Восстанавливаем привязку по новым данным; запись продолжается."
if recovering
else phase_message(boot)
),
)
def stage(sample, role, extra=None):
nonlocal sequence
sequence += 1
target = directory / f"step-{sequence:03d}"
target.mkdir()
info = dict(
session_id=query_key[0],
generation=query_key[1],
role=role,
events=sample["events"],
query_path=sample["path"].tolist(),
sequence=sample["sequence"],
segment=sample["segment"],
distance_m=sample["distance"],
fresh_floor_ns=sample.get("fresh_floor_ns"),
requested_monotonic_ns=clock.monotonic_ns(),
sampled_at_utc=utc_now_iso(),
raw_capture_owned_by="observation-session-recorder",
point_radius_m=sample.get("point_radius_m"),
initialization_attempt=initialization_attempt(),
recovery_attempt=recovery_attempt,
recovery_reference_position=recovery_position if recovering else None,
candidate_trial=boot.candidate_trial if boot else None,
candidate_index=boot.candidate_index if boot else None,
**(extra or {}),
)
(target / "source.json").write_text(json.dumps(info, allow_nan=False))
return target, (sample, role, target, info)
def finish(*, active=True):
nonlocal future, recovery_position
if future is None or (active and not future.done()):
return
try:
result = future.result()
except (ValueError, OSError, subprocess.SubprocessError) as exc:
future = None
sample, role, target, info = pending
(target / "decision.json").write_text(
json.dumps(
dict(
temporal=dict(accepted=False, reason="calculation-unavailable"),
diagnostic=f"{type(exc).__name__}: {exc}",
completed_monotonic_ns=clock.monotonic_ns(),
)
)
)
current = source.snapshot()
if active and current["active"] and not current.get("spatial_stop_requested", False):
boot.stop("calculation-unavailable")
publish_phase(force=True)
return
future = None
sample, role, target, info = pending
current = source.snapshot()
valid = (
active
and not service.cancel.is_set()
and current["active"]
and not current.get("spatial_stop_requested", False)
and (current["session_id"], current["session_generation"]) == query_key
)
now = clock.monotonic_ns()
if not valid:
reason = (
"spatial-stop-requested"
if current.get("spatial_stop_requested", False)
else "input-ended"
)
temporal = dict(accepted=False, reason=reason)
boot.stop(reason)
elif role == "stationary-initialization":
temporal = boot.offer_prior(result, now, buffer.segment)
else:
temporal = boot.accept_fresh(result, sample, now, buffer.segment)
if temporal["accepted"] and boot.gate.state == "tracking":
pose = np.asarray(sample["path"][-1])
matrix = boot.gate.matrix
recovery_position = (matrix[:3, :3] @ pose + matrix[:3, 3]).tolist()
if valid and role == "stationary-initialization":
service.update(
initialization_result={
k: v for k, v in result.items() if k != "matched_query_indices"
},
initialization_temporal=temporal,
)
elif valid:
service.commit_result(
result,
sample,
temporal,
boot.gate.state,
phase=boot.phase,
message=phase_message(boot),
tracking_established=ever_tracking or boot.tracking_established,
)
(target / "decision.json").write_text(
json.dumps(
dict(
completed_monotonic_ns=now,
completed_at_utc=utc_now_iso(),
temporal=temporal,
planning_phase=boot.phase,
tracking_state=boot.gate.state,
streak=boot.gate.streak,
worker_wall_s=(now - info["requested_monotonic_ns"]) / 1e9,
),
allow_nan=False,
)
)
if valid:
publish_phase(force=True)
try:
while not service.cancel.is_set():
current = source.snapshot()
if query_key:
if (current["session_id"], current["session_generation"]) != query_key:
raise ValueError(
"Сессия сканера сменилась. Для нового прохода требуется новое исследование."
)
if current.get("spatial_stop_requested", False) or not current["active"]:
end_reason = (
"spatial-stop-requested"
if current.get("spatial_stop_requested", False)
else "input-ended"
)
break
retry_attempt = service.consume_reinitialization(run_id)
if retry_attempt is not None:
if future is not None:
# The public operation admits only a terminal initial
# failure. Keep this fence in case a caller races an
# internal state update.
raise RuntimeError("Нельзя переинициализировать во время расчёта привязки.")
reset_initialization(retry_attempt)
continue
now = clock.monotonic()
if boot is not None:
boot.tick(clock.monotonic_ns(), buffer.segment)
finish()
current = source.snapshot()
if not current["active"] or current.get("spatial_stop_requested", False):
continue
publish_phase()
if boot.phase == "lost" and ever_tracking and future is None:
begin_recovery(boot.reason)
elif boot.phase == "lost" and boot.retry_route_search and future is None:
# Fresh confirmation disproved the dense-start hypothesis.
# Recollect in the same recording before whole-route search;
# neither an old prefix nor a failed transform is reused.
route_search_only = True
boot = None
latest_pose = None
last_phase = None
event = source.take("planning-" + run_id)
current = source.snapshot()
if query_key and (current["session_id"], current["session_generation"]) != query_key:
raise ValueError(
"Сессия сканера сменилась. Для нового прохода требуется новое исследование."
)
if query_key and not current["active"]:
end_reason = "input-ended"
break
if query_key and current.get("spatial_stop_requested", False):
end_reason = "spatial-stop-requested"
break
if event is not None:
if event.generation <= service.run["baseline_generation"]:
continue
if event.session_id in {
service.run["draft"]["zone"]["session_id"],
service.run["baseline_session_id"],
}:
raise ValueError("Повторный проход должен иметь новую идентичность записи.")
key = (event.session_id, event.generation)
if query_key is None:
query_key = key
service.update(
state="running",
query_session_id=key[0],
query_generation=key[1],
planning_phase="waiting-cloud",
message=PHASE_MESSAGE["waiting-cloud"],
)
if key != query_key:
raise ValueError("Изменилась идентичность входного потока.")
if event.kind in {"session-end", "spatial-stop-requested"}:
end_reason = (
"spatial-stop-requested"
if event.kind == "spatial-stop-requested"
else "input-ended"
)
break
if event.kind not in {"pose", "points"}:
continue
if event.kind == "pose":
service.observe_pose(event)
# A rejected location is intentionally a non-terminal state.
# Keep raw capture running, but do not retain numerical
# receipts while the operator carries the scanner to a better
# point before requesting the next stationary prefix.
if waiting_retry or (
boot is not None and boot.phase == "lost" and not ever_tracking
):
try:
buffer.ingest(event)
except PoseDiscontinuity:
buffer = LiveCloudBuffer(
service.reference_path, point_radius_m=query_radius_m
)
buffer.ingest(event)
service.observe_display(event, buffer, clock.monotonic_ns())
continue
try:
if boot is None:
if event.kind == "pose":
latest_pose = event
continue
if (
latest_pose is None
or not 0 <= event.monotonic_ns - latest_pose.monotonic_ns <= 500_000_000
):
continue
# Hardware calibration may precede the first usable cloud by
# many seconds. It is not part of our ten-second prefix.
if not recovering:
buffer = LiveCloudBuffer(
service.reference_path, point_radius_m=query_radius_m
)
buffer.ingest(latest_pose)
buffer.ingest(event)
if len(buffer.snapshot()["points"]) < 300:
continue
boot = StationaryBootstrap(
service.reference_path,
initialization_policy=ROUTE_RELOCALIZATION_POLICY,
point_radius_m=query_radius_m,
)
service.observe_display(latest_pose, buffer, clock.monotonic_ns())
boot.ingest(latest_pose, buffer.segment)
boot.ingest(event, buffer.segment)
else:
buffer.ingest(event)
boot.ingest(event, buffer.segment)
except ValueError as exc:
if ever_tracking and (
isinstance(exc, PoseDiscontinuity)
or (boot is not None and boot.phase == "collecting")
):
distance = buffer.distance
segment = buffer.segment + 1
buffer = LiveCloudBuffer(
service.reference_path, point_radius_m=query_radius_m
)
buffer.distance = distance # Do not add the coordinate jump as travel.
buffer.segment = segment
begin_recovery(str(exc))
continue
if boot is not None and boot.phase == "collecting":
retry_prefix_interrupted(exc)
continue
raise
service.observe_display(event, buffer, clock.monotonic_ns())
boot.tick(clock.monotonic_ns(), buffer.segment)
publish_phase()
if (
event.kind == "points"
and now - last_snapshot >= PRESENTATION_POLICY["snapshot_s"]
):
sample = buffer.snapshot()
service.update_sample(sample, clock.monotonic_ns())
service.update(
distance_m=sample["distance"],
query_points=len(sample["points"]),
receipt_gaps=sample["gaps"],
)
last_snapshot = now
if buffer.distance >= service.run["maximum_distance_m"]:
# A pose can reach the limit before the next cloud snapshot.
service.update_sample(buffer.snapshot(), clock.monotonic_ns())
service.update(distance_m=buffer.distance)
end_reason = "distance-limit"
break
if boot is None or future is not None:
continue
# Do not freeze ahead of an already queued prefix receipt.
if (
event is None
or event.monotonic_ns > boot.origin + BOOTSTRAP_POLICY["prefix_seconds"] * 1e9
):
try:
acquisition = boot.start_search(clock.monotonic_ns())
except ValueError as exc:
# The retry may be moved while no new receipt arrives. Its
# prefix is then incomplete at the freeze boundary; that is
# actionable operator feedback, not a terminal planning
# failure and must leave raw recording running.
if ever_tracking:
begin_recovery(str(exc))
continue
retry_prefix_interrupted(exc)
continue
if acquisition is not None:
sample, _hint, _forward, meta = acquisition
target, pending = stage(
sample,
"stationary-initialization",
{
"prefix": meta,
"initialization_scope": "last-confirmed-place-first"
if recovering
else "route-only" if route_search_only else "start-first",
"initialization_policy": ROUTE_RELOCALIZATION_POLICY,
},
)
future = executor.submit(
initialize,
target,
service.reference,
service.reference_path,
sample["points"],
sample["path"][0],
**({"reference_position": recovery_position} if recovering else {}),
**({"route_only": True} if route_search_only else {}),
)
publish_phase()
continue
if event is not None and event.kind == "points":
validation = boot.validation(clock.monotonic_ns(), buffer.distance)
if validation is not None:
sample, hint = validation
try:
reference, window = reference_window(
service.reference, sample, hint, index=reference_index
)
except ReferenceCoverageError as exc:
target, _ = stage(
sample, "fresh-validation", {"reference_window_error": str(exc)}
)
(target / "decision.json").write_text(
json.dumps(
dict(
temporal=dict(accepted=False, reason="reference-coverage"),
diagnostic=str(exc),
completed_monotonic_ns=clock.monotonic_ns(),
)
)
)
boot.stop("reference-coverage")
publish_phase(force=True)
continue
target, pending = stage(
sample, "fresh-validation", {"reference_window": window}
)
future = executor.submit(calculate, target, reference, sample["points"], hint)
if boot is not None:
boot.stop(end_reason)
with service.lock:
service.accepted_sample = None
service.last_result_ns = 0
service.update(
state="cancelled" if service.cancel.is_set() else "completed",
planning_phase="ended",
planning_reason=end_reason,
tracking_state="lost",
tracking_reason=end_reason,
termination_reason=end_reason,
message=(
"Достигнут предел проверочного прохода. "
"Запись управляется штатными кнопками сканера."
if end_reason == "distance-limit"
else "Исследование завершено. Запись управляется штатными кнопками сканера."
),
finished_at_utc=utc_now_iso(),
)
# Publish ended before waiting for an already-running bounded fit.
# Late numerical output is retained as rejected evidence, never live.
if future and not service.cancel.is_set():
finish(active=False)
except ValueError as exc:
# Technical reasons are retained in failure.txt; operator copy is neutral.
text = str(exc)
if "not stationary" in text:
raise ValueError(
"Сканер перемещён до завершения накопления. "
"Для повторной проверки начните новый проход и дождитесь подтверждения на месте."
) from exc
if "stationary prefix" in text.lower() or "stationary geometry" in text.lower():
service.update(planning_diagnostic=text)
raise ValueError(
"Недостаточно непрерывных данных для привязки. "
"Завершите проход и начните повторную проверку."
) from exc
raise
+230
View File
@@ -0,0 +1,230 @@
"""Bounded 1x replay of stationary collection, provisional search and fresh checks."""
import json
import time
from concurrent.futures import ThreadPoolExecutor
import numpy as np
from k1link.artifacts import utc_now_iso
from .causal_replay import digest
from .causal_tracking import TRACKING_POLICY
from .entry_acquisition_worker import run_entry_acquisition
from .live_buffer import LiveCloudBuffer
from .registration import POLICY
from .registration_worker import run_registration
from .stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap
from .stationary_entry import STATIONARY_POLICY
def replay_stationary(
events,
reference,
reference_path,
directory,
*,
calculate=run_registration,
initialize=run_entry_acquisition,
max_seconds=65.0,
max_distance=40.0,
):
if not 0 < max_seconds <= 120 or not 0 < max_distance <= 40:
raise ValueError("Replay exceeds functional probe bounds.")
iterator = iter(events)
event = next(iterator, None)
if event is None:
raise ValueError("Empty replay.")
directory.mkdir(parents=True, exist_ok=False)
origin, started = event.monotonic_ns, time.monotonic_ns()
boot, buffer = StationaryBootstrap(reference_path), LiveCloudBuffer(reference_path)
deliveries, steps, transitions = [], [], []
report = dict(
schema_version="missioncore.stationary-causal-replay/v1",
created_at_utc=utc_now_iso(),
started_monotonic_ns=started,
query_origin_monotonic_ns=origin,
pace=1,
bootstrap_policy=BOOTSTRAP_POLICY,
registration_policy=POLICY,
stationary_policy=STATIONARY_POLICY,
tracking_policy=TRACKING_POLICY,
maximum_seconds=max_seconds,
maximum_distance_m=max_distance,
vehicle_control=False,
localization_confirmed=False,
slam_reset_verified=False,
first_prior_s=None,
first_candidate_s=None,
first_tracking_s=None,
steps=steps,
transitions=transitions,
)
future, pending, last_state = None, None, None
def source_now():
return origin + time.monotonic_ns() - started
def observe(now):
nonlocal last_state
state = (boot.phase, boot.reason, boot.gate.state, boot.gate.reason, buffer.segment)
if state != last_state:
transitions.append(
dict(
time_s=(now - origin) / 1e9,
phase=boot.phase,
reason=boot.reason,
tracking_state=boot.gate.state,
tracking_reason=boot.gate.reason,
streak=boot.gate.streak,
segment=buffer.segment,
)
)
last_state = state
def finish(now, *, active=True):
nonlocal future, pending
if future is None or not future.done():
return
sample, info = pending
try:
result = future.result()
except (ValueError, RuntimeError) as exc:
result = dict(status="rejected", reasons=["worker-error"], error=str(exc))
future = None
if not active:
temporal = dict(accepted=False, reason="input-ended")
elif info["role"] == "stationary-initialization":
temporal = boot.offer_prior(result, now, buffer.segment)
if temporal.get("provisional"):
report["first_prior_s"] = (now - origin) / 1e9
else:
temporal = boot.accept_fresh(result, sample, now, buffer.segment)
info.update(
completed_s=(now - origin) / 1e9,
worker_wall_s=(now - info.pop("_started_ns")) / 1e9,
temporal=temporal,
phase=boot.phase,
tracking_state=boot.gate.state,
streak=boot.gate.streak,
result={k: v for k, v in result.items() if k != "matched_query_indices"},
)
steps.append(info)
if temporal["accepted"] and report["first_candidate_s"] is None:
report["first_candidate_s"] = info["completed_s"]
if boot.gate.state == "tracking" and report["first_tracking_s"] is None:
report["first_tracking_s"] = info["completed_s"]
observe(now)
def stage(sample, now, role, extra=None):
step = len(steps) + 1
target = directory / f"step-{step:03d}"
target.mkdir()
info = dict(
step=step,
role=role,
requested_s=(now - origin) / 1e9,
sample_s=(sample["monotonic_ns"] - origin) / 1e9,
sequence=sample["sequence"],
segment=sample["segment"],
distance_m=sample["distance"],
points=len(sample["points"]),
source_events=sample["events"],
fresh_floor_ns=sample.get("fresh_floor_ns"),
**(extra or {}),
)
(target / "source.json").write_text(json.dumps(info, allow_nan=False))
np.save(target / "query-path.npy", sample["path"], allow_pickle=False)
return target, (sample, {**info, "_started_ns": now})
pool = ThreadPoolExecutor(max_workers=1, thread_name_prefix="stationary-replay-fit")
try:
while event is not None:
due = (event.monotonic_ns - origin) / 1e9
if due > max_seconds:
report["end_reason"] = "time-bound"
break
now = source_now()
if (now - origin) / 1e9 > max_seconds + 35:
raise ValueError("Replay exceeded wall-clock allowance.")
boot.tick(now, buffer.segment)
finish(now)
# All prefix receipts have been delivered before freezing. The pending
# event contributes only its due time, never its future pose or points.
if future is None and due > BOOTSTRAP_POLICY["prefix_seconds"]:
acquisition = boot.start_search(now)
if acquisition is not None:
sample, initial, basis, meta = acquisition
target, pending = stage(
sample, now, "stationary-initialization", {"prefix": meta}
)
future = pool.submit(
initialize,
target,
reference,
sample["points"],
initial,
sample["path"][0],
basis,
mode="stationary",
)
observe(now)
if now < event.monotonic_ns:
time.sleep(min(0.02, (event.monotonic_ns - now) / 1e9))
continue
before = time.monotonic_ns()
buffer.ingest(event)
boot.ingest(event, buffer.segment)
deliveries.append(
dict(
sequence=event.sequence,
kind=event.kind,
time_s=due,
lateness_s=max(0, (now - event.monotonic_ns) / 1e9),
ingest_s=(time.monotonic_ns() - before) / 1e9,
)
)
boot.tick(source_now(), buffer.segment)
observe(source_now())
if buffer.distance >= max_distance:
report["end_reason"] = "distance-bound"
break
if future is None and event.kind == "points" and len(steps) < 24:
now = source_now()
validation = boot.validation(now, buffer.distance)
if validation is not None:
sample, hint = validation
target, pending = stage(sample, now, "fresh-validation")
future = pool.submit(calculate, target, reference, sample["points"], hint)
event = next(iterator, None)
report.setdefault("end_reason", "input-ended")
report["input_end_s"] = (source_now() - origin) / 1e9
boot.stop("input-ended")
observe(source_now())
while future is not None:
finish(source_now(), active=False)
if future is not None:
time.sleep(0.02)
report["state"] = "completed"
except Exception as exc:
boot.stop("replay-error")
observe(source_now())
report.update(state="error", error=f"{type(exc).__name__}: {exc}")
raise
finally:
pool.shutdown(wait=True, cancel_futures=True)
close = getattr(iterator, "close", None)
if close:
close()
report.update(
finished_at_utc=utc_now_iso(),
elapsed_s=(time.monotonic_ns() - started) / 1e9,
gaps=buffer.gaps,
distance_m=buffer.distance,
)
(directory / "deliveries.json").write_text(json.dumps(deliveries))
report["artifacts"] = {
str(p.relative_to(directory)): digest(p) for p in directory.rglob("*") if p.is_file()
}
(directory / "report.json").write_text(json.dumps(report, allow_nan=False, indent=2))
return report
+30
View File
@@ -0,0 +1,30 @@
"""Read-only, vendor-neutral preview input. No capture or device commands."""
from dataclasses import dataclass
from typing import Protocol
@dataclass(frozen=True)
class PlanningLiveEvent:
session_id: str
generation: int
sequence: int
monotonic_ns: int
epoch_ns: int
kind: str
points: object = None
position: object = None
orientation_xyzw: object = None
class PlanningLiveSource(Protocol):
"""Return one receipt per take, or None only when no receipt is available.
Auxiliary modalities use kind='ignored' with identity/timestamps preserved
and no payload. They are neither geometry nor an empty-queue signal.
"""
def snapshot(self) -> dict: ...
def open(self, consumer_id: str) -> None: ...
def take(self, consumer_id: str) -> PlanningLiveEvent | None: ...
def close(self, consumer_id: str) -> None: ...
+128
View File
@@ -0,0 +1,128 @@
"""On-demand, source-bound overview cache. No catalog-wide decoding."""
from __future__ import annotations
import hashlib
import json
import logging
import os
import shutil
import threading
from concurrent.futures import ThreadPoolExecutor
from pathlib import Path
from typing import Mapping
from uuid import uuid4
from .store import SessionStore
from .plugin_contract import RecordingExporter
from .recording import (_validate_source, _validate_source_state,
_validated_artifact_digests, _stage_replay_prefix)
SCHEMA = 'missioncore.session-overview/v1'
class SessionOverviewService:
def __init__(self, store: SessionStore, exporters: Mapping[str, RecordingExporter]):
self.store = store
self.exporters = exporters
self.root = store.data_dir / 'session-overviews'
self.root.mkdir(parents=True, exist_ok=True)
self.executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix='session-overview')
self.guard = threading.RLock()
self.cancel = threading.Event()
self.jobs: dict[str, dict] = {}
def close(self) -> None:
self.cancel.set()
self.executor.shutdown(wait=True, cancel_futures=True)
def get(self, session_id: str, *, start: bool = True) -> dict:
detail = self.store.get_session(session_id)
base = {'schema_version': SCHEMA, 'session': detail.as_dict()}
exporter = self.exporters.get(detail.plugin_id)
if not detail.summary.replayable or detail.summary.lab is not None or exporter is None:
return {**base, 'state': 'ready', 'metrics': None, 'scene_url': None}
command = self.store.prepare_replay(session_id)
source = _validate_source(command)
identity = hashlib.sha256(json.dumps([SCHEMA, session_id, source.identity], default=str).encode()).hexdigest()
directory = self.root / identity
cached = self._cached(directory)
if cached:
return {**base, **cached, 'generation': identity, 'scene_url': f'/api/v1/observation-sessions/{session_id}/overview/scene.rrd?generation={identity}'}
with self.guard:
if identity in self.jobs:
return {**base, **self.jobs[identity]}
if not start:
return {**base, 'state': 'missing'}
if sum(j['state'] in {'queued', 'preparing'} for j in self.jobs.values()) >= 8:
return {**base, 'state': 'error', 'message': 'Подготовка занята. Повторите позже.'}
self.jobs[identity] = {'state': 'queued', 'messages_processed': 0}
self.executor.submit(self._build, identity, source, exporter)
return {**base, **self.jobs[identity]}
def retry(self, session_id: str) -> dict:
detail = self.store.get_session(session_id)
if detail.summary.replayable and detail.summary.lab is None:
source = _validate_source(self.store.prepare_replay(session_id))
identity = hashlib.sha256(json.dumps([SCHEMA, session_id, source.identity], default=str).encode()).hexdigest()
with self.guard:
if self.jobs.get(identity, {}).get('state') == 'error':
self.jobs.pop(identity, None)
return self.get(session_id)
def scene(self, session_id: str, generation: str) -> Path:
current = self.get(session_id, start=False)
if current.get('state') != 'ready' or current.get('generation') != generation:
raise ValueError('overview generation is unavailable')
return self.root / generation / 'scene.rrd'
def _cached(self, directory: Path) -> dict | None:
try:
report = directory / 'overview.json'
if report.stat().st_size > 2 * 1024 * 1024:
return None
doc = json.loads(report.read_text())
stat = (directory / 'scene.rrd').stat()
if doc['schema_version'] != SCHEMA or [stat.st_size, stat.st_mtime_ns] != doc['scene_stat']:
return None
return {'state': 'ready', 'metrics': doc['metrics'], 'scene_sha256': doc['scene_sha256']}
except (OSError, ValueError, KeyError, TypeError):
return None
def _build(self, identity: str, source, exporter: RecordingExporter) -> None:
directory = self.root / identity
directory.mkdir(exist_ok=True)
candidate = directory / ('.' + uuid4().hex + '.rrd')
staged = None
try:
with self.guard:
self.jobs[identity] = {'state': 'preparing', 'messages_processed': 0}
digests = _validated_artifact_digests(source)
staged, primary, _ = _stage_replay_prefix(directory, source, cancel_event=self.cancel)
def pulse():
with self.guard:
self.jobs[identity]['messages_processed'] += 100
metrics = dict(exporter(primary, candidate, cancel_event=self.cancel, activity_callback=pulse))
if self.cancel.is_set() or source.identity != _validate_source_state(source).identity:
raise ValueError('overview source changed')
if digests != _validated_artifact_digests(source):
raise ValueError('overview source changed')
if candidate.stat().st_size > 32 * 1024 * 1024:
raise ValueError('overview exceeded display budget')
digest = hashlib.sha256(candidate.read_bytes()).hexdigest()
os.replace(candidate, directory / 'scene.rrd')
stat = (directory / 'scene.rrd').stat()
document = {'schema_version': SCHEMA, 'metrics': metrics, 'source_digests': digests,
'scene_sha256': digest, 'scene_stat': [stat.st_size, stat.st_mtime_ns]}
temporary = directory / '.overview.json'
temporary.write_text(json.dumps(document, allow_nan=False))
os.replace(temporary, directory / 'overview.json')
with self.guard:
self.jobs.pop(identity, None)
except Exception:
logging.getLogger(__name__).exception('Session overview preparation failed')
with self.guard:
self.jobs[identity] = {'state': 'error', 'message': 'Не удалось подготовить обзор записи.'}
finally:
candidate.unlink(missing_ok=True)
if staged is not None:
shutil.rmtree(staged, ignore_errors=True)
+86
View File
@@ -0,0 +1,86 @@
"""View-only height clipping and camera presets for bounded overview RRDs."""
from functools import lru_cache
from pathlib import Path
from typing import Literal
import numpy as np
import rerun as rr
from rerun import blueprint as rrb
from rerun.experimental import RrdReader
@lru_cache(maxsize=1)
def _geometry(path: Path, size: int, modified: int):
if size > 32 * 1024 * 1024:
raise ValueError('overview exceeds display budget')
reader = RrdReader(path)
entry = reader.recordings()[0]
xyz = np.empty((0, 3), dtype=np.float32)
colors = np.empty(0, dtype=np.uint32)
for chunk in reader.stream():
if chunk.entity_path == '/world/cloud':
batch = chunk.to_record_batch()
xyz = batch.column('Points3D:positions')[0].values.values.to_numpy().reshape(-1, 3)
colors = batch.column('Points3D:colors')[0].values.to_numpy()
if len(xyz) > 180_000 or not np.isfinite(xyz).all():
raise ValueError('overview geometry is invalid')
return entry.application_id, entry.recording_id, xyz, colors
def geometry(path: Path):
stat = path.stat()
return _geometry(path, stat.st_size, stat.st_mtime_ns)
def spatial_metadata(path: Path) -> dict:
_, _, xyz, _ = geometry(path)
return {'height_min_m': float(xyz[:, 2].min()) if len(xyz) else None,
'height_max_m': float(xyz[:, 2].max()) if len(xyz) else None,
'sample_points': len(xyz)}
def _camera_eye(xyz: np.ndarray, mode: Literal['3d', 'top'], aspect: float) -> dict:
points = xyz.astype(np.float64) if len(xyz) else np.array([[-1., -1., -1.], [1., 1., 1.]])
center = (points.min(axis=0) + points.max(axis=0)) / 2
centered = points - center
# Align an elongated survey with the width of the viewport, regardless of K1's initial yaw.
_, axes = np.linalg.eigh(centered[:, :2].T @ centered[:, :2])
along = axes[:, -1]
if along[np.argmax(np.abs(along))] < 0:
along = -along
side = np.array([-along[1], along[0], 0.])
direction = np.array([0., 0., 1.]) if mode == 'top' else side * .8 + np.array([0., 0., .75])
direction /= np.linalg.norm(direction)
up = side if mode == 'top' else np.array([0., 0., 1.])
right = np.cross(-direction, up)
right /= np.linalg.norm(right)
screen_up = np.cross(right, -direction)
# Conservative 45-degree vertical field of view, with space around the cloud.
tangent = np.tan(np.pi / 8)
depth = centered @ direction
required = np.maximum(np.abs(centered @ right) / (aspect * tangent),
np.abs(centered @ screen_up) / tangent) + depth
distance = max(2., float(required.max()) * 1.15)
return {'position': (center + direction * distance).tolist(),
'lookTarget': center.tolist(), 'eyeUp': up.tolist()}
def render_spatial_update(path: Path, ceiling_m: float | None, mode: Literal['3d', 'top'] | None, aspect: float = 1.5):
app_id, recording_id, xyz, colors = geometry(path)
selected = xyz[:, 2] <= ceiling_m if ceiling_m is not None else np.ones(len(xyz), dtype=bool)
recording = rr.RecordingStream(app_id, recording_id=recording_id, send_properties=False)
sink = rr.binary_stream(recording)
eye = None
try:
# Static replacement changes only the display derivative; poses and source metrics are untouched.
recording.log('world/cloud', rr.Points3D(xyz[selected], colors=colors[selected], radii=rr.Radius.ui_points(1.5)), static=True)
if mode is not None:
eye = _camera_eye(xyz, mode, aspect)
view = rrb.Spatial3DView(name='Облако и траектория', origin='/world', contents=['/world/**'],
background=[9, 10, 12, 255], eye_controls=rrb.EyeControls3D(kind=rrb.Eye3DKind.Orbital,
position=eye['position'], look_target=eye['lookTarget'], eye_up=eye['eyeUp']))
recording.send_blueprint(rrb.Blueprint(view, auto_layout=False, auto_views=False, collapse_panels=True))
data = sink.read(flush=True)
return data, int(selected.sum()), eye
finally:
recording.disconnect()
+7
View File
@@ -14,9 +14,11 @@ from pathlib import Path
from typing import Literal, Protocol
from .models import ObservationSessionCandidate, ReplayCommand
from .live_planning import PlanningLiveSource
RecordingProgressPulse = Callable[[], None]
RecordingExportResult = Mapping[str, object]
SubmapExtractor = Callable[[Path, dict, int, int], tuple[object, dict]]
class PluginRecordingExportError(RuntimeError):
@@ -80,3 +82,8 @@ class ObservationRuntimeContribution:
archives: tuple[ObservationArchiveSource, ...]
recording_exporter: RecordingExporter
point_color_renderer: RecordedPointColorRenderer | None = None
overview_exporter: RecordingExporter | None = None
planning_exporter: RecordingExporter | None = None
submap_extractor: SubmapExtractor | None = None
live_planning_source: PlanningLiveSource | None = None
scene_submap_extractor: SubmapExtractor | None = None
+23
View File
@@ -230,6 +230,13 @@ from k1link.web.runtime_readiness import (
build_runtime_readiness,
)
from k1link.web.session_api import build_session_router
from k1link.web.session_overview_api import build_session_overview_router
from k1link.sessions.overview import SessionOverviewService
from k1link.missions.sources import PlanningSources
from k1link.missions.drafts import MissionDrafts
from k1link.missions.registration_runs import RegistrationRuns
from k1link.web.mission_registration_api import build_mission_registration_router
from k1link.web.mission_planner_api import build_mission_planner_router
from k1link.web.simulation_projects_api import build_simulation_projects_router
from k1link.web.simulation_world_provider_api import build_simulation_world_provider_router
from k1link.web.system_telemetry_api import build_system_telemetry_router
@@ -463,6 +470,14 @@ session_recording_materializer = SessionRecordingMaterializer(
exporters=plugin_environment.recording_exporters,
artifact_gateway=session_artifact_gateway,
)
session_overview_service = SessionOverviewService(session_store, plugin_environment.overview_exporters)
mission_drafts = MissionDrafts(session_store.data_dir / 'missions', PlanningSources(
session_store, plugin_environment.planning_exporters, plugin_environment.submap_extractors,
plugin_environment.scene_submap_extractors))
mission_registration_runs = RegistrationRuns(mission_drafts)
from k1link.missions.live_tests import PlanningLiveTests
from k1link.web.planning_live_api import build_planning_live_router
planning_live_tests = PlanningLiveTests(mission_drafts, plugin_environment.live_planning_sources, mission_registration_runs.lock)
session_recorded_media_inspector = RecordedMediaInspector(
session_store.data_dir / "recorded-media-preparations"
)
@@ -956,6 +971,9 @@ async def app_lifespan(application: FastAPI) -> AsyncIterator[None]:
with suppress(asyncio.CancelledError):
await publication_reconciler
await asyncio.to_thread(session_recording_preparation_manager.close)
await asyncio.to_thread(session_overview_service.close)
await asyncio.to_thread(planning_live_tests.close)
await asyncio.to_thread(mission_registration_runs.close)
await asyncio.to_thread(lidar_local_surface_read_service.close)
plugin_environment.close()
@@ -1128,6 +1146,11 @@ if session_artifact_gateway is not None and _ffmpeg is not None:
for legacy_router in plugin_environment.legacy_routers:
app.include_router(legacy_router)
app.include_router(build_session_overview_router(session_overview_service))
app.include_router(build_mission_planner_router(mission_drafts))
app.include_router(build_planning_live_router(planning_live_tests))
app.include_router(build_mission_registration_router(mission_registration_runs, planning_live_tests))
app.include_router(
build_session_router(
session_store,
@@ -14,6 +14,7 @@ from k1link.sessions.plugin_contract import (
ObservationArchiveSource,
RecordedPointColorRenderer,
RecordingExporter,
SubmapExtractor,
)
from k1link.web.plugin_catalog import DevicePluginCatalog, DevicePluginManifest
from k1link.web.plugin_runtime import (
@@ -52,6 +53,47 @@ class InstalledDevicePluginEnvironment:
if contribution.observation is not None
}
@property
def overview_exporters(self) -> dict[str, RecordingExporter]:
return {
contribution.runtime.descriptor.plugin_id: exporter
for contribution in self._contributions
if contribution.observation is not None
if (exporter := contribution.observation.overview_exporter) is not None
}
@property
def planning_exporters(self) -> dict[str, RecordingExporter]:
return {
contribution.runtime.descriptor.plugin_id: exporter
for contribution in self._contributions
if contribution.observation is not None
if (exporter := contribution.observation.planning_exporter) is not None
}
@property
def live_planning_sources(self):
return {c.runtime.descriptor.plugin_id: c.observation.live_planning_source
for c in self._contributions if c.observation is not None
and c.observation.live_planning_source is not None}
@property
def submap_extractors(self) -> dict[str, SubmapExtractor]:
return {
contribution.runtime.descriptor.plugin_id: extractor
for contribution in self._contributions
if contribution.observation is not None
if (extractor := contribution.observation.submap_extractor) is not None
}
@property
def scene_submap_extractors(self) -> dict[str, SubmapExtractor]:
return {
c.runtime.descriptor.plugin_id: c.observation.scene_submap_extractor
for c in self._contributions
if c.observation is not None and c.observation.scene_submap_extractor is not None
}
@property
def point_color_renderers(self) -> dict[str, RecordedPointColorRenderer]:
return {
+64
View File
@@ -0,0 +1,64 @@
"""Mission planning API: immutable recorded sources, draft persistence, data checks."""
from uuid import UUID
from typing import Literal
from fastapi import APIRouter, HTTPException
from pydantic import BaseModel, ConfigDict, Field
from starlette.concurrency import run_in_threadpool
from k1link.sessions.models import SessionNotFoundError, SessionStoreError
from k1link.sessions.recording import RecordingMaterializationError
from k1link.missions.drafts import MissionDrafts
class DraftRequest(BaseModel):
model_config = ConfigDict(extra='forbid')
id: UUID | None = None
revision: int = Field(default=0, ge=0)
name: str = Field(min_length=1, max_length=120)
session_id: str = Field(pattern=r'^[A-Za-z0-9][A-Za-z0-9._-]{0,127}$')
generation: str = Field(pattern='^[a-f0-9]{64}$')
start_index: int = Field(ge=0, strict=True)
end_index: int = Field(ge=1, strict=True)
direction: Literal['forward', 'reverse'] = 'forward'
class CheckRequest(BaseModel):
model_config = ConfigDict(extra='forbid')
revision: int = Field(ge=1, strict=True)
def build_mission_planner_router(drafts: MissionDrafts) -> APIRouter:
router = APIRouter(prefix='/api/v1/mission-planner')
async def call(operation, *args):
try:
return await run_in_threadpool(operation, *args)
except (KeyError, SessionNotFoundError) as exc:
raise HTTPException(404, 'Запись или черновик не найдены.') from exc
except (SessionStoreError, RecordingMaterializationError, OSError) as exc:
raise HTTPException(409, 'Исходная запись недоступна или изменилась.') from exc
except ValueError as exc:
raise HTTPException(409, str(exc)) from exc
@router.get('/sources/{session_id}')
async def source(session_id: str):
return await call(drafts.sources.get, session_id)
@router.get('/drafts')
async def list_drafts():
# Catalog excludes the full route geometry; detail is loaded on demand.
items = await call(drafts.list)
return {'items': [{k: v for k, v in item.items() if k != 'route'} for item in items]}
@router.get('/drafts/{draft_id}')
async def get_draft(draft_id: UUID):
return await call(drafts.get, str(draft_id))
@router.post('/drafts')
async def save_draft(request: DraftRequest):
return await call(drafts.save, request)
@router.post('/drafts/{draft_id}/checks')
async def check_draft(draft_id: UUID, request: CheckRequest):
return await call(drafts.check, str(draft_id), request.revision)
return router
@@ -0,0 +1,79 @@
"""Recorded cloud comparison, separate from data-only route checks."""
import sqlite3
from uuid import UUID
from fastapi import APIRouter, HTTPException
from fastapi.responses import FileResponse
from pydantic import BaseModel, ConfigDict, Field
from starlette.concurrency import run_in_threadpool
from k1link.sessions.models import SessionNotFoundError, SessionStoreError
from k1link.sessions.recording import RecordingMaterializationError
class RegistrationRequest(BaseModel):
model_config = ConfigDict(extra='forbid')
revision: int = Field(ge=1, strict=True)
session_id: str = Field(pattern=r'^[A-Za-z0-9][A-Za-z0-9._-]{0,127}$')
generation: str = Field(pattern='^[a-f0-9]{64}$')
start_index: int = Field(ge=0, strict=True)
end_index: int = Field(ge=1, strict=True)
class DeleteProjectRequest(BaseModel):
model_config = ConfigDict(extra='forbid')
revision: int = Field(ge=1, strict=True)
def build_mission_registration_router(runs, live=None):
from k1link.missions.projects import PlanningProjects
projects = PlanningProjects(runs, live)
router = APIRouter(prefix='/api/v1/mission-planner')
async def call(operation, *args):
try:
return await run_in_threadpool(operation, *args)
except (KeyError, SessionNotFoundError) as exc:
raise HTTPException(404, 'Запись или результат не найдены.') from exc
except (SessionStoreError, RecordingMaterializationError, OSError) as exc:
raise HTTPException(409, 'Исходная запись недоступна или изменилась.') from exc
except ValueError as exc:
raise HTTPException(409, str(exc)) from exc
except sqlite3.Error as exc:
raise HTTPException(
409, 'Каталог проектов недоступен. Повторите попытку.'
) from exc
@router.get('/projects')
async def project_catalog():
return {'items': await call(projects.list)}
@router.get('/projects/{kind}/{project_id}')
async def project(kind: str, project_id: UUID):
return await call(projects.get, kind, str(project_id))
@router.delete('/projects/{kind}/{project_id}')
async def delete_project(kind: str, project_id: UUID, request: DeleteProjectRequest):
return await call(projects.remove, kind, str(project_id), request.revision)
@router.get('/projects/live/{project_id}/scene.rrd')
async def live_project_scene(project_id: UUID):
path = await call(projects.live_scene, str(project_id))
return FileResponse(path, media_type='application/octet-stream')
@router.post('/drafts/{draft_id}/registration-runs')
async def start(draft_id: UUID, request: RegistrationRequest):
return await call(runs.start, str(draft_id), request.model_dump())
@router.get('/drafts/{draft_id}/registration-runs')
async def list_runs(draft_id: UUID):
return {'items': await call(runs.list, str(draft_id))}
@router.get('/registration-runs/{run_id}')
async def report(run_id: UUID):
return await call(runs.get, str(run_id))
@router.get('/registration-runs/{run_id}/scene.rrd')
async def scene(run_id: UUID):
path = await call(projects.verified_scene, str(run_id))
return FileResponse(path, media_type='application/octet-stream')
return router
+158
View File
@@ -0,0 +1,158 @@
"""Planning-profile preparation and preview; never an acquisition endpoint."""
from typing import Literal
from uuid import UUID
from fastapi import APIRouter, HTTPException, Query, Response
from pydantic import BaseModel, ConfigDict, Field
from starlette.concurrency import run_in_threadpool
class LiveTestRequest(BaseModel):
model_config = ConfigDict(extra="forbid")
draft_id: UUID
revision: int = Field(ge=1, strict=True)
class BrowserPresentationSample(BaseModel):
model_config = ConfigDict(extra="forbid")
cloud_revision: int = Field(ge=1, le=1_000_000_000, strict=True)
cloud_sequence: int = Field(ge=1, le=1_000_000_000, strict=True)
display_epoch: str = Field(min_length=36, max_length=36)
request_ms: float = Field(ge=0, le=5_000)
rerun_admission_ms: float = Field(ge=0, le=5_000)
first_animation_frame_ms: float | None = Field(default=None, ge=0, le=5_000)
second_animation_frame_ms: float | None = Field(default=None, ge=0, le=5_000)
frame_timeout: bool
source_to_second_animation_frame_upper_bound_ms: float | None = Field(
default=None, ge=0, le=15_000
)
class BrowserPresentationObservationRequest(BaseModel):
model_config = ConfigDict(extra="forbid")
schema_version: Literal["missioncore.planning-browser-presentation/v1"]
samples: list[BrowserPresentationSample] = Field(min_length=1, max_length=8)
def build_planning_live_router(service):
router = APIRouter(prefix="/api/v1/mission-planner/live-tests")
async def call(fn, *args):
try:
return await run_in_threadpool(fn, *args)
except KeyError as exc:
raise HTTPException(404, "Исследование не найдено.") from exc
except (ValueError, RuntimeError) as exc:
raise HTTPException(409, str(exc)) from exc
@router.get("/active")
async def active():
return await call(service.get)
@router.get("")
async def history():
return {"items": await call(service.history)}
@router.post("/{run_id}/select")
async def select(run_id: UUID):
return await call(service.select, str(run_id))
@router.post("")
async def start(body: LiveTestRequest):
return await call(service.start, str(body.draft_id), body.revision)
@router.post("/{run_id}/stop")
async def stop(run_id: UUID):
return await call(service.stop, str(run_id))
@router.post("/{run_id}/reinitialize")
async def reinitialize(run_id: UUID):
return await call(service.request_reinitialization, str(run_id))
@router.post("/{run_id}/presentation-observations", status_code=204)
async def presentation_observations(run_id: UUID, body: BrowserPresentationObservationRequest):
await call(
service.record_browser_presentation,
str(run_id),
[sample.model_dump() for sample in body.samples],
)
return Response(status_code=204)
@router.get("/{run_id}/scene.rrd")
async def scene(
run_id: UUID,
base: bool = False,
mode: Literal["3d", "top"] = "3d",
reference: bool = True,
query: bool = True,
trajectory: bool = True,
grid: bool = True,
point_size: float = Query(1.8, ge=0.5, le=12),
ceiling_m: float | None = Query(default=None, allow_inf_nan=False),
):
options = dict(
mode=mode,
reference=reference,
query=query,
trajectory=trajectory,
grid=grid,
point_size=point_size,
ceiling_m=ceiling_m,
)
return Response(
await call(service.scene, str(run_id), base, options),
media_type="application/octet-stream",
headers={"Cache-Control": "no-store"},
)
@router.get("/{run_id}/scene-delta.rrd")
async def scene_delta(
run_id: UUID,
cursor: str = Query("", max_length=2048),
base: bool = False,
mode: Literal["3d", "top"] = "3d",
reset: int = Query(0, ge=0),
reference: bool = True,
query: bool = True,
trajectory: bool = True,
grid: bool = True,
point_size: float = Query(1.8, ge=0.5, le=12),
ceiling_m: float | None = Query(default=None, allow_inf_nan=False),
):
options = dict(
mode=mode,
reset=reset,
reference=reference,
query=query,
trajectory=trajectory,
grid=grid,
point_size=point_size,
ceiling_m=ceiling_m,
)
payload, next_cursor, info = await call(
service.scene_update, str(run_id), cursor, base, options
)
return Response(
payload,
status_code=200 if payload else 204,
media_type="application/octet-stream",
headers={
"Cache-Control": "no-store",
"X-Planning-Scene-Cursor": next_cursor,
"X-Planning-Presentation": "live" if info["live"] else "historical",
"X-Planning-Cloud-Age": str(info["cloud_age_s"] or 0),
"X-Planning-Fit-Age": str(info["fit_age_s"] or 0),
"X-Planning-Cloud-Revision": str(info["cloud_revision"] or 0),
"X-Planning-Cloud-Sequence": str(info["cloud_sequence"] or 0),
"X-Planning-Display-Epoch": str(info["display_epoch"] or ""),
"X-Planning-Height-Min": (
str(info["height_min_m"]) if info["height_min_m"] is not None else ""
),
"X-Planning-Height-Max": (
str(info["height_max_m"]) if info["height_max_m"] is not None else ""
),
},
)
return router
+59
View File
@@ -0,0 +1,59 @@
from fastapi import APIRouter, HTTPException, Query
from fastapi.responses import FileResponse, Response
from pydantic import BaseModel, ConfigDict, Field
from typing import Literal
import json
from starlette.concurrency import run_in_threadpool
from k1link.sessions.models import SessionNotFoundError, SessionStoreError
from k1link.sessions.recording import RecordingMaterializationError
from k1link.sessions.overview import SessionOverviewService
from k1link.sessions.overview_spatial import spatial_metadata, render_spatial_update
class OverviewSpatialRequest(BaseModel):
model_config = ConfigDict(extra='forbid')
generation: str = Field(pattern='^[a-f0-9]{64}$')
ceiling_m: float | None = Field(default=None, allow_inf_nan=False)
mode: Literal['3d', 'top'] | None = None
aspect: float = Field(default=1.5, ge=.1, le=20, allow_inf_nan=False)
def build_session_overview_router(service: SessionOverviewService) -> APIRouter:
router = APIRouter(prefix='/api/v1/observation-sessions')
async def call(operation, *args):
try:
return await run_in_threadpool(operation, *args)
except SessionNotFoundError as exc:
raise HTTPException(404, 'Запись не найдена.') from exc
except (ValueError, OSError, SessionStoreError, RecordingMaterializationError) as exc:
raise HTTPException(409, 'Исходные данные записи недоступны или изменились.') from exc
@router.get('/{session_id}/overview')
async def overview(session_id: str):
return await call(service.get, session_id)
@router.post('/{session_id}/overview/retry')
async def retry(session_id: str):
return await call(service.retry, session_id)
@router.get('/{session_id}/overview/scene.rrd')
async def scene(session_id: str, generation: str = Query(pattern='^[a-f0-9]{64}$')):
path = await call(service.scene, session_id, generation)
return FileResponse(path, media_type='application/octet-stream', headers={'Cache-Control': 'private, no-cache, no-transform'})
@router.get('/{session_id}/overview/spatial')
async def spatial(session_id: str, generation: str = Query(pattern='^[a-f0-9]{64}$')):
path = await call(service.scene, session_id, generation)
return await call(spatial_metadata, path)
@router.post('/{session_id}/overview/spatial')
async def spatial_update(session_id: str, request: OverviewSpatialRequest):
path = await call(service.scene, session_id, request.generation)
data, visible, eye = await call(render_spatial_update, path, request.ceiling_m, request.mode, request.aspect)
headers = {'Cache-Control': 'no-store', 'X-Overview-Visible-Points': str(visible)}
if eye is not None:
headers['X-Overview-Eye'] = json.dumps(eye)
return Response(data, media_type='application/octet-stream', headers=headers)
return router