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:
@@ -0,0 +1,124 @@
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
import hashlib
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from test_stream_summary import _write_capture, _pcl_payload, _pose_payload
|
||||
from test_session_recording import _command
|
||||
from k1link.device_plugins.xgrids_k1.session_overview import export_session_overview
|
||||
from k1link.sessions.overview import SessionOverviewService
|
||||
|
||||
|
||||
def test_overview_reads_geometry_without_fabricating_missing_timing(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pcl', _pcl_payload(scaler=1000, point_count=4)),
|
||||
('lixel/application/report/lio_pose', _pose_payload((0, 0, 0))),
|
||||
('lixel/application/report/lio_pose', _pose_payload((3, 4, 0)))])
|
||||
digest = hashlib.sha256(src.read_bytes()).hexdigest()
|
||||
result = export_session_overview(src, tmp_path / 'scene.rrd')
|
||||
assert result['point_count'] == 4
|
||||
assert result['sample_points'] > 0
|
||||
assert result['path_m'] == 5
|
||||
assert result['chart'] == [] and result['mean_hz'] is None
|
||||
assert result['spatial_available']
|
||||
assert (tmp_path / 'scene.rrd').stat().st_size > 100
|
||||
assert hashlib.sha256(src.read_bytes()).hexdigest() == digest
|
||||
|
||||
|
||||
def test_overview_counts_corrupt_frames_and_cancels(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pcl', b'bad')])
|
||||
assert export_session_overview(src, tmp_path / 'scene.rrd')['decode_errors'] == 1
|
||||
stop = threading.Event(); stop.set()
|
||||
with pytest.raises(RuntimeError, match='cancelled'):
|
||||
export_session_overview(src, tmp_path / 'unused.rrd', cancel_event=stop)
|
||||
|
||||
|
||||
def test_cache_is_single_flight_source_bound_and_reused_after_restart(tmp_path):
|
||||
command = _command(tmp_path / 'source')
|
||||
detail = SimpleNamespace(plugin_id=command.plugin_id, summary=SimpleNamespace(replayable=True, lab=None), as_dict=lambda: {'session_id': command.session_id})
|
||||
store = SimpleNamespace(data_dir=tmp_path / 'data', get_session=lambda _: detail, prepare_replay=lambda _: command)
|
||||
calls = []
|
||||
def exporter(source, destination, **kwargs):
|
||||
calls.append(source)
|
||||
destination.write_bytes(b'bounded-rrd')
|
||||
return {'point_count': 42}
|
||||
service = SessionOverviewService(store, {command.plugin_id: exporter})
|
||||
try:
|
||||
for _ in range(5): service.get(command.session_id)
|
||||
for _ in range(100):
|
||||
result = service.get(command.session_id)
|
||||
if result['state'] == 'ready': break
|
||||
time.sleep(.01)
|
||||
assert result['metrics']['point_count'] == 42
|
||||
assert len(calls) == 1
|
||||
assert service.scene(command.session_id, result['generation']).read_bytes() == b'bounded-rrd'
|
||||
with pytest.raises(ValueError): service.scene(command.session_id, '0'*64)
|
||||
finally: service.close()
|
||||
restored = SessionOverviewService(store, {command.plugin_id: exporter})
|
||||
try:
|
||||
assert restored.get(command.session_id)['state'] == 'ready'
|
||||
assert len(calls) == 1
|
||||
command.primary_artifact.path.write_bytes(b'X' * command.primary_artifact.file_byte_length)
|
||||
assert restored.get(command.session_id)['state'] in {'queued', 'preparing'}
|
||||
finally: restored.close()
|
||||
|
||||
|
||||
def test_lab_overview_does_not_leak_unbounded_parent_geometry(tmp_path):
|
||||
detail = SimpleNamespace(plugin_id='test', summary=SimpleNamespace(replayable=True, lab=object()), as_dict=lambda: {'session_id': 'derived'})
|
||||
store = SimpleNamespace(data_dir=tmp_path, get_session=lambda _: detail, prepare_replay=lambda _: pytest.fail('parent must not be opened'))
|
||||
service = SessionOverviewService(store, {'test': lambda *_: pytest.fail('not called')})
|
||||
try:
|
||||
result = service.get('derived')
|
||||
assert result['metrics'] is None and result['scene_url'] is None
|
||||
finally: service.close()
|
||||
|
||||
|
||||
def test_height_slice_is_reversible_and_only_replaces_display_points(tmp_path):
|
||||
import rerun as rr
|
||||
import numpy as np
|
||||
from rerun.experimental import RrdReader
|
||||
from k1link.sessions.overview_spatial import spatial_metadata, render_spatial_update
|
||||
source = tmp_path / 'overview.rrd'
|
||||
recording = rr.RecordingStream('missioncore_session_overview')
|
||||
recording.save(source)
|
||||
recording.log('world/cloud', rr.Points3D([[0, 0, 0], [1, 0, 3], [2, 0, 8]], colors=[[20, 30, 40]] * 3), static=True)
|
||||
recording.flush(); recording.disconnect()
|
||||
digest = hashlib.sha256(source.read_bytes()).hexdigest()
|
||||
assert spatial_metadata(source) == {'height_min_m': 0, 'height_max_m': 8, 'sample_points': 3}
|
||||
for ceiling, expected in [(3, 2), (-1, 0), (None, 3)]:
|
||||
data, count, eye = render_spatial_update(source, ceiling, 'top')
|
||||
assert count == expected and eye['eyeUp'] == [0, 1, 0]
|
||||
assert eye['position'][2] > eye['lookTarget'][2]
|
||||
out = tmp_path / f'view-{expected}.rrd'; out.write_bytes(data)
|
||||
cloud = next(c for c in RrdReader(out).stream() if c.entity_path == '/world/cloud')
|
||||
points = cloud.to_record_batch().column('Points3D:positions')[0].values.values.to_numpy().reshape(-1, 3)
|
||||
assert len(points) == expected
|
||||
assert ceiling is None or np.all(points[:, 2] <= ceiling)
|
||||
assert RrdReader(out).recordings()[0].recording_id == RrdReader(source).recordings()[0].recording_id
|
||||
assert hashlib.sha256(source.read_bytes()).hexdigest() == digest
|
||||
|
||||
|
||||
def test_camera_presets_fit_an_elongated_survey_to_viewport_width():
|
||||
import numpy as np
|
||||
from k1link.sessions.overview_spatial import _camera_eye
|
||||
points = np.array([[x, y, z] for x in (-10, 10) for y in (-250, 250) for z in (0, 30)])
|
||||
top = _camera_eye(points, 'top', 2.5)
|
||||
assert np.allclose(top['eyeUp'], [-1, 0, 0])
|
||||
assert np.allclose(np.array(top['position'])[:2], top['lookTarget'][:2])
|
||||
assert top['position'][2] < _camera_eye(points, 'top', .7)['position'][2]
|
||||
for mode in ('top', '3d'):
|
||||
eye = _camera_eye(points, mode, 2.5)
|
||||
position, target, up = map(np.asarray, (eye['position'], eye['lookTarget'], eye['eyeUp']))
|
||||
forward = target - position; forward /= np.linalg.norm(forward)
|
||||
right = np.cross(forward, up); right /= np.linalg.norm(right)
|
||||
screen_up = np.cross(right, forward)
|
||||
relative = points - position
|
||||
depth = relative @ forward
|
||||
assert np.all(depth > 0)
|
||||
assert np.all(np.abs(relative @ right) < depth * np.tan(np.pi / 8) * 2.5)
|
||||
assert np.all(np.abs(relative @ screen_up) < depth * np.tan(np.pi / 8))
|
||||
Reference in New Issue
Block a user