feat: qualify bounded K1 surface shadow
This commit is contained in:
DCCONSTRUCTIONS
@@ -2,9 +2,11 @@ from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from fastapi import APIRouter
|
||||
from fastapi.routing import APIRoute
|
||||
|
||||
@@ -12,6 +14,8 @@ from k1link.compute import (
|
||||
E10_LIDAR_PACK_SCHEMA,
|
||||
E10LidarFieldSource,
|
||||
K1LocalSurfaceProfile,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowRuntime,
|
||||
K1LocalSurfaceV1,
|
||||
build_k1_local_surface,
|
||||
)
|
||||
@@ -51,9 +55,7 @@ def _source_pack(root: Path) -> Path:
|
||||
zz = 0.04 * xx - 0.015 * yy + 0.008 * np.sin(xx * 2 + frame_index)
|
||||
ground = np.column_stack((xx.ravel(), yy.ravel(), zz.ravel()))
|
||||
obstacle_xy = ground[::13, :2]
|
||||
obstacle_z = (
|
||||
0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75
|
||||
)
|
||||
obstacle_z = 0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75
|
||||
obstacle = np.column_stack((obstacle_xy, obstacle_z))
|
||||
cloud = np.concatenate((ground, obstacle)).astype("<f4")
|
||||
clouds.append(cloud)
|
||||
@@ -188,8 +190,9 @@ def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
|
||||
assert len(evidence["cell_points_xyz_m"]) == detail["prediction"]["cell_count"]
|
||||
assert len(evidence["cell_signed_residual_m"]) == detail["prediction"]["cell_count"]
|
||||
assert len(evidence["cell_inlier"]) == detail["prediction"]["cell_count"]
|
||||
assert sum(evidence["cell_inlier"]) / detail["prediction"]["cell_count"] == (
|
||||
detail["prediction"]["inlier_fraction"]
|
||||
assert (
|
||||
sum(evidence["cell_inlier"]) / detail["prediction"]["cell_count"]
|
||||
== (detail["prediction"]["inlier_fraction"])
|
||||
)
|
||||
assert detail["temporal"]["compared"] is True
|
||||
assert detail["authority"]["commands_enabled"] is False
|
||||
@@ -239,3 +242,165 @@ def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
|
||||
assert review["review_profile_id"] == "missioncore-local-surface-attention/v1"
|
||||
assert review["access"] == "read-only"
|
||||
assert str(tmp_path) not in repr(review)
|
||||
|
||||
|
||||
def _shadow_input(
|
||||
source: E10LidarFieldSource,
|
||||
frame_index: int,
|
||||
) -> K1LocalSurfaceShadowInput:
|
||||
offsets = source.arrays["cloud_offsets"]
|
||||
start = int(offsets[frame_index])
|
||||
end = int(offsets[frame_index + 1])
|
||||
points = np.asarray(
|
||||
source.arrays["cloud_points_map"][start:end],
|
||||
dtype=np.float64,
|
||||
).copy()
|
||||
position = np.asarray(
|
||||
source.arrays["pose_positions_map"][frame_index],
|
||||
dtype=np.float64,
|
||||
).copy()
|
||||
points.flags.writeable = False
|
||||
position.flags.writeable = False
|
||||
return K1LocalSurfaceShadowInput(
|
||||
frame_index=frame_index,
|
||||
source_frame_index=int(source.arrays["source_frame_indices"][frame_index]),
|
||||
session_seconds=float(source.arrays["session_seconds"][frame_index]),
|
||||
pose_binding_age_ms=abs(float(source.arrays["pose_point_delta_ms"][frame_index])),
|
||||
points_map=points,
|
||||
position_map=position,
|
||||
published_monotonic_ns=time.monotonic_ns(),
|
||||
)
|
||||
|
||||
|
||||
def test_k1_local_surface_shadow_matches_replay_and_stays_non_authoritative(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
source_path = _source_pack(tmp_path / "source")
|
||||
profile = K1LocalSurfaceProfile(
|
||||
profile_id="synthetic-shadow-local-surface/v1",
|
||||
local_radius_m=5.0,
|
||||
cell_size_m=0.5,
|
||||
surface_ttl_s=0.5,
|
||||
minimum_surface_cells=12,
|
||||
)
|
||||
source = E10LidarFieldSource(source_path)
|
||||
try:
|
||||
output = build_k1_local_surface(
|
||||
source,
|
||||
tmp_path / "models",
|
||||
profile=profile,
|
||||
)
|
||||
finally:
|
||||
source.close()
|
||||
|
||||
source = E10LidarFieldSource(source_path)
|
||||
model = K1LocalSurfaceV1(output)
|
||||
runtime = K1LocalSurfaceShadowRuntime(
|
||||
"synthetic-shadow",
|
||||
profile=profile,
|
||||
queue_capacity=2,
|
||||
result_capacity=16,
|
||||
)
|
||||
try:
|
||||
published = []
|
||||
for frame_index in range(source.frame_count):
|
||||
if not bool(source.arrays["sample_available"][frame_index]):
|
||||
continue
|
||||
runtime.publish(_shadow_input(source, frame_index))
|
||||
assert runtime.wait_until_idle(2.0)
|
||||
published.append(frame_index)
|
||||
runtime.close()
|
||||
results = runtime.results()
|
||||
assert [item.frame_index for item in results] == published
|
||||
offsets = source.arrays["cloud_offsets"]
|
||||
for result in results:
|
||||
frame_index = result.frame_index
|
||||
start = int(offsets[frame_index])
|
||||
end = int(offsets[frame_index + 1])
|
||||
if bool(model.arrays["frame_valid"][frame_index]):
|
||||
assert result.state == "valid"
|
||||
assert result.sensor_height_m == pytest.approx(
|
||||
float(model.arrays["sensor_height_m"][frame_index]),
|
||||
abs=1e-12,
|
||||
)
|
||||
assert result.slope_deg == pytest.approx(
|
||||
float(model.arrays["slope_deg"][frame_index]),
|
||||
abs=1e-12,
|
||||
)
|
||||
assert result.roughness_m == pytest.approx(
|
||||
float(model.arrays["roughness_m"][frame_index]),
|
||||
abs=1e-12,
|
||||
)
|
||||
assert result.confidence == pytest.approx(
|
||||
float(model.arrays["confidence"][frame_index]),
|
||||
abs=1e-12,
|
||||
)
|
||||
assert np.array_equal(
|
||||
result.point_class,
|
||||
model.arrays["point_class"][start:end],
|
||||
)
|
||||
assert np.array_equal(
|
||||
result.point_step_candidate,
|
||||
model.arrays["point_step_candidate"][start:end],
|
||||
)
|
||||
else:
|
||||
assert result.state == "pose-stale"
|
||||
snapshot = runtime.snapshot()
|
||||
assert snapshot["queue"]["capacity"] == 2
|
||||
assert snapshot["queue"]["published"] == len(published)
|
||||
assert snapshot["queue"]["consumed"] == len(published)
|
||||
assert snapshot["queue"]["dropped_overflow"] == 0
|
||||
assert snapshot["results"]["failed"] == 0
|
||||
assert snapshot["occupancy_policy"]["absence_of_points_means_free"] is False
|
||||
assert snapshot["authority"]["commands_enabled"] is False
|
||||
assert snapshot["authority"]["navigation_or_safety_accepted"] is False
|
||||
assert snapshot["closed"] is True
|
||||
finally:
|
||||
runtime.close()
|
||||
source.close()
|
||||
model.close()
|
||||
|
||||
|
||||
def test_k1_local_surface_shadow_overload_is_bounded_and_latest_wins(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
source_path = _source_pack(tmp_path / "source")
|
||||
source = E10LidarFieldSource(source_path)
|
||||
runtime = K1LocalSurfaceShadowRuntime(
|
||||
"synthetic-overload",
|
||||
profile=K1LocalSurfaceProfile(
|
||||
profile_id="synthetic-shadow-overload/v1",
|
||||
local_radius_m=5.0,
|
||||
cell_size_m=0.5,
|
||||
minimum_surface_cells=12,
|
||||
),
|
||||
queue_capacity=1,
|
||||
result_capacity=2,
|
||||
)
|
||||
try:
|
||||
template = _shadow_input(source, 0)
|
||||
for frame_index in range(200):
|
||||
runtime.publish(
|
||||
K1LocalSurfaceShadowInput(
|
||||
frame_index=frame_index,
|
||||
source_frame_index=10_000 + frame_index,
|
||||
session_seconds=10.0 + frame_index * 0.1,
|
||||
pose_binding_age_ms=4.0,
|
||||
points_map=template.points_map,
|
||||
position_map=template.position_map,
|
||||
published_monotonic_ns=time.monotonic_ns(),
|
||||
)
|
||||
)
|
||||
runtime.close()
|
||||
snapshot = runtime.snapshot()
|
||||
queue = snapshot["queue"]
|
||||
assert queue["maximum_depth"] <= queue["capacity"] == 1
|
||||
assert queue["dropped_overflow"] > 0
|
||||
assert queue["consumed"] + queue["dropped_overflow"] == queue["published"]
|
||||
assert snapshot["results"]["depth"] <= 2
|
||||
assert runtime.results()[-1].frame_index == 199
|
||||
assert snapshot["results"]["latest"]["frame_index"] == 199
|
||||
assert snapshot["authority"]["commands_enabled"] is False
|
||||
finally:
|
||||
runtime.close()
|
||||
source.close()
|
||||
|
||||
Reference in New Issue
Block a user