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:
@@ -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),
|
||||
}
|
||||
@@ -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")),
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
"""Recorded-zone mission drafts. No vehicle execution authority."""
|
||||
@@ -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
|
||||
@@ -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}
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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:])
|
||||
@@ -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),
|
||||
)
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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"
|
||||
),
|
||||
}
|
||||
@@ -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
|
||||
@@ -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),
|
||||
)
|
||||
@@ -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(),
|
||||
)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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: ...
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user