feat: wire physical K1 surface shadow
This commit is contained in:
parent
e6d5411bdd
commit
a1e2cb523f
12
README.md
12
README.md
|
|
@ -168,6 +168,18 @@ and scalar results matched the immutable replay derivative exactly. This is a
|
||||||
recorded-source-paced execution gate only; physical K1 worker binding,
|
recorded-source-paced execution gate only; physical K1 worker binding,
|
||||||
free-space, commands, navigation and safety authority remain unavailable.
|
free-space, commands, navigation and safety authority remain unavailable.
|
||||||
|
|
||||||
|
The physical shadow seam is now implemented but not yet physically qualified.
|
||||||
|
The authenticated external worker can bind decoded map-frame `lio_pcl` and
|
||||||
|
`lio_pose` in either arrival order through
|
||||||
|
`missioncore.k1-local-surface-pose-binder/v1`, then feed the accepted estimator
|
||||||
|
without blocking camera cadence. Point input and estimator work are
|
||||||
|
capacity-two latest-wins queues; pose history and result history are bounded.
|
||||||
|
The pinned `e28_physical_k1_local_surface_profile.json` rejects the run unless
|
||||||
|
at least 100 point frames bind and exact accounting, pose misses, queue drops,
|
||||||
|
p95 result age, profile identity and shadow-only authority all pass. No
|
||||||
|
physical acceptance result is claimed until that profile is executed against
|
||||||
|
an attached K1 on the NVIDIA worker.
|
||||||
|
|
||||||
The complete RELLIS-3D v1.1 release is now admitted there and its full
|
The complete RELLIS-3D v1.1 release is now admitted there and its full
|
||||||
`2,413`-frame validation split is available in **Полигон → Датасеты**. The
|
`2,413`-frame validation split is available in **Полигон → Датасеты**. The
|
||||||
sealed Current/Patchwork++ comparison rejected Patchwork++ for navigation:
|
sealed Current/Patchwork++ comparison rejected Patchwork++ for navigation:
|
||||||
|
|
|
||||||
|
|
@ -517,10 +517,28 @@ immutable replay derivative: zero state, point-class or step-candidate
|
||||||
mismatches and zero scalar delta. The accepted result is
|
mismatches and zero scalar delta. The accepted result is
|
||||||
`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`.
|
`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`.
|
||||||
|
|
||||||
The next gate is not another replay tuning pass. It is an explicit bounded
|
The authenticated external worker now contains the next bounded seam.
|
||||||
LiDAR↔pose binder on the authenticated external worker stream followed by a
|
`missioncore.k1-local-surface-pose-binder/v1` accepts decoded map-frame
|
||||||
physical K1 shadow run, still without commands, free-space, navigation or
|
`lio_pcl` and `lio_pose` in either arrival order, binds by bounded worker
|
||||||
safety authority.
|
host-arrival time, and publishes only matched pairs to the capacity-two
|
||||||
|
latest-wins estimator queue. Point backlog is bounded to two, pose history to
|
||||||
|
16 and the diagnostic result ring to eight. Shutdown flushes or explicitly
|
||||||
|
counts every point frame; the final report records pose-binding misses,
|
||||||
|
binder/runtime overflow, exact queue accounting, processing failures and p95
|
||||||
|
result age.
|
||||||
|
|
||||||
|
The pinned physical profile is
|
||||||
|
`experiments/perception/worker/e28_physical_k1_local_surface_profile.json`.
|
||||||
|
It requires at least `100` bound frames, at most `5%` pose misses, `1%` binder
|
||||||
|
point replacement, `1%` runtime replacement and `80 ms` p95 worker-local
|
||||||
|
result age. The existing D-only PowerShell worker launcher accepts this
|
||||||
|
physical profile and verifies that the minimal hash-addressed package contains
|
||||||
|
the local-surface geometry, binder/runtime and ground primitives.
|
||||||
|
|
||||||
|
This is implementation readiness, not a physical result. The next gate is to
|
||||||
|
build and stage the new content-addressed worker package on D, execute the
|
||||||
|
profile against an attached K1 and retain the generated report. Commands,
|
||||||
|
free-space, navigation and safety authority remain disabled.
|
||||||
|
|
||||||
Exit: one immutable K1 session yields both a persistent reconstruction and a
|
Exit: one immutable K1 session yields both a persistent reconstruction and a
|
||||||
bounded local world state without hard-coded terrain height or scanner-side
|
bounded local world state without hard-coded terrain height or scanner-side
|
||||||
|
|
@ -545,6 +563,9 @@ independent gate without increasing unsafe false-free or false-dynamic output.
|
||||||
|
|
||||||
- [x] Add a provider-neutral bounded LiDAR local-surface queue independent of
|
- [x] Add a provider-neutral bounded LiDAR local-surface queue independent of
|
||||||
camera cadence and qualify it at recorded 1× source pace.
|
camera cadence and qualify it at recorded 1× source pace.
|
||||||
|
- [x] Bind authenticated decoded physical `lio_pcl + lio_pose` to that queue
|
||||||
|
with bounded buffers, exact accounting and a pinned physical acceptance
|
||||||
|
profile.
|
||||||
- [ ] Run the accepted K1 local-surface/local-map profile on the NVIDIA worker.
|
- [ ] Run the accepted K1 local-surface/local-map profile on the NVIDIA worker.
|
||||||
- [ ] Fuse K1 geometric evidence with E26 camera evidence as independent
|
- [ ] Fuse K1 geometric evidence with E26 camera evidence as independent
|
||||||
sources; a LiDAR-native detector remains optional.
|
sources; a LiDAR-native detector remains optional.
|
||||||
|
|
@ -612,7 +633,7 @@ large for a free-space claim. Deterministic triage has reduced the first manual
|
||||||
inspection set to four high-priority frames in two episodes. Prior-plane
|
inspection set to four high-priority frames in two episodes. Prior-plane
|
||||||
residual explainability now shows that episode `09` is a localized positive
|
residual explainability now shows that episode `09` is a localized positive
|
||||||
structure rather than symmetric plane drift. The highest-value immediate work
|
structure rather than symmetric plane drift. The highest-value immediate work
|
||||||
is therefore a bounded live-shadow queue, followed by a separate
|
is therefore the physical K1 run through the now-implemented bounded
|
||||||
dynamic-observation layer. Nvblox,
|
live-shadow seam, followed by a separate dynamic-observation layer. Nvblox,
|
||||||
raw-scan detectors and alternative SLAM remain optional later gates because the
|
raw-scan detectors and alternative SLAM remain optional later gates because the
|
||||||
current report contract does not carry their required ray/timing semantics.
|
current report contract does not carry their required ray/timing semantics.
|
||||||
|
|
|
||||||
|
|
@ -92,10 +92,12 @@ Not implemented:
|
||||||
- no model training or production promotion;
|
- no model training or production promotion;
|
||||||
- no RELLIS ROS bag admission, continuous synchronized playback or production
|
- no RELLIS ROS bag admission, continuous synchronized playback or production
|
||||||
promotion;
|
promotion;
|
||||||
- no ray-cleared free-space or planner-authoritative rolling occupancy map.
|
- no ray-cleared free-space or planner-authoritative rolling occupancy map;
|
||||||
- the accepted K1 local-surface profile now passes a 15-second
|
- the accepted K1 local-surface profile now passes a 15-second
|
||||||
recorded-source-paced bounded shadow gate; physical K1 worker binding is not
|
recorded-source-paced bounded shadow gate; authenticated physical
|
||||||
implemented yet, and the residual overlay remains non-authoritative.
|
`lio_pcl + lio_pose` binding and its pinned acceptance profile are
|
||||||
|
implemented, but no attached-K1 run has qualified them yet. The residual
|
||||||
|
overlay remains non-authoritative.
|
||||||
|
|
||||||
## Product surface boundary
|
## Product surface boundary
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -17,9 +17,12 @@ SCHEMA = "missioncore.e15-worker-package/v1"
|
||||||
COPIED_FILES = (
|
COPIED_FILES = (
|
||||||
"k1link/__init__.py",
|
"k1link/__init__.py",
|
||||||
"k1link/compute/inline_temporal.py",
|
"k1link/compute/inline_temporal.py",
|
||||||
|
"k1link/compute/lidar_local_surface_geometry.py",
|
||||||
|
"k1link/compute/lidar_local_surface_shadow.py",
|
||||||
"k1link/compute/live_perception.py",
|
"k1link/compute/live_perception.py",
|
||||||
"k1link/data_plane/__init__.py",
|
"k1link/data_plane/__init__.py",
|
||||||
"k1link/data_plane/views.py",
|
"k1link/data_plane/views.py",
|
||||||
|
"k1link/ground_segmentation.py",
|
||||||
"k1link/device_plugins/__init__.py",
|
"k1link/device_plugins/__init__.py",
|
||||||
"k1link/device_plugins/xgrids_k1/protocol/__init__.py",
|
"k1link/device_plugins/xgrids_k1/protocol/__init__.py",
|
||||||
"k1link/device_plugins/xgrids_k1/protocol/normalizer.py",
|
"k1link/device_plugins/xgrids_k1/protocol/normalizer.py",
|
||||||
|
|
@ -71,6 +74,7 @@ def _identity(source_root: Path) -> dict[str, Any]:
|
||||||
"imports": [
|
"imports": [
|
||||||
"k1link.compute.inline_temporal.TemporalStabilizer",
|
"k1link.compute.inline_temporal.TemporalStabilizer",
|
||||||
"k1link.compute.inline_temporal.StreamingSemanticStabilizer",
|
"k1link.compute.inline_temporal.StreamingSemanticStabilizer",
|
||||||
|
"k1link.compute.lidar_local_surface_shadow.K1LocalSurfaceShadowCoordinator",
|
||||||
"k1link.compute.live_perception.LiveSensorSynchronizer",
|
"k1link.compute.live_perception.LiveSensorSynchronizer",
|
||||||
"k1link.data_plane.DecodedPointCloudView",
|
"k1link.data_plane.DecodedPointCloudView",
|
||||||
"k1link.data_plane.DecodedPoseView",
|
"k1link.data_plane.DecodedPoseView",
|
||||||
|
|
|
||||||
|
|
@ -136,11 +136,26 @@ if (
|
||||||
$live = Get-Content -LiteralPath $liveProfile -Raw | ConvertFrom-Json
|
$live = Get-Content -LiteralPath $liveProfile -Raw | ConvertFrom-Json
|
||||||
if (
|
if (
|
||||||
$live.schema_version -ne "missioncore.e15-shadow-inference-profile/v1" -or
|
$live.schema_version -ne "missioncore.e15-shadow-inference-profile/v1" -or
|
||||||
$live.mode -ne "replay-shadow-gate" -or
|
$live.mode -notin @("replay-shadow-gate", "physical-shadow-gate") -or
|
||||||
[bool]$live.authority.commands_enabled -or
|
[bool]$live.authority.commands_enabled -or
|
||||||
[bool]$live.authority.navigation_or_safety_accepted -or
|
[bool]$live.authority.navigation_or_safety_accepted -or
|
||||||
$live.transport.pyav_version -ne "18.0.0"
|
$live.transport.pyav_version -ne "18.0.0"
|
||||||
) { throw "LAB E15 replay-shadow authority contract changed" }
|
) { throw "LAB E15/E28 shadow authority contract changed" }
|
||||||
|
if ($live.mode -eq "physical-shadow-gate") {
|
||||||
|
foreach ($relative in @(
|
||||||
|
"k1link\compute\lidar_local_surface_geometry.py",
|
||||||
|
"k1link\compute\lidar_local_surface_shadow.py",
|
||||||
|
"k1link\ground_segmentation.py"
|
||||||
|
)) {
|
||||||
|
if (-not (Test-Path -LiteralPath (Join-Path $package $relative) -PathType Leaf)) {
|
||||||
|
throw "LAB E28 worker package lacks local-surface runtime"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (
|
||||||
|
-not [bool]$live.local_surface.enabled -or
|
||||||
|
$live.local_surface.profile_id -ne "k1-vendor-map-dynamic-local-surface/v1"
|
||||||
|
) { throw "LAB E28 local-surface profile contract changed" }
|
||||||
|
}
|
||||||
if ($stabilityProfile) {
|
if ($stabilityProfile) {
|
||||||
$stability = Get-Content -LiteralPath $stabilityProfile -Raw | ConvertFrom-Json
|
$stability = Get-Content -LiteralPath $stabilityProfile -Raw | ConvertFrom-Json
|
||||||
if (
|
if (
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,71 @@
|
||||||
|
{
|
||||||
|
"schema_version": "missioncore.e15-shadow-inference-profile/v1",
|
||||||
|
"mode": "physical-shadow-gate",
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": false,
|
||||||
|
"navigation_or_safety_accepted": false
|
||||||
|
},
|
||||||
|
"source": {
|
||||||
|
"source_id": "sensor.camera.right",
|
||||||
|
"resolution": [
|
||||||
|
800,
|
||||||
|
600
|
||||||
|
],
|
||||||
|
"calibration_slot": "camera_1",
|
||||||
|
"calibration_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9"
|
||||||
|
},
|
||||||
|
"transport": {
|
||||||
|
"wire_schema": "missioncore.live-perception-wire/v1",
|
||||||
|
"camera_media": "persistent-fmp4-pyav",
|
||||||
|
"pyav_version": "18.0.0",
|
||||||
|
"maximum_media_buffer_bytes": 8388608,
|
||||||
|
"camera_metadata_capacity": 16
|
||||||
|
},
|
||||||
|
"scheduling": {
|
||||||
|
"detector_queue_capacity": 2,
|
||||||
|
"semantic_queue_capacity": 1,
|
||||||
|
"semantic_sample_every_frames": 5,
|
||||||
|
"semantic_ttl_ms": 750.0,
|
||||||
|
"sensor_wait_ms": 90.0
|
||||||
|
},
|
||||||
|
"temporal": {
|
||||||
|
"binding": "nearest-recorded-host-arrival-best-effort",
|
||||||
|
"maximum_lidar_camera_delta_ms": 100.0,
|
||||||
|
"maximum_pose_point_delta_ms": 100.0,
|
||||||
|
"buffer_capacity_per_modality": 32,
|
||||||
|
"retention_seconds": 3.0,
|
||||||
|
"clock_qualification": "not-hardware-synchronized"
|
||||||
|
},
|
||||||
|
"local_surface": {
|
||||||
|
"enabled": true,
|
||||||
|
"profile_id": "k1-vendor-map-dynamic-local-surface/v1",
|
||||||
|
"profile_sha256": "7a59edc8404d0177a39175578743589bfb6b7822837170cded38ca2b6698cc26",
|
||||||
|
"point_queue_capacity": 2,
|
||||||
|
"pose_buffer_capacity": 16,
|
||||||
|
"future_pose_wait_ms": 25.0,
|
||||||
|
"retention_seconds": 3.0,
|
||||||
|
"result_capacity": 8,
|
||||||
|
"acceptance": {
|
||||||
|
"minimum_bound_frames": 100,
|
||||||
|
"maximum_pose_miss_fraction": 0.05,
|
||||||
|
"maximum_point_drop_fraction": 0.01,
|
||||||
|
"maximum_runtime_drop_fraction": 0.01,
|
||||||
|
"maximum_p95_result_age_ms": 80.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"acceptance": {
|
||||||
|
"minimum_camera_frames": 140,
|
||||||
|
"detector_minimum_effective_fps": 9.5,
|
||||||
|
"detector_maximum_drop_fraction": 0.01,
|
||||||
|
"semantic_minimum_effective_fps": 1.8,
|
||||||
|
"semantic_maximum_drop_fraction": 0.05,
|
||||||
|
"semantic_maximum_p95_completion_age_ms": 400.0,
|
||||||
|
"minimum_fresh_semantic_coverage": 0.9,
|
||||||
|
"minimum_fused_fraction": 0.85,
|
||||||
|
"maximum_p95_decode_age_ms": 80.0,
|
||||||
|
"maximum_p95_world_state_age_ms": 200.0,
|
||||||
|
"require_zero_transport_gaps": true,
|
||||||
|
"require_zero_camera_sequence_gaps": true,
|
||||||
|
"require_zero_failures": true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -176,6 +176,7 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]:
|
||||||
transport = profile.get("transport")
|
transport = profile.get("transport")
|
||||||
scheduling = profile.get("scheduling")
|
scheduling = profile.get("scheduling")
|
||||||
temporal = profile.get("temporal")
|
temporal = profile.get("temporal")
|
||||||
|
local_surface = profile.get("local_surface")
|
||||||
acceptance = profile.get("acceptance")
|
acceptance = profile.get("acceptance")
|
||||||
if (
|
if (
|
||||||
profile.get("schema_version") != PROFILE_SCHEMA
|
profile.get("schema_version") != PROFILE_SCHEMA
|
||||||
|
|
@ -212,6 +213,78 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]:
|
||||||
or not 1 <= float(temporal.get("maximum_pose_point_delta_ms", 0)) <= 1000
|
or not 1 <= float(temporal.get("maximum_pose_point_delta_ms", 0)) <= 1000
|
||||||
):
|
):
|
||||||
raise RuntimeError("LAB E15 bounded runtime contract is invalid")
|
raise RuntimeError("LAB E15 bounded runtime contract is invalid")
|
||||||
|
if local_surface is not None:
|
||||||
|
from k1link.compute.lidar_local_surface_geometry import (
|
||||||
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||||
|
)
|
||||||
|
|
||||||
|
local_acceptance = (
|
||||||
|
local_surface.get("acceptance")
|
||||||
|
if isinstance(local_surface, dict)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
expected_profile_sha256 = hashlib.sha256(
|
||||||
|
canonical_json(DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict())
|
||||||
|
).hexdigest()
|
||||||
|
local_fractions = (
|
||||||
|
"maximum_pose_miss_fraction",
|
||||||
|
"maximum_point_drop_fraction",
|
||||||
|
"maximum_runtime_drop_fraction",
|
||||||
|
)
|
||||||
|
point_capacity = (
|
||||||
|
local_surface.get("point_queue_capacity")
|
||||||
|
if isinstance(local_surface, dict)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
pose_capacity = (
|
||||||
|
local_surface.get("pose_buffer_capacity")
|
||||||
|
if isinstance(local_surface, dict)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
result_capacity = (
|
||||||
|
local_surface.get("result_capacity")
|
||||||
|
if isinstance(local_surface, dict)
|
||||||
|
else None
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
profile.get("mode") != "physical-shadow-gate"
|
||||||
|
or not isinstance(local_surface, dict)
|
||||||
|
or local_surface.get("enabled") is not True
|
||||||
|
or local_surface.get("profile_id")
|
||||||
|
!= DEFAULT_K1_LOCAL_SURFACE_PROFILE.profile_id
|
||||||
|
or local_surface.get("profile_sha256")
|
||||||
|
!= expected_profile_sha256
|
||||||
|
or not isinstance(point_capacity, int)
|
||||||
|
or isinstance(point_capacity, bool)
|
||||||
|
or point_capacity not in range(1, 9)
|
||||||
|
or not isinstance(pose_capacity, int)
|
||||||
|
or isinstance(pose_capacity, bool)
|
||||||
|
or pose_capacity not in range(2, 257)
|
||||||
|
or not isinstance(result_capacity, int)
|
||||||
|
or isinstance(result_capacity, bool)
|
||||||
|
or result_capacity not in range(1, 257)
|
||||||
|
or not 0
|
||||||
|
<= float(local_surface.get("future_pose_wait_ms", -1))
|
||||||
|
<= DEFAULT_K1_LOCAL_SURFACE_PROFILE.maximum_pose_binding_ms
|
||||||
|
or not 0.1
|
||||||
|
<= float(local_surface.get("retention_seconds", 0))
|
||||||
|
<= 30
|
||||||
|
or float(temporal.get("maximum_pose_point_delta_ms", 0))
|
||||||
|
!= DEFAULT_K1_LOCAL_SURFACE_PROFILE.maximum_pose_binding_ms
|
||||||
|
or not isinstance(local_acceptance, dict)
|
||||||
|
or int(local_acceptance.get("minimum_bound_frames", 0)) < 2
|
||||||
|
or any(
|
||||||
|
not 0 <= float(local_acceptance.get(key, -1)) <= 1
|
||||||
|
for key in local_fractions
|
||||||
|
)
|
||||||
|
or float(
|
||||||
|
local_acceptance.get("maximum_p95_result_age_ms", 0)
|
||||||
|
)
|
||||||
|
<= 0
|
||||||
|
):
|
||||||
|
raise RuntimeError(
|
||||||
|
"LAB E28 physical local-surface profile contract is invalid"
|
||||||
|
)
|
||||||
fractions = (
|
fractions = (
|
||||||
"detector_maximum_drop_fraction",
|
"detector_maximum_drop_fraction",
|
||||||
"semantic_maximum_drop_fraction",
|
"semantic_maximum_drop_fraction",
|
||||||
|
|
@ -244,6 +317,125 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]:
|
||||||
return profile, sha256(resolved)
|
return profile, sha256(resolved)
|
||||||
|
|
||||||
|
|
||||||
|
def _local_surface_acceptance_checks(
|
||||||
|
snapshot: dict[str, object],
|
||||||
|
config: dict[str, Any],
|
||||||
|
) -> dict[str, bool]:
|
||||||
|
binder = snapshot.get("binder")
|
||||||
|
runtime = snapshot.get("runtime")
|
||||||
|
acceptance = config["acceptance"]
|
||||||
|
if not isinstance(binder, dict):
|
||||||
|
binder = {}
|
||||||
|
if not isinstance(runtime, dict):
|
||||||
|
runtime = {}
|
||||||
|
points = binder.get("points")
|
||||||
|
poses = binder.get("poses")
|
||||||
|
queue_state = runtime.get("queue")
|
||||||
|
results = runtime.get("results")
|
||||||
|
runtime_profile = runtime.get("profile")
|
||||||
|
result_age = results.get("result_age_ms") if isinstance(results, dict) else None
|
||||||
|
if not isinstance(points, dict):
|
||||||
|
points = {}
|
||||||
|
if not isinstance(poses, dict):
|
||||||
|
poses = {}
|
||||||
|
if not isinstance(queue_state, dict):
|
||||||
|
queue_state = {}
|
||||||
|
if not isinstance(results, dict):
|
||||||
|
results = {}
|
||||||
|
if not isinstance(runtime_profile, dict):
|
||||||
|
runtime_profile = {}
|
||||||
|
if not isinstance(result_age, dict):
|
||||||
|
result_age = {}
|
||||||
|
|
||||||
|
point_published = int(points.get("published", 0))
|
||||||
|
point_bound = int(points.get("bound", 0))
|
||||||
|
point_missed = int(points.get("missed", 0))
|
||||||
|
point_dropped = int(points.get("dropped_overflow", 0))
|
||||||
|
point_depth = int(points.get("depth", 0))
|
||||||
|
runtime_published = int(queue_state.get("published", 0))
|
||||||
|
runtime_consumed = int(queue_state.get("consumed", 0))
|
||||||
|
runtime_dropped = int(queue_state.get("dropped_overflow", 0))
|
||||||
|
runtime_depth = int(queue_state.get("depth", 0))
|
||||||
|
result_published = int(results.get("published", 0))
|
||||||
|
result_failed = int(results.get("failed", 0))
|
||||||
|
p95_result_age = result_age.get("p95")
|
||||||
|
|
||||||
|
return {
|
||||||
|
"local_surface_session_initialized": bool(runtime),
|
||||||
|
"local_surface_closed": snapshot.get("closed") is True
|
||||||
|
and runtime.get("closed") is True,
|
||||||
|
"local_surface_minimum_bound_frames": point_bound
|
||||||
|
>= int(acceptance["minimum_bound_frames"]),
|
||||||
|
"local_surface_binder_accounting": point_bound
|
||||||
|
+ point_missed
|
||||||
|
+ point_dropped
|
||||||
|
+ point_depth
|
||||||
|
== point_published,
|
||||||
|
"local_surface_binder_to_runtime_accounting": point_bound
|
||||||
|
== runtime_published,
|
||||||
|
"local_surface_point_buffer_bound": (
|
||||||
|
int(points.get("capacity", 0)) == int(config["point_queue_capacity"])
|
||||||
|
and int(points.get("maximum_depth", 0))
|
||||||
|
<= int(points.get("capacity", 0))
|
||||||
|
and point_depth == 0
|
||||||
|
),
|
||||||
|
"local_surface_pose_buffer_bound": (
|
||||||
|
int(poses.get("capacity", 0)) == int(config["pose_buffer_capacity"])
|
||||||
|
and int(poses.get("maximum_depth", 0))
|
||||||
|
<= int(poses.get("capacity", 0))
|
||||||
|
),
|
||||||
|
"local_surface_maximum_pose_miss_fraction": point_missed
|
||||||
|
/ max(1, point_published)
|
||||||
|
<= float(acceptance["maximum_pose_miss_fraction"]),
|
||||||
|
"local_surface_maximum_point_drop_fraction": point_dropped
|
||||||
|
/ max(1, point_published)
|
||||||
|
<= float(acceptance["maximum_point_drop_fraction"]),
|
||||||
|
"local_surface_runtime_accounting": runtime_consumed
|
||||||
|
+ runtime_dropped
|
||||||
|
+ runtime_depth
|
||||||
|
== runtime_published,
|
||||||
|
"local_surface_runtime_result_accounting": result_published
|
||||||
|
+ result_failed
|
||||||
|
== runtime_consumed,
|
||||||
|
"local_surface_runtime_queue_bound": (
|
||||||
|
int(queue_state.get("capacity", 0)) == int(config["point_queue_capacity"])
|
||||||
|
and int(queue_state.get("maximum_depth", 0))
|
||||||
|
<= int(queue_state.get("capacity", 0))
|
||||||
|
and runtime_depth == 0
|
||||||
|
),
|
||||||
|
"local_surface_maximum_runtime_drop_fraction": runtime_dropped
|
||||||
|
/ max(1, runtime_published)
|
||||||
|
<= float(acceptance["maximum_runtime_drop_fraction"]),
|
||||||
|
"local_surface_zero_runtime_failures": result_failed == 0,
|
||||||
|
"local_surface_maximum_p95_result_age_ms": (
|
||||||
|
isinstance(p95_result_age, (int, float))
|
||||||
|
and not isinstance(p95_result_age, bool)
|
||||||
|
and float(p95_result_age)
|
||||||
|
<= float(acceptance["maximum_p95_result_age_ms"])
|
||||||
|
),
|
||||||
|
"local_surface_profile_pinned": (
|
||||||
|
runtime_profile.get("profile_id") == config["profile_id"]
|
||||||
|
),
|
||||||
|
"local_surface_shadow_authority_only": (
|
||||||
|
snapshot.get("authority")
|
||||||
|
== {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
}
|
||||||
|
and runtime.get("authority")
|
||||||
|
== {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
}
|
||||||
|
),
|
||||||
|
"local_surface_unknown_never_free": runtime.get("occupancy_policy")
|
||||||
|
== {
|
||||||
|
"absence_of_points_means_free": False,
|
||||||
|
"unknown_is_traversable": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
def read_projection_pack(
|
def read_projection_pack(
|
||||||
root: Path, expected_calibration_sha256: str
|
root: Path, expected_calibration_sha256: str
|
||||||
) -> tuple[ProjectionProfile, dict[str, Any]]:
|
) -> tuple[ProjectionProfile, dict[str, Any]]:
|
||||||
|
|
@ -312,7 +504,7 @@ def read_worker_package(root: Path) -> dict[str, Any]:
|
||||||
or manifest.get("package_id") != f"e15-worker-package-{identity_sha256}"
|
or manifest.get("package_id") != f"e15-worker-package-{identity_sha256}"
|
||||||
or resolved.name != manifest.get("package_id")
|
or resolved.name != manifest.get("package_id")
|
||||||
or not isinstance(artifacts, list)
|
or not isinstance(artifacts, list)
|
||||||
or len(artifacts) != 12
|
or len(artifacts) not in {12, 15}
|
||||||
):
|
):
|
||||||
raise RuntimeError("LAB E15 worker package identity is invalid")
|
raise RuntimeError("LAB E15 worker package identity is invalid")
|
||||||
expected = set()
|
expected = set()
|
||||||
|
|
@ -568,6 +760,7 @@ def _receiver(
|
||||||
max_duration_seconds: float,
|
max_duration_seconds: float,
|
||||||
decoder: PersistentFmp4Decoder,
|
decoder: PersistentFmp4Decoder,
|
||||||
synchronizer: LiveSensorSynchronizer,
|
synchronizer: LiveSensorSynchronizer,
|
||||||
|
local_surface: Any | None,
|
||||||
lidar_quality: LidarQualityMonitor,
|
lidar_quality: LidarQualityMonitor,
|
||||||
state: _TransportState,
|
state: _TransportState,
|
||||||
sensor_decode_ms: dict[str, list[float]],
|
sensor_decode_ms: dict[str, list[float]],
|
||||||
|
|
@ -646,6 +839,8 @@ def _receiver(
|
||||||
session_id = str(header["session_id"])
|
session_id = str(header["session_id"])
|
||||||
if state.session_id is None:
|
if state.session_id is None:
|
||||||
state.session_id = session_id
|
state.session_id = session_id
|
||||||
|
if local_surface is not None:
|
||||||
|
local_surface.begin_session(session_id)
|
||||||
elif state.session_id != session_id:
|
elif state.session_id != session_id:
|
||||||
raise ShadowRuntimeError("shadow session identity changed")
|
raise ShadowRuntimeError("shadow session identity changed")
|
||||||
modality = str(header["modality"])
|
modality = str(header["modality"])
|
||||||
|
|
@ -698,8 +893,12 @@ def _receiver(
|
||||||
if modality == "lidar" and isinstance(normalized, DecodedPointCloudView):
|
if modality == "lidar" and isinstance(normalized, DecodedPointCloudView):
|
||||||
lidar_quality.observe(normalized)
|
lidar_quality.observe(normalized)
|
||||||
synchronizer.publish_point_cloud(normalized)
|
synchronizer.publish_point_cloud(normalized)
|
||||||
|
if local_surface is not None:
|
||||||
|
local_surface.publish_point_cloud(normalized)
|
||||||
elif modality == "pose" and isinstance(normalized, DecodedPoseView):
|
elif modality == "pose" and isinstance(normalized, DecodedPoseView):
|
||||||
synchronizer.publish_pose(normalized)
|
synchronizer.publish_pose(normalized)
|
||||||
|
if local_surface is not None:
|
||||||
|
local_surface.publish_pose(normalized)
|
||||||
else:
|
else:
|
||||||
raise ShadowRuntimeError("known sensor modality did not normalize")
|
raise ShadowRuntimeError("known sensor modality did not normalize")
|
||||||
else:
|
else:
|
||||||
|
|
@ -709,6 +908,12 @@ def _receiver(
|
||||||
state.failures.append(exc)
|
state.failures.append(exc)
|
||||||
finally:
|
finally:
|
||||||
decoder.finish_input()
|
decoder.finish_input()
|
||||||
|
if local_surface is not None:
|
||||||
|
try:
|
||||||
|
local_surface.close(timeout_seconds=30)
|
||||||
|
except BaseException as exc:
|
||||||
|
assert state.failures is not None
|
||||||
|
state.failures.append(exc)
|
||||||
sender_stop.set()
|
sender_stop.set()
|
||||||
if sender_thread is not None:
|
if sender_thread is not None:
|
||||||
sender_thread.join(timeout=5)
|
sender_thread.join(timeout=5)
|
||||||
|
|
@ -765,6 +970,21 @@ def _common(args: argparse.Namespace) -> dict[str, Any]:
|
||||||
if dependency["identity"]["profile_sha256"] != semantic_sha256:
|
if dependency["identity"]["profile_sha256"] != semantic_sha256:
|
||||||
raise RuntimeError("LAB E15 semantic dependency identity changed")
|
raise RuntimeError("LAB E15 semantic dependency identity changed")
|
||||||
worker_package = read_worker_package(args.worker_package)
|
worker_package = read_worker_package(args.worker_package)
|
||||||
|
if live.get("local_surface") is not None:
|
||||||
|
worker_sources = {
|
||||||
|
str(item.get("path"))
|
||||||
|
for item in worker_package["identity"]["source_files"]
|
||||||
|
if isinstance(item, dict)
|
||||||
|
}
|
||||||
|
required_surface_sources = {
|
||||||
|
"k1link/compute/lidar_local_surface_geometry.py",
|
||||||
|
"k1link/compute/lidar_local_surface_shadow.py",
|
||||||
|
"k1link/ground_segmentation.py",
|
||||||
|
}
|
||||||
|
if not required_surface_sources <= worker_sources:
|
||||||
|
raise RuntimeError(
|
||||||
|
"LAB E28 worker package lacks local-surface runtime"
|
||||||
|
)
|
||||||
stability = None
|
stability = None
|
||||||
stability_sha256 = None
|
stability_sha256 = None
|
||||||
if args.stability_profile is not None:
|
if args.stability_profile is not None:
|
||||||
|
|
@ -926,6 +1146,22 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
capacity_per_modality=int(temporal["buffer_capacity_per_modality"]),
|
capacity_per_modality=int(temporal["buffer_capacity_per_modality"]),
|
||||||
retention_seconds=float(temporal["retention_seconds"]),
|
retention_seconds=float(temporal["retention_seconds"]),
|
||||||
)
|
)
|
||||||
|
local_surface_config = live.get("local_surface")
|
||||||
|
local_surface = None
|
||||||
|
if isinstance(local_surface_config, dict):
|
||||||
|
from k1link.compute.lidar_local_surface_shadow import (
|
||||||
|
K1LocalSurfaceShadowCoordinator,
|
||||||
|
)
|
||||||
|
|
||||||
|
local_surface = K1LocalSurfaceShadowCoordinator(
|
||||||
|
point_capacity=int(local_surface_config["point_queue_capacity"]),
|
||||||
|
pose_capacity=int(local_surface_config["pose_buffer_capacity"]),
|
||||||
|
future_pose_wait_ms=float(
|
||||||
|
local_surface_config["future_pose_wait_ms"]
|
||||||
|
),
|
||||||
|
retention_seconds=float(local_surface_config["retention_seconds"]),
|
||||||
|
result_capacity=int(local_surface_config["result_capacity"]),
|
||||||
|
)
|
||||||
lidar_quality = LidarQualityMonitor(K1_LIVE_LIDAR_PROFILE)
|
lidar_quality = LidarQualityMonitor(K1_LIVE_LIDAR_PROFILE)
|
||||||
first_camera_epoch_ns: list[int] = []
|
first_camera_epoch_ns: list[int] = []
|
||||||
last_camera_epoch_ns: list[int] = []
|
last_camera_epoch_ns: list[int] = []
|
||||||
|
|
@ -1025,6 +1261,21 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
runtime_path = output / "runtime-telemetry.jsonl"
|
runtime_path = output / "runtime-telemetry.jsonl"
|
||||||
run_started = time.perf_counter()
|
run_started = time.perf_counter()
|
||||||
process_cpu_started = time.process_time()
|
process_cpu_started = time.process_time()
|
||||||
|
runtime_snapshotters = {
|
||||||
|
"detector": detector_queue.snapshot,
|
||||||
|
"semantic": semantic_queue.snapshot,
|
||||||
|
"decoder": decoder.snapshot,
|
||||||
|
"synchronizer": synchronizer.snapshot,
|
||||||
|
"lidar_quality": lidar_quality.snapshot,
|
||||||
|
"result": lambda: {
|
||||||
|
"capacity": result_queue.maxsize,
|
||||||
|
"depth": result_queue.qsize(),
|
||||||
|
"dropped_overflow": transport.results_dropped,
|
||||||
|
"published": transport.results_published,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
if local_surface is not None:
|
||||||
|
runtime_snapshotters["local_surface"] = local_surface.snapshot
|
||||||
|
|
||||||
with (
|
with (
|
||||||
semantic_path.open("x", encoding="utf-8", newline="\n") as semantic_stream,
|
semantic_path.open("x", encoding="utf-8", newline="\n") as semantic_stream,
|
||||||
|
|
@ -1037,19 +1288,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
_RuntimeTelemetry(
|
_RuntimeTelemetry(
|
||||||
runtime_stream,
|
runtime_stream,
|
||||||
interval_seconds=1.0,
|
interval_seconds=1.0,
|
||||||
snapshotters={
|
snapshotters=runtime_snapshotters,
|
||||||
"detector": detector_queue.snapshot,
|
|
||||||
"semantic": semantic_queue.snapshot,
|
|
||||||
"decoder": decoder.snapshot,
|
|
||||||
"synchronizer": synchronizer.snapshot,
|
|
||||||
"lidar_quality": lidar_quality.snapshot,
|
|
||||||
"result": lambda: {
|
|
||||||
"capacity": result_queue.maxsize,
|
|
||||||
"depth": result_queue.qsize(),
|
|
||||||
"dropped_overflow": transport.results_dropped,
|
|
||||||
"published": transport.results_published,
|
|
||||||
},
|
|
||||||
},
|
|
||||||
) as runtime_telemetry,
|
) as runtime_telemetry,
|
||||||
):
|
):
|
||||||
|
|
||||||
|
|
@ -1111,6 +1350,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"max_duration_seconds": args.max_duration_seconds,
|
"max_duration_seconds": args.max_duration_seconds,
|
||||||
"decoder": decoder,
|
"decoder": decoder,
|
||||||
"synchronizer": synchronizer,
|
"synchronizer": synchronizer,
|
||||||
|
"local_surface": local_surface,
|
||||||
"lidar_quality": lidar_quality,
|
"lidar_quality": lidar_quality,
|
||||||
"state": transport,
|
"state": transport,
|
||||||
"sensor_decode_ms": sensor_decode_ms,
|
"sensor_decode_ms": sensor_decode_ms,
|
||||||
|
|
@ -1400,6 +1640,9 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
temporal_semantic_summary = (
|
temporal_semantic_summary = (
|
||||||
None if semantic_stabilizer is None else semantic_stabilizer.snapshot()
|
None if semantic_stabilizer is None else semantic_stabilizer.snapshot()
|
||||||
)
|
)
|
||||||
|
local_surface_snapshot = (
|
||||||
|
None if local_surface is None else local_surface.snapshot()
|
||||||
|
)
|
||||||
acceptance = live["acceptance"]
|
acceptance = live["acceptance"]
|
||||||
checks = {
|
checks = {
|
||||||
"minimum_camera_frames": decoded_frame_count >= int(acceptance["minimum_camera_frames"]),
|
"minimum_camera_frames": decoded_frame_count >= int(acceptance["minimum_camera_frames"]),
|
||||||
|
|
@ -1454,6 +1697,14 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"authority_remains_shadow_only": live["authority"]
|
"authority_remains_shadow_only": live["authority"]
|
||||||
== {"commands_enabled": False, "navigation_or_safety_accepted": False},
|
== {"commands_enabled": False, "navigation_or_safety_accepted": False},
|
||||||
}
|
}
|
||||||
|
if isinstance(local_surface_config, dict):
|
||||||
|
assert local_surface_snapshot is not None
|
||||||
|
checks.update(
|
||||||
|
_local_surface_acceptance_checks(
|
||||||
|
local_surface_snapshot,
|
||||||
|
local_surface_config,
|
||||||
|
)
|
||||||
|
)
|
||||||
if stability is not None:
|
if stability is not None:
|
||||||
temporal_acceptance = stability["acceptance"]
|
temporal_acceptance = stability["acceptance"]
|
||||||
assert temporal_track_summary is not None
|
assert temporal_track_summary is not None
|
||||||
|
|
@ -1502,6 +1753,16 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"identity_sha256": common["projection_manifest"]["identity_sha256"],
|
"identity_sha256": common["projection_manifest"]["identity_sha256"],
|
||||||
},
|
},
|
||||||
"lidar_evidence": lidar_readiness_document(K1_LIVE_LIDAR_PROFILE),
|
"lidar_evidence": lidar_readiness_document(K1_LIVE_LIDAR_PROFILE),
|
||||||
|
"local_surface": (
|
||||||
|
None
|
||||||
|
if local_surface_config is None
|
||||||
|
else {
|
||||||
|
"profile_id": local_surface_config["profile_id"],
|
||||||
|
"profile_sha256": local_surface_config["profile_sha256"],
|
||||||
|
"binder_schema": "missioncore.k1-local-surface-pose-binder/v1",
|
||||||
|
"runtime_schema": "missioncore.k1-local-surface-shadow-runtime/v1",
|
||||||
|
}
|
||||||
|
),
|
||||||
"worker_package": {
|
"worker_package": {
|
||||||
"id": common["worker_package"]["package_id"],
|
"id": common["worker_package"]["package_id"],
|
||||||
"identity_sha256": common["worker_package"]["identity_sha256"],
|
"identity_sha256": common["worker_package"]["identity_sha256"],
|
||||||
|
|
@ -1566,6 +1827,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"sensor_decode_ms": sensor_decode_summary,
|
"sensor_decode_ms": sensor_decode_summary,
|
||||||
},
|
},
|
||||||
"lidar_quality": lidar_quality.snapshot(),
|
"lidar_quality": lidar_quality.snapshot(),
|
||||||
|
"local_surface": local_surface_snapshot,
|
||||||
"latency_ms": latency_summary,
|
"latency_ms": latency_summary,
|
||||||
"temporal_stability": {
|
"temporal_stability": {
|
||||||
"enabled": stability is not None,
|
"enabled": stability is not None,
|
||||||
|
|
@ -1616,6 +1878,16 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"Camera/LiDAR matching uses recorded host arrival time, not a hardware clock.",
|
"Camera/LiDAR matching uses recorded host arrival time, not a hardware clock.",
|
||||||
"K1 LiDAR is a vendor map increment, not an admitted raw sensor sweep.",
|
"K1 LiDAR is a vendor map increment, not an admitted raw sensor sweep.",
|
||||||
"K1 LiDAR has no admitted per-point time, ring, scan geometry or IMU stream.",
|
"K1 LiDAR has no admitted per-point time, ring, scan geometry or IMU stream.",
|
||||||
|
*(
|
||||||
|
[
|
||||||
|
"K1 local-surface pose binding uses bounded worker host-arrival "
|
||||||
|
"time, not hardware synchronization.",
|
||||||
|
"K1 local-surface outputs are diagnostic observed-surface evidence; "
|
||||||
|
"they do not infer free or traversable unknown space.",
|
||||||
|
]
|
||||||
|
if local_surface_snapshot is not None
|
||||||
|
else []
|
||||||
|
),
|
||||||
"Cross-host source epoch age is diagnostic and excluded from acceptance.",
|
"Cross-host source epoch age is diagnostic and excluded from acceptance.",
|
||||||
"COCO and Cityscapes models are not forest-domain or safety validated.",
|
"COCO and Cityscapes models are not forest-domain or safety validated.",
|
||||||
"Amodal cuboids infer unobserved volume from class priors.",
|
"Amodal cuboids infer unobserved volume from class priors.",
|
||||||
|
|
|
||||||
|
|
@ -119,8 +119,12 @@ from .lidar_local_surface import (
|
||||||
k1_local_surface_catalog_item,
|
k1_local_surface_catalog_item,
|
||||||
)
|
)
|
||||||
from .lidar_local_surface_shadow import (
|
from .lidar_local_surface_shadow import (
|
||||||
|
K1_LOCAL_SURFACE_BINDER_SCHEMA,
|
||||||
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
||||||
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||||
|
K1LocalSurfaceBoundViews,
|
||||||
|
K1LocalSurfacePoseBinder,
|
||||||
|
K1LocalSurfaceShadowCoordinator,
|
||||||
K1LocalSurfaceShadowEstimator,
|
K1LocalSurfaceShadowEstimator,
|
||||||
K1LocalSurfaceShadowInput,
|
K1LocalSurfaceShadowInput,
|
||||||
K1LocalSurfaceShadowResult,
|
K1LocalSurfaceShadowResult,
|
||||||
|
|
@ -217,6 +221,7 @@ __all__ = [
|
||||||
"LIDAR_GROUND_BENCHMARK_SCHEMA",
|
"LIDAR_GROUND_BENCHMARK_SCHEMA",
|
||||||
"LIDAR_GROUND_FRAME_SCHEMA",
|
"LIDAR_GROUND_FRAME_SCHEMA",
|
||||||
"K1_LOCAL_SURFACE_FRAME_SCHEMA",
|
"K1_LOCAL_SURFACE_FRAME_SCHEMA",
|
||||||
|
"K1_LOCAL_SURFACE_BINDER_SCHEMA",
|
||||||
"K1_LOCAL_SURFACE_REPORT_SCHEMA",
|
"K1_LOCAL_SURFACE_REPORT_SCHEMA",
|
||||||
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
|
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
|
||||||
"K1_LOCAL_SURFACE_SCHEMA",
|
"K1_LOCAL_SURFACE_SCHEMA",
|
||||||
|
|
@ -246,6 +251,9 @@ __all__ = [
|
||||||
"LidarGroundBenchmarkV1",
|
"LidarGroundBenchmarkV1",
|
||||||
"LidarGroundError",
|
"LidarGroundError",
|
||||||
"K1LocalSurfaceProfile",
|
"K1LocalSurfaceProfile",
|
||||||
|
"K1LocalSurfaceBoundViews",
|
||||||
|
"K1LocalSurfacePoseBinder",
|
||||||
|
"K1LocalSurfaceShadowCoordinator",
|
||||||
"K1LocalSurfaceShadowEstimator",
|
"K1LocalSurfaceShadowEstimator",
|
||||||
"K1LocalSurfaceShadowInput",
|
"K1LocalSurfaceShadowInput",
|
||||||
"K1LocalSurfaceShadowResult",
|
"K1LocalSurfaceShadowResult",
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,403 @@
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from collections.abc import Mapping
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Final
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import numpy.typing as npt
|
||||||
|
|
||||||
|
from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError
|
||||||
|
|
||||||
|
POINT_UNCLASSIFIED: Final = 0
|
||||||
|
POINT_SURFACE: Final = 1
|
||||||
|
POINT_OCCUPIED: Final = 2
|
||||||
|
POINT_BELOW_SURFACE: Final = 3
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class K1LocalSurfaceProfile:
|
||||||
|
"""Dependency-light parameters shared by replay and live shadow."""
|
||||||
|
|
||||||
|
profile_id: str = "k1-vendor-map-dynamic-local-surface/v1"
|
||||||
|
local_radius_m: float = 10.0
|
||||||
|
cell_size_m: float = 0.45
|
||||||
|
surface_ttl_s: float = 1.25
|
||||||
|
cell_lower_percentile: float = 20.0
|
||||||
|
initial_lower_fraction: float = 0.55
|
||||||
|
minimum_surface_cells: int = 18
|
||||||
|
robust_iterations: int = 5
|
||||||
|
robust_mad_scale: float = 2.8
|
||||||
|
minimum_inlier_band_m: float = 0.10
|
||||||
|
surface_band_m: float = 0.16
|
||||||
|
obstacle_min_height_m: float = 0.20
|
||||||
|
obstacle_max_height_m: float = 3.5
|
||||||
|
maximum_pose_binding_ms: float = 100.0
|
||||||
|
maximum_slope_deg: float = 40.0
|
||||||
|
step_min_height_m: float = 0.07
|
||||||
|
step_max_height_m: float = 0.32
|
||||||
|
step_max_plane_residual_m: float = 0.45
|
||||||
|
temporal_height_jump_m: float = 0.03
|
||||||
|
temporal_slope_jump_deg: float = 0.5
|
||||||
|
temporal_roughness_jump_m: float = 0.015
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
numeric = (
|
||||||
|
self.local_radius_m,
|
||||||
|
self.cell_size_m,
|
||||||
|
self.surface_ttl_s,
|
||||||
|
self.cell_lower_percentile,
|
||||||
|
self.initial_lower_fraction,
|
||||||
|
self.robust_mad_scale,
|
||||||
|
self.minimum_inlier_band_m,
|
||||||
|
self.surface_band_m,
|
||||||
|
self.obstacle_min_height_m,
|
||||||
|
self.obstacle_max_height_m,
|
||||||
|
self.maximum_pose_binding_ms,
|
||||||
|
self.maximum_slope_deg,
|
||||||
|
self.step_min_height_m,
|
||||||
|
self.step_max_height_m,
|
||||||
|
self.step_max_plane_residual_m,
|
||||||
|
self.temporal_height_jump_m,
|
||||||
|
self.temporal_slope_jump_deg,
|
||||||
|
self.temporal_roughness_jump_m,
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
not self.profile_id.strip()
|
||||||
|
or len(self.profile_id) > 160
|
||||||
|
or not np.isfinite(numeric).all()
|
||||||
|
or not 1.0 <= self.local_radius_m <= 100.0
|
||||||
|
or not 0.05 <= self.cell_size_m <= 5.0
|
||||||
|
or not 0.05 <= self.surface_ttl_s <= 30.0
|
||||||
|
or not 0.0 <= self.cell_lower_percentile <= 50.0
|
||||||
|
or not 0.05 <= self.initial_lower_fraction <= 0.95
|
||||||
|
or not 3 <= self.minimum_surface_cells <= 100_000
|
||||||
|
or not 1 <= self.robust_iterations <= 20
|
||||||
|
or not 1.0 <= self.robust_mad_scale <= 10.0
|
||||||
|
or not 0.01 <= self.minimum_inlier_band_m <= 1.0
|
||||||
|
or not 0.01 <= self.surface_band_m <= 1.0
|
||||||
|
or not self.surface_band_m <= self.obstacle_min_height_m
|
||||||
|
or not self.obstacle_min_height_m < self.obstacle_max_height_m <= 20.0
|
||||||
|
or not 1.0 <= self.maximum_pose_binding_ms <= 10_000.0
|
||||||
|
or not 1.0 <= self.maximum_slope_deg < 90.0
|
||||||
|
or not 0.02 <= self.step_min_height_m < self.step_max_height_m
|
||||||
|
or not self.step_max_height_m <= self.step_max_plane_residual_m <= 2.0
|
||||||
|
or not 0.02 <= self.temporal_height_jump_m <= 2.0
|
||||||
|
or not 0.1 <= self.temporal_slope_jump_deg <= 45.0
|
||||||
|
or not 0.005 <= self.temporal_roughness_jump_m <= 1.0
|
||||||
|
):
|
||||||
|
raise LidarGroundError("K1 local-surface profile is invalid")
|
||||||
|
|
||||||
|
def to_dict(self) -> dict[str, object]:
|
||||||
|
return {
|
||||||
|
"schema_version": "missioncore.k1-local-surface-profile/v1",
|
||||||
|
"profile_id": self.profile_id,
|
||||||
|
"input": {
|
||||||
|
"representation": "legacy-e10-vendor-map-with-pose",
|
||||||
|
"gravity_alignment": "vendor-map-z-assumed-diagnostic",
|
||||||
|
"physical_sensor_height_required": False,
|
||||||
|
"hardcoded_height_m": None,
|
||||||
|
},
|
||||||
|
"rolling_surface": {
|
||||||
|
"local_radius_m": self.local_radius_m,
|
||||||
|
"cell_size_m": self.cell_size_m,
|
||||||
|
"surface_ttl_s": self.surface_ttl_s,
|
||||||
|
"cell_lower_percentile": self.cell_lower_percentile,
|
||||||
|
"initial_lower_fraction": self.initial_lower_fraction,
|
||||||
|
"minimum_surface_cells": self.minimum_surface_cells,
|
||||||
|
"robust_iterations": self.robust_iterations,
|
||||||
|
"robust_mad_scale": self.robust_mad_scale,
|
||||||
|
"minimum_inlier_band_m": self.minimum_inlier_band_m,
|
||||||
|
"maximum_slope_deg": self.maximum_slope_deg,
|
||||||
|
"step_min_height_m": self.step_min_height_m,
|
||||||
|
"step_max_height_m": self.step_max_height_m,
|
||||||
|
"step_max_plane_residual_m": self.step_max_plane_residual_m,
|
||||||
|
},
|
||||||
|
"classification": {
|
||||||
|
"surface_band_m": self.surface_band_m,
|
||||||
|
"obstacle_min_height_m": self.obstacle_min_height_m,
|
||||||
|
"obstacle_max_height_m": self.obstacle_max_height_m,
|
||||||
|
"absence_of_points_means_free": False,
|
||||||
|
"unknown_is_traversable": False,
|
||||||
|
},
|
||||||
|
"pose_binding": {
|
||||||
|
"basis": "recorded-nearest-host-monotonic-arrival",
|
||||||
|
"maximum_age_ms": self.maximum_pose_binding_ms,
|
||||||
|
},
|
||||||
|
"temporal_qualification": {
|
||||||
|
"prediction_input": "previous-ttl-window-only",
|
||||||
|
"current_frame_excluded_from_prediction": True,
|
||||||
|
"height_jump_m": self.temporal_height_jump_m,
|
||||||
|
"slope_jump_deg": self.temporal_slope_jump_deg,
|
||||||
|
"roughness_jump_m": self.temporal_roughness_jump_m,
|
||||||
|
},
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE: Final = K1LocalSurfaceProfile()
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class PredictionEvidence:
|
||||||
|
prior_plane: npt.NDArray[np.float64]
|
||||||
|
cell_points: npt.NDArray[np.float64]
|
||||||
|
signed_residuals: npt.NDArray[np.float64]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def residual_p50_m(self) -> float:
|
||||||
|
return float(np.percentile(np.abs(self.signed_residuals), 50))
|
||||||
|
|
||||||
|
@property
|
||||||
|
def residual_p95_m(self) -> float:
|
||||||
|
return float(np.percentile(np.abs(self.signed_residuals), 95))
|
||||||
|
|
||||||
|
def inlier_fraction(self, surface_band_m: float) -> float:
|
||||||
|
return float(np.mean(np.abs(self.signed_residuals) <= surface_band_m))
|
||||||
|
|
||||||
|
|
||||||
|
def update_cache(
|
||||||
|
cache: dict[tuple[int, int], tuple[float, float]],
|
||||||
|
cloud: npt.NDArray[np.float64],
|
||||||
|
session_seconds: float,
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> None:
|
||||||
|
keys, points = cloud_cell_observations(cloud, profile)
|
||||||
|
for key, point in zip(keys, points, strict=True):
|
||||||
|
cache[(int(key[0]), int(key[1]))] = (float(point[2]), session_seconds)
|
||||||
|
|
||||||
|
|
||||||
|
def cloud_cell_observations(
|
||||||
|
cloud: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]:
|
||||||
|
if cloud.shape[0] == 0:
|
||||||
|
return np.empty((0, 2), dtype=np.int64), np.empty((0, 3), dtype=np.float64)
|
||||||
|
cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||||
|
order = np.lexsort((cells[:, 1], cells[:, 0]))
|
||||||
|
sorted_cells = cells[order]
|
||||||
|
sorted_z = cloud[order, 2]
|
||||||
|
changes: npt.NDArray[np.int64] = (
|
||||||
|
np.flatnonzero(np.any(np.diff(sorted_cells, axis=0) != 0, axis=1)) + 1
|
||||||
|
).astype(np.int64, copy=False)
|
||||||
|
starts = np.concatenate((np.asarray([0]), changes))
|
||||||
|
ends = np.concatenate((changes, np.asarray([cloud.shape[0]])))
|
||||||
|
keys = np.empty((starts.shape[0], 2), dtype=np.int64)
|
||||||
|
points = np.empty((starts.shape[0], 3), dtype=np.float64)
|
||||||
|
half_cell = profile.cell_size_m * 0.5
|
||||||
|
for index, (start, end) in enumerate(zip(starts, ends, strict=True)):
|
||||||
|
keys[index] = sorted_cells[start]
|
||||||
|
points[index] = (
|
||||||
|
float(sorted_cells[start, 0]) * profile.cell_size_m + half_cell,
|
||||||
|
float(sorted_cells[start, 1]) * profile.cell_size_m + half_cell,
|
||||||
|
float(np.percentile(sorted_z[start:end], profile.cell_lower_percentile)),
|
||||||
|
)
|
||||||
|
return keys, points
|
||||||
|
|
||||||
|
|
||||||
|
def expire_cache(
|
||||||
|
cache: dict[tuple[int, int], tuple[float, float]],
|
||||||
|
session_seconds: float,
|
||||||
|
position: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> None:
|
||||||
|
maximum_radius_sq = (profile.local_radius_m + profile.cell_size_m) ** 2
|
||||||
|
expired = [
|
||||||
|
key
|
||||||
|
for key, (_, observed_seconds) in cache.items()
|
||||||
|
if session_seconds - observed_seconds > profile.surface_ttl_s
|
||||||
|
or ((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2
|
||||||
|
+ ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2
|
||||||
|
> maximum_radius_sq
|
||||||
|
]
|
||||||
|
for key in expired:
|
||||||
|
del cache[key]
|
||||||
|
|
||||||
|
|
||||||
|
def local_cache_records(
|
||||||
|
cache: Mapping[tuple[int, int], tuple[float, float]],
|
||||||
|
position: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> tuple[
|
||||||
|
npt.NDArray[np.int64],
|
||||||
|
npt.NDArray[np.float64],
|
||||||
|
npt.NDArray[np.float64],
|
||||||
|
]:
|
||||||
|
values = [
|
||||||
|
(
|
||||||
|
(key[0] + 0.5) * profile.cell_size_m,
|
||||||
|
(key[1] + 0.5) * profile.cell_size_m,
|
||||||
|
z,
|
||||||
|
observed_seconds,
|
||||||
|
)
|
||||||
|
for key, (z, observed_seconds) in sorted(cache.items())
|
||||||
|
if (
|
||||||
|
((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2
|
||||||
|
+ ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2
|
||||||
|
<= profile.local_radius_m**2
|
||||||
|
)
|
||||||
|
]
|
||||||
|
if not values:
|
||||||
|
return (
|
||||||
|
np.empty((0, 2), dtype=np.int64),
|
||||||
|
np.empty((0, 3), dtype=np.float64),
|
||||||
|
np.empty(0, dtype=np.float64),
|
||||||
|
)
|
||||||
|
array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64)
|
||||||
|
keys = np.floor(array[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||||
|
return keys, array[:, :3], array[:, 3]
|
||||||
|
|
||||||
|
|
||||||
|
def fit_surface(
|
||||||
|
cell_points: npt.NDArray[np.float64],
|
||||||
|
position: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> tuple[
|
||||||
|
npt.NDArray[np.float64],
|
||||||
|
npt.NDArray[np.bool_],
|
||||||
|
npt.NDArray[np.float64],
|
||||||
|
] | None:
|
||||||
|
centered_xy = cell_points[:, :2] - position[:2]
|
||||||
|
design = np.column_stack(
|
||||||
|
(centered_xy[:, 0], centered_xy[:, 1], np.ones(cell_points.shape[0]))
|
||||||
|
)
|
||||||
|
cutoff = float(np.quantile(cell_points[:, 2], profile.initial_lower_fraction))
|
||||||
|
inliers = cell_points[:, 2] <= cutoff
|
||||||
|
if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells:
|
||||||
|
return None
|
||||||
|
coefficients: npt.NDArray[np.float64] = np.zeros(3, dtype=np.float64)
|
||||||
|
for _ in range(profile.robust_iterations):
|
||||||
|
try:
|
||||||
|
coefficients, _, rank, _ = np.linalg.lstsq(
|
||||||
|
design[inliers], cell_points[inliers, 2], rcond=None
|
||||||
|
)
|
||||||
|
except np.linalg.LinAlgError:
|
||||||
|
return None
|
||||||
|
if rank < 3 or not np.isfinite(coefficients).all():
|
||||||
|
return None
|
||||||
|
residuals = cell_points[:, 2] - design @ coefficients
|
||||||
|
center = float(np.median(residuals[inliers]))
|
||||||
|
mad = float(np.median(np.abs(residuals[inliers] - center)))
|
||||||
|
band = max(
|
||||||
|
profile.minimum_inlier_band_m,
|
||||||
|
profile.robust_mad_scale * 1.4826 * mad,
|
||||||
|
)
|
||||||
|
updated = np.abs(residuals - center) <= band
|
||||||
|
if int(np.count_nonzero(updated)) < profile.minimum_surface_cells:
|
||||||
|
return None
|
||||||
|
if np.array_equal(updated, inliers):
|
||||||
|
break
|
||||||
|
inliers = updated
|
||||||
|
coefficients, _, rank, _ = np.linalg.lstsq(
|
||||||
|
design[inliers], cell_points[inliers, 2], rcond=None
|
||||||
|
)
|
||||||
|
if rank < 3 or not np.isfinite(coefficients).all():
|
||||||
|
return None
|
||||||
|
a, b, c = (float(value) for value in coefficients)
|
||||||
|
unnormalized: npt.NDArray[np.float64] = np.asarray(
|
||||||
|
[-a, -b, 1.0, a * float(position[0]) + b * float(position[1]) - c],
|
||||||
|
dtype=np.float64,
|
||||||
|
)
|
||||||
|
norm = float(np.linalg.norm(unnormalized[:3]))
|
||||||
|
if norm <= 0 or not np.isfinite(norm):
|
||||||
|
return None
|
||||||
|
plane = unnormalized / norm
|
||||||
|
residuals = height_above_plane(cell_points, plane)
|
||||||
|
center = float(np.median(residuals[inliers]))
|
||||||
|
mad = float(np.median(np.abs(residuals[inliers] - center)))
|
||||||
|
band = max(
|
||||||
|
profile.minimum_inlier_band_m,
|
||||||
|
profile.robust_mad_scale * 1.4826 * mad,
|
||||||
|
)
|
||||||
|
inliers = np.abs(residuals - center) <= band
|
||||||
|
if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells:
|
||||||
|
return None
|
||||||
|
return plane.astype("<f8"), inliers, residuals
|
||||||
|
|
||||||
|
|
||||||
|
def prediction_metrics(
|
||||||
|
prior_cell_points: npt.NDArray[np.float64],
|
||||||
|
current_cell_points: npt.NDArray[np.float64],
|
||||||
|
position: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> PredictionEvidence | None:
|
||||||
|
if (
|
||||||
|
prior_cell_points.shape[0] < profile.minimum_surface_cells
|
||||||
|
or current_cell_points.shape[0] < profile.minimum_surface_cells
|
||||||
|
):
|
||||||
|
return None
|
||||||
|
prior_fit = fit_surface(prior_cell_points, position, profile)
|
||||||
|
if prior_fit is None:
|
||||||
|
return None
|
||||||
|
prior_plane, _, _ = prior_fit
|
||||||
|
cutoff = float(np.quantile(current_cell_points[:, 2], profile.initial_lower_fraction))
|
||||||
|
evaluation = current_cell_points[:, 2] <= cutoff
|
||||||
|
if int(np.count_nonzero(evaluation)) < profile.minimum_surface_cells:
|
||||||
|
return None
|
||||||
|
evaluation_points = current_cell_points[evaluation]
|
||||||
|
signed_residuals = height_above_plane(evaluation_points, prior_plane)
|
||||||
|
if signed_residuals.size == 0 or not np.isfinite(signed_residuals).all():
|
||||||
|
return None
|
||||||
|
return PredictionEvidence(
|
||||||
|
prior_plane=prior_plane,
|
||||||
|
cell_points=evaluation_points,
|
||||||
|
signed_residuals=signed_residuals,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def step_candidate_keys(
|
||||||
|
cell_keys: npt.NDArray[np.int64],
|
||||||
|
cell_points: npt.NDArray[np.float64],
|
||||||
|
plane: npt.NDArray[np.float64],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> set[tuple[int, int]]:
|
||||||
|
if cell_keys.shape[0] != cell_points.shape[0]:
|
||||||
|
raise LidarGroundError("K1 local-surface cell alignment is invalid")
|
||||||
|
residual = height_above_plane(cell_points, plane)
|
||||||
|
lookup = {
|
||||||
|
(int(key[0]), int(key[1])): float(value)
|
||||||
|
for key, value in zip(cell_keys, residual, strict=True)
|
||||||
|
if abs(float(value)) <= profile.step_max_plane_residual_m
|
||||||
|
}
|
||||||
|
candidates: set[tuple[int, int]] = set()
|
||||||
|
for key, value in lookup.items():
|
||||||
|
for neighbor in ((key[0] + 1, key[1]), (key[0], key[1] + 1)):
|
||||||
|
neighbor_value = lookup.get(neighbor)
|
||||||
|
if neighbor_value is None:
|
||||||
|
continue
|
||||||
|
delta = abs(value - neighbor_value)
|
||||||
|
if profile.step_min_height_m <= delta <= profile.step_max_height_m:
|
||||||
|
candidates.add(key)
|
||||||
|
candidates.add(neighbor)
|
||||||
|
return candidates
|
||||||
|
|
||||||
|
|
||||||
|
def point_step_candidates(
|
||||||
|
cloud: npt.NDArray[np.float64],
|
||||||
|
local: npt.NDArray[np.bool_],
|
||||||
|
heights: npt.NDArray[np.float64],
|
||||||
|
candidate_keys: set[tuple[int, int]],
|
||||||
|
profile: K1LocalSurfaceProfile,
|
||||||
|
) -> npt.NDArray[np.uint8]:
|
||||||
|
result = np.zeros(cloud.shape[0], dtype=np.uint8)
|
||||||
|
if not candidate_keys:
|
||||||
|
return result
|
||||||
|
cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64)
|
||||||
|
for index in np.flatnonzero(local):
|
||||||
|
key = (int(cells[index, 0]), int(cells[index, 1]))
|
||||||
|
if (
|
||||||
|
key in candidate_keys
|
||||||
|
and abs(float(heights[index])) <= profile.step_max_plane_residual_m
|
||||||
|
):
|
||||||
|
result[index] = 1
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def height_above_plane(
|
||||||
|
points: npt.NDArray[np.float64],
|
||||||
|
plane: npt.NDArray[np.float64],
|
||||||
|
) -> npt.NDArray[np.float64]:
|
||||||
|
return points @ plane[:3] + float(plane[3])
|
||||||
|
|
@ -13,26 +13,45 @@ import numpy.typing as npt
|
||||||
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
|
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
|
||||||
from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError
|
from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError
|
||||||
|
|
||||||
from .lidar_local_surface import (
|
from .lidar_local_surface_geometry import (
|
||||||
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||||
POINT_BELOW_SURFACE,
|
POINT_BELOW_SURFACE,
|
||||||
POINT_OCCUPIED,
|
POINT_OCCUPIED,
|
||||||
POINT_SURFACE,
|
POINT_SURFACE,
|
||||||
K1LocalSurfaceProfile,
|
K1LocalSurfaceProfile,
|
||||||
_cloud_cell_observations,
|
)
|
||||||
_expire_cache,
|
from .lidar_local_surface_geometry import (
|
||||||
_fit_surface,
|
cloud_cell_observations as _cloud_cell_observations,
|
||||||
_height_above_plane,
|
)
|
||||||
_local_cache_records,
|
from .lidar_local_surface_geometry import (
|
||||||
_point_step_candidates,
|
expire_cache as _expire_cache,
|
||||||
_prediction_metrics,
|
)
|
||||||
_step_candidate_keys,
|
from .lidar_local_surface_geometry import (
|
||||||
_update_cache,
|
fit_surface as _fit_surface,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
height_above_plane as _height_above_plane,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
local_cache_records as _local_cache_records,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
point_step_candidates as _point_step_candidates,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
prediction_metrics as _prediction_metrics,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
step_candidate_keys as _step_candidate_keys,
|
||||||
|
)
|
||||||
|
from .lidar_local_surface_geometry import (
|
||||||
|
update_cache as _update_cache,
|
||||||
)
|
)
|
||||||
from .live_perception import LatestWinsQueue
|
from .live_perception import LatestWinsQueue
|
||||||
|
|
||||||
K1_LOCAL_SURFACE_SHADOW_SCHEMA: Final = "missioncore.k1-local-surface-shadow-runtime/v1"
|
K1_LOCAL_SURFACE_SHADOW_SCHEMA: Final = "missioncore.k1-local-surface-shadow-runtime/v1"
|
||||||
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-shadow-frame/v1"
|
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-shadow-frame/v1"
|
||||||
|
K1_LOCAL_SURFACE_BINDER_SCHEMA: Final = "missioncore.k1-local-surface-pose-binder/v1"
|
||||||
|
|
||||||
ShadowFrameState = Literal[
|
ShadowFrameState = Literal[
|
||||||
"valid",
|
"valid",
|
||||||
|
|
@ -42,6 +61,208 @@ ShadowFrameState = Literal[
|
||||||
]
|
]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class K1LocalSurfaceBoundViews:
|
||||||
|
"""One point frame paired to the nearest admitted host-arrival pose."""
|
||||||
|
|
||||||
|
point_cloud: DecodedPointCloudView
|
||||||
|
pose: DecodedPoseView
|
||||||
|
pose_binding_age_ms: float
|
||||||
|
|
||||||
|
|
||||||
|
class K1LocalSurfacePoseBinder:
|
||||||
|
"""Bounded event-order-independent LiDAR↔pose binding for shadow work."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
maximum_pose_binding_ms: float,
|
||||||
|
point_capacity: int = 2,
|
||||||
|
pose_capacity: int = 16,
|
||||||
|
future_pose_wait_ms: float = 25.0,
|
||||||
|
retention_seconds: float = 3.0,
|
||||||
|
) -> None:
|
||||||
|
if (
|
||||||
|
not math.isfinite(maximum_pose_binding_ms)
|
||||||
|
or not 1 <= maximum_pose_binding_ms <= 10_000
|
||||||
|
or not 1 <= point_capacity <= 8
|
||||||
|
or not 2 <= pose_capacity <= 256
|
||||||
|
or not math.isfinite(future_pose_wait_ms)
|
||||||
|
or not 0 <= future_pose_wait_ms <= maximum_pose_binding_ms
|
||||||
|
or not math.isfinite(retention_seconds)
|
||||||
|
or not 0.1 <= retention_seconds <= 30
|
||||||
|
):
|
||||||
|
raise LidarGroundError("K1 local-surface pose binder bounds are invalid")
|
||||||
|
self._maximum_delta_ns = round(maximum_pose_binding_ms * 1_000_000)
|
||||||
|
self._future_wait_ns = round(future_pose_wait_ms * 1_000_000)
|
||||||
|
self._retention_ns = round(retention_seconds * 1_000_000_000)
|
||||||
|
self._point_capacity = point_capacity
|
||||||
|
self._pose_capacity = pose_capacity
|
||||||
|
self._points: deque[DecodedPointCloudView] = deque()
|
||||||
|
self._poses: deque[DecodedPoseView] = deque()
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._latest_time_ns = 0
|
||||||
|
self._last_point_sequence = 0
|
||||||
|
self._last_pose_sequence = 0
|
||||||
|
self._point_published = 0
|
||||||
|
self._pose_published = 0
|
||||||
|
self._point_bound = 0
|
||||||
|
self._point_missed = 0
|
||||||
|
self._point_dropped_overflow = 0
|
||||||
|
self._pose_dropped_overflow = 0
|
||||||
|
self._maximum_point_depth = 0
|
||||||
|
self._maximum_pose_depth = 0
|
||||||
|
self._binding_age_ms: deque[float] = deque(maxlen=512)
|
||||||
|
|
||||||
|
def publish_point_cloud(
|
||||||
|
self,
|
||||||
|
value: DecodedPointCloudView,
|
||||||
|
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||||
|
if value.frame_id != "map":
|
||||||
|
raise LidarGroundError("K1 local-surface pose binder requires map-frame points")
|
||||||
|
with self._lock:
|
||||||
|
if value.context.sequence <= self._last_point_sequence:
|
||||||
|
raise LidarGroundError("K1 local-surface point sequence is not increasing")
|
||||||
|
self._last_point_sequence = value.context.sequence
|
||||||
|
self._point_published += 1
|
||||||
|
if len(self._points) == self._point_capacity:
|
||||||
|
self._points.popleft()
|
||||||
|
self._point_dropped_overflow += 1
|
||||||
|
self._points.append(value)
|
||||||
|
self._maximum_point_depth = max(
|
||||||
|
self._maximum_point_depth,
|
||||||
|
len(self._points),
|
||||||
|
)
|
||||||
|
self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value))
|
||||||
|
self._prune_poses_locked()
|
||||||
|
return self._drain_locked(force=False)
|
||||||
|
|
||||||
|
def publish_pose(
|
||||||
|
self,
|
||||||
|
value: DecodedPoseView,
|
||||||
|
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||||
|
if value.frame_id != "map" or value.child_frame_id != "sensor":
|
||||||
|
raise LidarGroundError("K1 local-surface pose binder requires map-from-sensor pose")
|
||||||
|
with self._lock:
|
||||||
|
if value.context.sequence <= self._last_pose_sequence:
|
||||||
|
raise LidarGroundError("K1 local-surface pose sequence is not increasing")
|
||||||
|
self._last_pose_sequence = value.context.sequence
|
||||||
|
self._pose_published += 1
|
||||||
|
if len(self._poses) == self._pose_capacity:
|
||||||
|
self._poses.popleft()
|
||||||
|
self._pose_dropped_overflow += 1
|
||||||
|
self._poses.append(value)
|
||||||
|
self._maximum_pose_depth = max(
|
||||||
|
self._maximum_pose_depth,
|
||||||
|
len(self._poses),
|
||||||
|
)
|
||||||
|
self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value))
|
||||||
|
self._prune_poses_locked()
|
||||||
|
return self._drain_locked(force=False)
|
||||||
|
|
||||||
|
def flush(self) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||||
|
with self._lock:
|
||||||
|
return self._drain_locked(force=True)
|
||||||
|
|
||||||
|
def snapshot(self) -> dict[str, object]:
|
||||||
|
with self._lock:
|
||||||
|
ages: npt.NDArray[np.float64] = np.asarray(
|
||||||
|
self._binding_age_ms,
|
||||||
|
dtype=np.float64,
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"schema_version": K1_LOCAL_SURFACE_BINDER_SCHEMA,
|
||||||
|
"clock_basis": "host-monotonic-arrival",
|
||||||
|
"maximum_pose_binding_ms": self._maximum_delta_ns / 1_000_000,
|
||||||
|
"future_pose_wait_ms": self._future_wait_ns / 1_000_000,
|
||||||
|
"points": {
|
||||||
|
"capacity": self._point_capacity,
|
||||||
|
"depth": len(self._points),
|
||||||
|
"maximum_depth": self._maximum_point_depth,
|
||||||
|
"published": self._point_published,
|
||||||
|
"bound": self._point_bound,
|
||||||
|
"missed": self._point_missed,
|
||||||
|
"dropped_overflow": self._point_dropped_overflow,
|
||||||
|
},
|
||||||
|
"poses": {
|
||||||
|
"capacity": self._pose_capacity,
|
||||||
|
"depth": len(self._poses),
|
||||||
|
"maximum_depth": self._maximum_pose_depth,
|
||||||
|
"published": self._pose_published,
|
||||||
|
"dropped_overflow": self._pose_dropped_overflow,
|
||||||
|
},
|
||||||
|
"binding_age_ms": {
|
||||||
|
"sample_count": int(ages.shape[0]),
|
||||||
|
"p50": float(np.percentile(ages, 50)) if ages.size else None,
|
||||||
|
"p95": float(np.percentile(ages, 95)) if ages.size else None,
|
||||||
|
"maximum": float(np.max(ages)) if ages.size else None,
|
||||||
|
},
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
def _drain_locked(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
force: bool,
|
||||||
|
) -> tuple[K1LocalSurfaceBoundViews, ...]:
|
||||||
|
bound: list[K1LocalSurfaceBoundViews] = []
|
||||||
|
while self._points:
|
||||||
|
point_cloud = self._points[0]
|
||||||
|
point_time_ns = _view_time_ns(point_cloud)
|
||||||
|
pose = min(
|
||||||
|
self._poses,
|
||||||
|
key=lambda candidate: abs(_view_time_ns(candidate) - point_time_ns),
|
||||||
|
default=None,
|
||||||
|
)
|
||||||
|
if pose is None:
|
||||||
|
if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns:
|
||||||
|
self._points.popleft()
|
||||||
|
self._point_missed += 1
|
||||||
|
continue
|
||||||
|
break
|
||||||
|
pose_time_ns = _view_time_ns(pose)
|
||||||
|
delta_ns = abs(pose_time_ns - point_time_ns)
|
||||||
|
future_watermark_reached = self._latest_time_ns - point_time_ns >= self._future_wait_ns
|
||||||
|
recent_prior_pose = pose_time_ns <= point_time_ns and delta_ns <= self._future_wait_ns
|
||||||
|
if delta_ns <= self._maximum_delta_ns and (
|
||||||
|
force
|
||||||
|
or pose_time_ns >= point_time_ns
|
||||||
|
or recent_prior_pose
|
||||||
|
or future_watermark_reached
|
||||||
|
):
|
||||||
|
self._points.popleft()
|
||||||
|
age_ms = delta_ns / 1_000_000
|
||||||
|
self._point_bound += 1
|
||||||
|
self._binding_age_ms.append(age_ms)
|
||||||
|
bound.append(
|
||||||
|
K1LocalSurfaceBoundViews(
|
||||||
|
point_cloud=point_cloud,
|
||||||
|
pose=pose,
|
||||||
|
pose_binding_age_ms=age_ms,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns:
|
||||||
|
self._points.popleft()
|
||||||
|
self._point_missed += 1
|
||||||
|
continue
|
||||||
|
break
|
||||||
|
return tuple(bound)
|
||||||
|
|
||||||
|
def _prune_poses_locked(self) -> None:
|
||||||
|
cutoff = self._latest_time_ns - self._retention_ns
|
||||||
|
while self._poses and _view_time_ns(self._poses[0]) < cutoff:
|
||||||
|
self._poses.popleft()
|
||||||
|
|
||||||
|
|
||||||
|
def _view_time_ns(value: DecodedPointCloudView | DecodedPoseView) -> int:
|
||||||
|
received = value.context.received_monotonic_ns
|
||||||
|
return int(received if received is not None else value.context.captured_at_epoch_ns)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True, slots=True)
|
@dataclass(frozen=True, slots=True)
|
||||||
class K1LocalSurfaceShadowInput:
|
class K1LocalSurfaceShadowInput:
|
||||||
"""One immutable map-point/pose pair admitted to passive shadow work."""
|
"""One immutable map-point/pose pair admitted to passive shadow work."""
|
||||||
|
|
@ -111,7 +332,9 @@ class K1LocalSurfaceShadowInput:
|
||||||
pose_binding_age_ms=pose_binding_age_ms,
|
pose_binding_age_ms=pose_binding_age_ms,
|
||||||
points_map=points,
|
points_map=points,
|
||||||
position_map=position,
|
position_map=position,
|
||||||
published_monotonic_ns=time.monotonic_ns(),
|
published_monotonic_ns=(
|
||||||
|
point_cloud.context.processing_started_monotonic_ns
|
||||||
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -561,6 +784,8 @@ class K1LocalSurfaceShadowRuntime:
|
||||||
self._processed = 0
|
self._processed = 0
|
||||||
self._failed = 0
|
self._failed = 0
|
||||||
self._state_counts: Counter[str] = Counter()
|
self._state_counts: Counter[str] = Counter()
|
||||||
|
self._processing_ms: deque[float] = deque(maxlen=512)
|
||||||
|
self._result_age_ms: deque[float] = deque(maxlen=512)
|
||||||
self._last_error: str | None = None
|
self._last_error: str | None = None
|
||||||
self._inflight = False
|
self._inflight = False
|
||||||
self._condition = threading.Condition()
|
self._condition = threading.Condition()
|
||||||
|
|
@ -638,6 +863,8 @@ class K1LocalSurfaceShadowRuntime:
|
||||||
"dropped_ring_overflow": self._result_dropped,
|
"dropped_ring_overflow": self._result_dropped,
|
||||||
"state_counts": dict(sorted(self._state_counts.items())),
|
"state_counts": dict(sorted(self._state_counts.items())),
|
||||||
"failed": self._failed,
|
"failed": self._failed,
|
||||||
|
"processing_ms": _bounded_distribution(self._processing_ms),
|
||||||
|
"result_age_ms": _bounded_distribution(self._result_age_ms),
|
||||||
"latest": latest.document() if latest is not None else None,
|
"latest": latest.document() if latest is not None else None,
|
||||||
},
|
},
|
||||||
"last_error": self._last_error,
|
"last_error": self._last_error,
|
||||||
|
|
@ -675,7 +902,128 @@ class K1LocalSurfaceShadowRuntime:
|
||||||
self._results.append(result)
|
self._results.append(result)
|
||||||
self._processed += 1
|
self._processed += 1
|
||||||
self._state_counts[result.state] += 1
|
self._state_counts[result.state] += 1
|
||||||
|
self._processing_ms.append(result.processing_ms)
|
||||||
|
self._result_age_ms.append(result.result_age_ms)
|
||||||
finally:
|
finally:
|
||||||
with self._condition:
|
with self._condition:
|
||||||
self._inflight = False
|
self._inflight = False
|
||||||
self._condition.notify_all()
|
self._condition.notify_all()
|
||||||
|
|
||||||
|
|
||||||
|
class K1LocalSurfaceShadowCoordinator:
|
||||||
|
"""Lazy session lifecycle around the bounded pose binder and estimator."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||||
|
point_capacity: int = 2,
|
||||||
|
pose_capacity: int = 16,
|
||||||
|
future_pose_wait_ms: float = 25.0,
|
||||||
|
retention_seconds: float = 3.0,
|
||||||
|
result_capacity: int = 8,
|
||||||
|
) -> None:
|
||||||
|
self.profile = profile
|
||||||
|
self._queue_capacity = point_capacity
|
||||||
|
self._result_capacity = result_capacity
|
||||||
|
self._binder = K1LocalSurfacePoseBinder(
|
||||||
|
maximum_pose_binding_ms=profile.maximum_pose_binding_ms,
|
||||||
|
point_capacity=point_capacity,
|
||||||
|
pose_capacity=pose_capacity,
|
||||||
|
future_pose_wait_ms=future_pose_wait_ms,
|
||||||
|
retention_seconds=retention_seconds,
|
||||||
|
)
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._runtime: K1LocalSurfaceShadowRuntime | None = None
|
||||||
|
self._session_id: str | None = None
|
||||||
|
self._closed = False
|
||||||
|
|
||||||
|
def begin_session(self, session_id: str) -> None:
|
||||||
|
with self._lock:
|
||||||
|
if self._closed:
|
||||||
|
raise RuntimeError("K1 local-surface shadow coordinator is closed")
|
||||||
|
if self._runtime is not None:
|
||||||
|
if self._session_id == session_id:
|
||||||
|
return
|
||||||
|
raise RuntimeError(
|
||||||
|
"K1 local-surface shadow coordinator session changed"
|
||||||
|
)
|
||||||
|
self._runtime = K1LocalSurfaceShadowRuntime(
|
||||||
|
session_id,
|
||||||
|
profile=self.profile,
|
||||||
|
queue_capacity=self._queue_capacity,
|
||||||
|
result_capacity=self._result_capacity,
|
||||||
|
)
|
||||||
|
self._session_id = session_id
|
||||||
|
|
||||||
|
def publish_point_cloud(self, value: DecodedPointCloudView) -> int:
|
||||||
|
runtime = self._active_runtime()
|
||||||
|
bindings = self._binder.publish_point_cloud(value)
|
||||||
|
for binding in bindings:
|
||||||
|
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||||
|
return len(bindings)
|
||||||
|
|
||||||
|
def publish_pose(self, value: DecodedPoseView) -> int:
|
||||||
|
runtime = self._active_runtime()
|
||||||
|
bindings = self._binder.publish_pose(value)
|
||||||
|
for binding in bindings:
|
||||||
|
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||||
|
return len(bindings)
|
||||||
|
|
||||||
|
def close(self, *, timeout_seconds: float = 30.0) -> None:
|
||||||
|
with self._lock:
|
||||||
|
if self._closed:
|
||||||
|
return
|
||||||
|
self._closed = True
|
||||||
|
runtime = self._runtime
|
||||||
|
if runtime is None:
|
||||||
|
return
|
||||||
|
for binding in self._binder.flush():
|
||||||
|
runtime.publish_views(binding.point_cloud, binding.pose)
|
||||||
|
runtime.close(timeout_seconds=timeout_seconds)
|
||||||
|
|
||||||
|
def snapshot(self) -> dict[str, object]:
|
||||||
|
with self._lock:
|
||||||
|
runtime = self._runtime
|
||||||
|
session_id = self._session_id
|
||||||
|
closed = self._closed
|
||||||
|
runtime_snapshot = runtime.snapshot() if runtime is not None else None
|
||||||
|
return {
|
||||||
|
"schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||||
|
"mode": "physical-live-shadow-diagnostic-only",
|
||||||
|
"session_id": session_id,
|
||||||
|
"binder": self._binder.snapshot(),
|
||||||
|
"runtime": runtime_snapshot,
|
||||||
|
"active": runtime is not None and not closed,
|
||||||
|
"closed": closed and (
|
||||||
|
runtime_snapshot is None or bool(runtime_snapshot["closed"])
|
||||||
|
),
|
||||||
|
"ground_truth": False,
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
def _active_runtime(self) -> K1LocalSurfaceShadowRuntime:
|
||||||
|
with self._lock:
|
||||||
|
if self._closed:
|
||||||
|
raise RuntimeError("K1 local-surface shadow coordinator is closed")
|
||||||
|
runtime = self._runtime
|
||||||
|
if runtime is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
"K1 local-surface shadow coordinator session is not active"
|
||||||
|
)
|
||||||
|
return runtime
|
||||||
|
|
||||||
|
|
||||||
|
def _bounded_distribution(
|
||||||
|
values: deque[float],
|
||||||
|
) -> dict[str, float | int | None]:
|
||||||
|
array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64)
|
||||||
|
return {
|
||||||
|
"sample_count": int(array.shape[0]),
|
||||||
|
"p50": float(np.percentile(array, 50)) if array.size else None,
|
||||||
|
"p95": float(np.percentile(array, 95)) if array.size else None,
|
||||||
|
"maximum": float(np.max(array)) if array.size else None,
|
||||||
|
}
|
||||||
|
|
|
||||||
|
|
@ -84,6 +84,138 @@ def test_e15_profile_rejects_command_authority(tmp_path: Path) -> None:
|
||||||
module.read_live_profile(changed)
|
module.read_live_profile(changed)
|
||||||
|
|
||||||
|
|
||||||
|
def test_e28_profile_pins_physical_k1_local_surface_gate() -> None:
|
||||||
|
module = _module()
|
||||||
|
profile_path = (
|
||||||
|
Path(__file__).resolve().parents[1]
|
||||||
|
/ "experiments"
|
||||||
|
/ "perception"
|
||||||
|
/ "worker"
|
||||||
|
/ "e28_physical_k1_local_surface_profile.json"
|
||||||
|
)
|
||||||
|
|
||||||
|
profile, digest = module.read_live_profile(profile_path)
|
||||||
|
|
||||||
|
assert len(digest) == 64
|
||||||
|
assert profile["mode"] == "physical-shadow-gate"
|
||||||
|
assert profile["local_surface"] == {
|
||||||
|
"enabled": True,
|
||||||
|
"profile_id": "k1-vendor-map-dynamic-local-surface/v1",
|
||||||
|
"profile_sha256": (
|
||||||
|
"7a59edc8404d0177a39175578743589bfb6b7822837170cded38ca2b6698cc26"
|
||||||
|
),
|
||||||
|
"point_queue_capacity": 2,
|
||||||
|
"pose_buffer_capacity": 16,
|
||||||
|
"future_pose_wait_ms": 25.0,
|
||||||
|
"retention_seconds": 3.0,
|
||||||
|
"result_capacity": 8,
|
||||||
|
"acceptance": {
|
||||||
|
"minimum_bound_frames": 100,
|
||||||
|
"maximum_pose_miss_fraction": 0.05,
|
||||||
|
"maximum_point_drop_fraction": 0.01,
|
||||||
|
"maximum_runtime_drop_fraction": 0.01,
|
||||||
|
"maximum_p95_result_age_ms": 80.0,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
assert profile["authority"] == {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def test_e28_profile_rejects_unpinned_local_surface(tmp_path: Path) -> None:
|
||||||
|
module = _module()
|
||||||
|
source = (
|
||||||
|
Path(__file__).resolve().parents[1]
|
||||||
|
/ "experiments"
|
||||||
|
/ "perception"
|
||||||
|
/ "worker"
|
||||||
|
/ "e28_physical_k1_local_surface_profile.json"
|
||||||
|
)
|
||||||
|
value = json.loads(source.read_text())
|
||||||
|
value["local_surface"]["profile_sha256"] = "0" * 64
|
||||||
|
changed = tmp_path / "unpinned-local-surface.json"
|
||||||
|
changed.write_text(json.dumps(value))
|
||||||
|
|
||||||
|
with pytest.raises(RuntimeError, match="E28 physical local-surface"):
|
||||||
|
module.read_live_profile(changed)
|
||||||
|
|
||||||
|
|
||||||
|
def test_e28_local_surface_acceptance_requires_exact_bounded_accounting() -> None:
|
||||||
|
module = _module()
|
||||||
|
config = {
|
||||||
|
"profile_id": "k1-vendor-map-dynamic-local-surface/v1",
|
||||||
|
"point_queue_capacity": 2,
|
||||||
|
"pose_buffer_capacity": 16,
|
||||||
|
"acceptance": {
|
||||||
|
"minimum_bound_frames": 100,
|
||||||
|
"maximum_pose_miss_fraction": 0.05,
|
||||||
|
"maximum_point_drop_fraction": 0.01,
|
||||||
|
"maximum_runtime_drop_fraction": 0.01,
|
||||||
|
"maximum_p95_result_age_ms": 80.0,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
authority = {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
}
|
||||||
|
snapshot = {
|
||||||
|
"closed": True,
|
||||||
|
"authority": authority,
|
||||||
|
"binder": {
|
||||||
|
"points": {
|
||||||
|
"capacity": 2,
|
||||||
|
"depth": 0,
|
||||||
|
"maximum_depth": 2,
|
||||||
|
"published": 102,
|
||||||
|
"bound": 100,
|
||||||
|
"missed": 1,
|
||||||
|
"dropped_overflow": 1,
|
||||||
|
},
|
||||||
|
"poses": {
|
||||||
|
"capacity": 16,
|
||||||
|
"maximum_depth": 12,
|
||||||
|
},
|
||||||
|
},
|
||||||
|
"runtime": {
|
||||||
|
"closed": True,
|
||||||
|
"profile": {
|
||||||
|
"profile_id": "k1-vendor-map-dynamic-local-surface/v1",
|
||||||
|
},
|
||||||
|
"queue": {
|
||||||
|
"capacity": 2,
|
||||||
|
"depth": 0,
|
||||||
|
"maximum_depth": 2,
|
||||||
|
"published": 100,
|
||||||
|
"consumed": 100,
|
||||||
|
"dropped_overflow": 0,
|
||||||
|
},
|
||||||
|
"results": {
|
||||||
|
"published": 100,
|
||||||
|
"failed": 0,
|
||||||
|
"result_age_ms": {"p95": 40.0},
|
||||||
|
},
|
||||||
|
"occupancy_policy": {
|
||||||
|
"absence_of_points_means_free": False,
|
||||||
|
"unknown_is_traversable": False,
|
||||||
|
},
|
||||||
|
"authority": authority,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
checks = module._local_surface_acceptance_checks(snapshot, config)
|
||||||
|
|
||||||
|
assert checks
|
||||||
|
assert all(checks.values())
|
||||||
|
snapshot["runtime"]["results"]["result_age_ms"]["p95"] = 90.0
|
||||||
|
assert (
|
||||||
|
module._local_surface_acceptance_checks(snapshot, config)[
|
||||||
|
"local_surface_maximum_p95_result_age_ms"
|
||||||
|
]
|
||||||
|
is False
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
def test_persistent_worker_request_is_single_run_d_backed_and_token_bounded(
|
def test_persistent_worker_request_is_single_run_d_backed_and_token_bounded(
|
||||||
tmp_path: Path,
|
tmp_path: Path,
|
||||||
) -> None:
|
) -> None:
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,8 @@
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import importlib.util
|
import importlib.util
|
||||||
|
import os
|
||||||
|
import subprocess
|
||||||
import sys
|
import sys
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
@ -29,10 +31,33 @@ def test_e15_worker_package_is_minimal_hash_addressed_projection(tmp_path: Path)
|
||||||
|
|
||||||
assert package.name == f"e15-worker-package-{manifest['identity_sha256']}"
|
assert package.name == f"e15-worker-package-{manifest['identity_sha256']}"
|
||||||
assert manifest["identity"]["classification"] == ("minimal-live-worker-import-projection")
|
assert manifest["identity"]["classification"] == ("minimal-live-worker-import-projection")
|
||||||
assert len(manifest["artifacts"]) == 12
|
assert len(manifest["artifacts"]) == 15
|
||||||
assert (package / "k1link" / "compute" / "inline_temporal.py").is_file()
|
assert (package / "k1link" / "compute" / "inline_temporal.py").is_file()
|
||||||
|
assert (
|
||||||
|
package
|
||||||
|
/ "k1link"
|
||||||
|
/ "compute"
|
||||||
|
/ "lidar_local_surface_shadow.py"
|
||||||
|
).is_file()
|
||||||
assert not (package / "k1link" / "device_plugins" / "xgrids_k1" / "mqtt").exists()
|
assert not (package / "k1link" / "device_plugins" / "xgrids_k1" / "mqtt").exists()
|
||||||
assert (
|
assert (
|
||||||
"observation"
|
"observation"
|
||||||
not in (package / "k1link" / "device_plugins" / "xgrids_k1" / "__init__.py").read_text()
|
not in (package / "k1link" / "device_plugins" / "xgrids_k1" / "__init__.py").read_text()
|
||||||
)
|
)
|
||||||
|
imported = subprocess.run(
|
||||||
|
[
|
||||||
|
sys.executable,
|
||||||
|
"-c",
|
||||||
|
(
|
||||||
|
"from k1link.compute.lidar_local_surface_shadow "
|
||||||
|
"import K1LocalSurfaceShadowCoordinator; "
|
||||||
|
"assert K1LocalSurfaceShadowCoordinator"
|
||||||
|
),
|
||||||
|
],
|
||||||
|
cwd=tmp_path,
|
||||||
|
env={**os.environ, "PYTHONPATH": str(package)},
|
||||||
|
check=False,
|
||||||
|
capture_output=True,
|
||||||
|
text=True,
|
||||||
|
)
|
||||||
|
assert imported.returncode == 0, imported.stderr
|
||||||
|
|
|
||||||
|
|
@ -19,6 +19,12 @@ from k1link.compute import (
|
||||||
K1LocalSurfaceV1,
|
K1LocalSurfaceV1,
|
||||||
build_k1_local_surface,
|
build_k1_local_surface,
|
||||||
)
|
)
|
||||||
|
from k1link.compute.lidar_local_surface import (
|
||||||
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE as REPLAY_LOCAL_SURFACE_PROFILE,
|
||||||
|
)
|
||||||
|
from k1link.compute.lidar_local_surface_geometry import (
|
||||||
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE as WORKER_LOCAL_SURFACE_PROFILE,
|
||||||
|
)
|
||||||
from k1link.web.lidar_api import build_lidar_router
|
from k1link.web.lidar_api import build_lidar_router
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -36,6 +42,10 @@ def _sha256(path: Path) -> str:
|
||||||
return hashlib.sha256(path.read_bytes()).hexdigest()
|
return hashlib.sha256(path.read_bytes()).hexdigest()
|
||||||
|
|
||||||
|
|
||||||
|
def test_replay_and_minimal_worker_pin_the_same_local_surface_profile() -> None:
|
||||||
|
assert WORKER_LOCAL_SURFACE_PROFILE.to_dict() == REPLAY_LOCAL_SURFACE_PROFILE.to_dict()
|
||||||
|
|
||||||
|
|
||||||
def _source_pack(root: Path) -> Path:
|
def _source_pack(root: Path) -> Path:
|
||||||
frame_count = 8
|
frame_count = 8
|
||||||
available = np.asarray([True, True, True, True, True, True, True, False])
|
available = np.asarray([True, True, True, True, True, True, True, False])
|
||||||
|
|
|
||||||
|
|
@ -3,6 +3,12 @@ from __future__ import annotations
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from k1link.compute import (
|
||||||
|
K1LocalSurfacePoseBinder,
|
||||||
|
K1LocalSurfaceShadowCoordinator,
|
||||||
|
)
|
||||||
from k1link.compute.live_perception import LiveSensorSynchronizer
|
from k1link.compute.live_perception import LiveSensorSynchronizer
|
||||||
from k1link.data_plane import ConsumerFrameContext, DecodedPointCloudView, DecodedPoseView
|
from k1link.data_plane import ConsumerFrameContext, DecodedPointCloudView, DecodedPoseView
|
||||||
|
|
||||||
|
|
@ -66,10 +72,7 @@ def test_live_sensor_synchronizer_reports_each_fail_closed_boundary() -> None:
|
||||||
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-unavailable"
|
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-unavailable"
|
||||||
|
|
||||||
synchronizer.publish_point_cloud(_points(1, 900_000_000))
|
synchronizer.publish_point_cloud(_points(1, 900_000_000))
|
||||||
assert (
|
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-camera-delta-exceeded"
|
||||||
synchronizer.bind_camera(1_000_000_000).state
|
|
||||||
== "lidar-camera-delta-exceeded"
|
|
||||||
)
|
|
||||||
|
|
||||||
synchronizer.publish_point_cloud(_points(2, 1_000_000_000))
|
synchronizer.publish_point_cloud(_points(2, 1_000_000_000))
|
||||||
assert synchronizer.bind_camera(1_000_000_000).state == "pose-unavailable"
|
assert synchronizer.bind_camera(1_000_000_000).state == "pose-unavailable"
|
||||||
|
|
@ -116,3 +119,80 @@ def test_live_sensor_synchronizer_storage_never_exceeds_capacity() -> None:
|
||||||
assert snapshot["pose_depth"] == 3
|
assert snapshot["pose_depth"] == 3
|
||||||
assert snapshot["evicted_points"] == 4
|
assert snapshot["evicted_points"] == 4
|
||||||
assert snapshot["evicted_poses"] == 4
|
assert snapshot["evicted_poses"] == 4
|
||||||
|
|
||||||
|
|
||||||
|
def test_local_surface_pose_binder_accepts_either_event_order() -> None:
|
||||||
|
points_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
||||||
|
assert points_first.publish_point_cloud(_points(1, 1_000_000_000)) == ()
|
||||||
|
bound = points_first.publish_pose(_pose(1, 1_010_000_000))
|
||||||
|
assert len(bound) == 1
|
||||||
|
assert bound[0].point_cloud.context.sequence == 1
|
||||||
|
assert bound[0].pose.context.sequence == 1
|
||||||
|
assert bound[0].pose_binding_age_ms == 10.0
|
||||||
|
|
||||||
|
pose_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
||||||
|
assert pose_first.publish_pose(_pose(1, 2_000_000_000)) == ()
|
||||||
|
bound = pose_first.publish_point_cloud(_points(1, 2_010_000_000))
|
||||||
|
assert len(bound) == 1
|
||||||
|
assert bound[0].pose_binding_age_ms == 10.0
|
||||||
|
|
||||||
|
snapshot = pose_first.snapshot()
|
||||||
|
assert snapshot["points"]["published"] == 1
|
||||||
|
assert snapshot["points"]["bound"] == 1
|
||||||
|
assert snapshot["points"]["missed"] == 0
|
||||||
|
assert snapshot["authority"]["commands_enabled"] is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_local_surface_pose_binder_is_bounded_and_accounts_for_misses() -> None:
|
||||||
|
binder = K1LocalSurfacePoseBinder(
|
||||||
|
maximum_pose_binding_ms=100,
|
||||||
|
point_capacity=2,
|
||||||
|
pose_capacity=3,
|
||||||
|
retention_seconds=10,
|
||||||
|
)
|
||||||
|
for sequence in range(1, 6):
|
||||||
|
assert (
|
||||||
|
binder.publish_point_cloud(_points(sequence, 1_000_000_000 + sequence * 1_000_000))
|
||||||
|
== ()
|
||||||
|
)
|
||||||
|
assert binder.flush() == ()
|
||||||
|
snapshot = binder.snapshot()
|
||||||
|
points = snapshot["points"]
|
||||||
|
assert points["capacity"] == 2
|
||||||
|
assert points["maximum_depth"] == 2
|
||||||
|
assert points["published"] == 5
|
||||||
|
assert points["bound"] == 0
|
||||||
|
assert points["dropped_overflow"] == 3
|
||||||
|
assert points["missed"] == 2
|
||||||
|
assert (
|
||||||
|
points["bound"] + points["dropped_overflow"] + points["missed"] + points["depth"]
|
||||||
|
== points["published"]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_local_surface_pose_binder_fails_closed_after_pose_deadline() -> None:
|
||||||
|
binder = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
||||||
|
assert binder.publish_point_cloud(_points(1, 1_000_000_000)) == ()
|
||||||
|
assert binder.publish_pose(_pose(1, 1_200_000_000)) == ()
|
||||||
|
snapshot = binder.snapshot()
|
||||||
|
assert snapshot["points"]["missed"] == 1
|
||||||
|
assert snapshot["points"]["depth"] == 0
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="sequence"):
|
||||||
|
binder.publish_pose(_pose(1, 1_210_000_000))
|
||||||
|
|
||||||
|
|
||||||
|
def test_local_surface_shadow_coordinator_closes_exact_accounting() -> None:
|
||||||
|
coordinator = K1LocalSurfaceShadowCoordinator(result_capacity=2)
|
||||||
|
coordinator.begin_session("physical-k1-shadow-test")
|
||||||
|
assert coordinator.publish_pose(_pose(1, 1_000_000_000)) == 0
|
||||||
|
assert coordinator.publish_point_cloud(_points(1, 1_010_000_000)) == 1
|
||||||
|
coordinator.close()
|
||||||
|
|
||||||
|
snapshot = coordinator.snapshot()
|
||||||
|
assert snapshot["closed"] is True
|
||||||
|
assert snapshot["binder"]["points"]["bound"] == 1
|
||||||
|
assert snapshot["runtime"]["queue"]["published"] == 1
|
||||||
|
assert snapshot["runtime"]["queue"]["consumed"] == 1
|
||||||
|
assert snapshot["runtime"]["results"]["failed"] == 0
|
||||||
|
assert snapshot["authority"]["navigation_or_safety_accepted"] is False
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue