feat: wire physical K1 surface shadow
This commit is contained in:
DCCONSTRUCTIONS
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,
|
||||
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
|
||||
`2,413`-frame validation split is available in **Полигон → Датасеты**. The
|
||||
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
|
||||
`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`.
|
||||
|
||||
The next gate is not another replay tuning pass. It is an explicit bounded
|
||||
LiDAR↔pose binder on the authenticated external worker stream followed by a
|
||||
physical K1 shadow run, still without commands, free-space, navigation or
|
||||
safety authority.
|
||||
The authenticated external worker now contains the next bounded seam.
|
||||
`missioncore.k1-local-surface-pose-binder/v1` accepts decoded map-frame
|
||||
`lio_pcl` and `lio_pose` in either arrival order, binds by bounded worker
|
||||
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
|
||||
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
|
||||
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.
|
||||
- [ ] Fuse K1 geometric evidence with E26 camera evidence as independent
|
||||
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
|
||||
residual explainability now shows that episode `09` is a localized positive
|
||||
structure rather than symmetric plane drift. The highest-value immediate work
|
||||
is therefore a bounded live-shadow queue, followed by a separate
|
||||
dynamic-observation layer. Nvblox,
|
||||
is therefore the physical K1 run through the now-implemented bounded
|
||||
live-shadow seam, followed by a separate dynamic-observation layer. Nvblox,
|
||||
raw-scan detectors and alternative SLAM remain optional later gates because the
|
||||
current report contract does not carry their required ray/timing semantics.
|
||||
|
|
|
|||
|
|
@ -92,10 +92,12 @@ Not implemented:
|
|||
- no model training or production promotion;
|
||||
- no RELLIS ROS bag admission, continuous synchronized playback or production
|
||||
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
|
||||
recorded-source-paced bounded shadow gate; physical K1 worker binding is not
|
||||
implemented yet, and the residual overlay remains non-authoritative.
|
||||
recorded-source-paced bounded shadow gate; authenticated physical
|
||||
`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
|
||||
|
||||
|
|
|
|||
|
|
@ -17,9 +17,12 @@ SCHEMA = "missioncore.e15-worker-package/v1"
|
|||
COPIED_FILES = (
|
||||
"k1link/__init__.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/data_plane/__init__.py",
|
||||
"k1link/data_plane/views.py",
|
||||
"k1link/ground_segmentation.py",
|
||||
"k1link/device_plugins/__init__.py",
|
||||
"k1link/device_plugins/xgrids_k1/protocol/__init__.py",
|
||||
"k1link/device_plugins/xgrids_k1/protocol/normalizer.py",
|
||||
|
|
@ -71,6 +74,7 @@ def _identity(source_root: Path) -> dict[str, Any]:
|
|||
"imports": [
|
||||
"k1link.compute.inline_temporal.TemporalStabilizer",
|
||||
"k1link.compute.inline_temporal.StreamingSemanticStabilizer",
|
||||
"k1link.compute.lidar_local_surface_shadow.K1LocalSurfaceShadowCoordinator",
|
||||
"k1link.compute.live_perception.LiveSensorSynchronizer",
|
||||
"k1link.data_plane.DecodedPointCloudView",
|
||||
"k1link.data_plane.DecodedPoseView",
|
||||
|
|
|
|||
|
|
@ -136,11 +136,26 @@ if (
|
|||
$live = Get-Content -LiteralPath $liveProfile -Raw | ConvertFrom-Json
|
||||
if (
|
||||
$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.navigation_or_safety_accepted -or
|
||||
$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) {
|
||||
$stability = Get-Content -LiteralPath $stabilityProfile -Raw | ConvertFrom-Json
|
||||
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")
|
||||
scheduling = profile.get("scheduling")
|
||||
temporal = profile.get("temporal")
|
||||
local_surface = profile.get("local_surface")
|
||||
acceptance = profile.get("acceptance")
|
||||
if (
|
||||
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
|
||||
):
|
||||
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 = (
|
||||
"detector_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)
|
||||
|
||||
|
||||
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(
|
||||
root: Path, expected_calibration_sha256: str
|
||||
) -> 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 resolved.name != manifest.get("package_id")
|
||||
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")
|
||||
expected = set()
|
||||
|
|
@ -568,6 +760,7 @@ def _receiver(
|
|||
max_duration_seconds: float,
|
||||
decoder: PersistentFmp4Decoder,
|
||||
synchronizer: LiveSensorSynchronizer,
|
||||
local_surface: Any | None,
|
||||
lidar_quality: LidarQualityMonitor,
|
||||
state: _TransportState,
|
||||
sensor_decode_ms: dict[str, list[float]],
|
||||
|
|
@ -646,6 +839,8 @@ def _receiver(
|
|||
session_id = str(header["session_id"])
|
||||
if state.session_id is None:
|
||||
state.session_id = session_id
|
||||
if local_surface is not None:
|
||||
local_surface.begin_session(session_id)
|
||||
elif state.session_id != session_id:
|
||||
raise ShadowRuntimeError("shadow session identity changed")
|
||||
modality = str(header["modality"])
|
||||
|
|
@ -698,8 +893,12 @@ def _receiver(
|
|||
if modality == "lidar" and isinstance(normalized, DecodedPointCloudView):
|
||||
lidar_quality.observe(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):
|
||||
synchronizer.publish_pose(normalized)
|
||||
if local_surface is not None:
|
||||
local_surface.publish_pose(normalized)
|
||||
else:
|
||||
raise ShadowRuntimeError("known sensor modality did not normalize")
|
||||
else:
|
||||
|
|
@ -709,6 +908,12 @@ def _receiver(
|
|||
state.failures.append(exc)
|
||||
finally:
|
||||
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()
|
||||
if sender_thread is not None:
|
||||
sender_thread.join(timeout=5)
|
||||
|
|
@ -765,6 +970,21 @@ def _common(args: argparse.Namespace) -> dict[str, Any]:
|
|||
if dependency["identity"]["profile_sha256"] != semantic_sha256:
|
||||
raise RuntimeError("LAB E15 semantic dependency identity changed")
|
||||
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_sha256 = 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"]),
|
||||
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)
|
||||
first_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"
|
||||
run_started = time.perf_counter()
|
||||
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 (
|
||||
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(
|
||||
runtime_stream,
|
||||
interval_seconds=1.0,
|
||||
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,
|
||||
},
|
||||
},
|
||||
snapshotters=runtime_snapshotters,
|
||||
) as runtime_telemetry,
|
||||
):
|
||||
|
||||
|
|
@ -1111,6 +1350,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
|||
"max_duration_seconds": args.max_duration_seconds,
|
||||
"decoder": decoder,
|
||||
"synchronizer": synchronizer,
|
||||
"local_surface": local_surface,
|
||||
"lidar_quality": lidar_quality,
|
||||
"state": transport,
|
||||
"sensor_decode_ms": sensor_decode_ms,
|
||||
|
|
@ -1400,6 +1640,9 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
|||
temporal_semantic_summary = (
|
||||
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"]
|
||||
checks = {
|
||||
"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"]
|
||||
== {"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:
|
||||
temporal_acceptance = stability["acceptance"]
|
||||
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"],
|
||||
},
|
||||
"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": {
|
||||
"id": common["worker_package"]["package_id"],
|
||||
"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,
|
||||
},
|
||||
"lidar_quality": lidar_quality.snapshot(),
|
||||
"local_surface": local_surface_snapshot,
|
||||
"latency_ms": latency_summary,
|
||||
"temporal_stability": {
|
||||
"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.",
|
||||
"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 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.",
|
||||
"COCO and Cityscapes models are not forest-domain or safety validated.",
|
||||
"Amodal cuboids infer unobserved volume from class priors.",
|
||||
|
|
|
|||
|
|
@ -119,8 +119,12 @@ from .lidar_local_surface import (
|
|||
k1_local_surface_catalog_item,
|
||||
)
|
||||
from .lidar_local_surface_shadow import (
|
||||
K1_LOCAL_SURFACE_BINDER_SCHEMA,
|
||||
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
||||
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||
K1LocalSurfaceBoundViews,
|
||||
K1LocalSurfacePoseBinder,
|
||||
K1LocalSurfaceShadowCoordinator,
|
||||
K1LocalSurfaceShadowEstimator,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowResult,
|
||||
|
|
@ -217,6 +221,7 @@ __all__ = [
|
|||
"LIDAR_GROUND_BENCHMARK_SCHEMA",
|
||||
"LIDAR_GROUND_FRAME_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_FRAME_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_BINDER_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_REPORT_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_SCHEMA",
|
||||
|
|
@ -246,6 +251,9 @@ __all__ = [
|
|||
"LidarGroundBenchmarkV1",
|
||||
"LidarGroundError",
|
||||
"K1LocalSurfaceProfile",
|
||||
"K1LocalSurfaceBoundViews",
|
||||
"K1LocalSurfacePoseBinder",
|
||||
"K1LocalSurfaceShadowCoordinator",
|
||||
"K1LocalSurfaceShadowEstimator",
|
||||
"K1LocalSurfaceShadowInput",
|
||||
"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.ground_segmentation import GroundSegmentationError as LidarGroundError
|
||||
|
||||
from .lidar_local_surface import (
|
||||
from .lidar_local_surface_geometry import (
|
||||
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||
POINT_BELOW_SURFACE,
|
||||
POINT_OCCUPIED,
|
||||
POINT_SURFACE,
|
||||
K1LocalSurfaceProfile,
|
||||
_cloud_cell_observations,
|
||||
_expire_cache,
|
||||
_fit_surface,
|
||||
_height_above_plane,
|
||||
_local_cache_records,
|
||||
_point_step_candidates,
|
||||
_prediction_metrics,
|
||||
_step_candidate_keys,
|
||||
_update_cache,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
cloud_cell_observations as _cloud_cell_observations,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
expire_cache as _expire_cache,
|
||||
)
|
||||
from .lidar_local_surface_geometry import (
|
||||
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
|
||||
|
||||
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_BINDER_SCHEMA: Final = "missioncore.k1-local-surface-pose-binder/v1"
|
||||
|
||||
ShadowFrameState = Literal[
|
||||
"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)
|
||||
class K1LocalSurfaceShadowInput:
|
||||
"""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,
|
||||
points_map=points,
|
||||
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._failed = 0
|
||||
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._inflight = False
|
||||
self._condition = threading.Condition()
|
||||
|
|
@ -638,6 +863,8 @@ class K1LocalSurfaceShadowRuntime:
|
|||
"dropped_ring_overflow": self._result_dropped,
|
||||
"state_counts": dict(sorted(self._state_counts.items())),
|
||||
"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,
|
||||
},
|
||||
"last_error": self._last_error,
|
||||
|
|
@ -675,7 +902,128 @@ class K1LocalSurfaceShadowRuntime:
|
|||
self._results.append(result)
|
||||
self._processed += 1
|
||||
self._state_counts[result.state] += 1
|
||||
self._processing_ms.append(result.processing_ms)
|
||||
self._result_age_ms.append(result.result_age_ms)
|
||||
finally:
|
||||
with self._condition:
|
||||
self._inflight = False
|
||||
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)
|
||||
|
||||
|
||||
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(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
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 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"
|
||||
/ "lidar_local_surface_shadow.py"
|
||||
).is_file()
|
||||
assert not (package / "k1link" / "device_plugins" / "xgrids_k1" / "mqtt").exists()
|
||||
assert (
|
||||
"observation"
|
||||
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,
|
||||
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
|
||||
|
||||
|
||||
|
|
@ -36,6 +42,10 @@ def _sha256(path: Path) -> str:
|
|||
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:
|
||||
frame_count = 8
|
||||
available = np.asarray([True, True, True, True, True, True, True, False])
|
||||
|
|
|
|||
|
|
@ -3,6 +3,12 @@ from __future__ import annotations
|
|||
import threading
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from k1link.compute import (
|
||||
K1LocalSurfacePoseBinder,
|
||||
K1LocalSurfaceShadowCoordinator,
|
||||
)
|
||||
from k1link.compute.live_perception import LiveSensorSynchronizer
|
||||
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"
|
||||
|
||||
synchronizer.publish_point_cloud(_points(1, 900_000_000))
|
||||
assert (
|
||||
synchronizer.bind_camera(1_000_000_000).state
|
||||
== "lidar-camera-delta-exceeded"
|
||||
)
|
||||
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-camera-delta-exceeded"
|
||||
|
||||
synchronizer.publish_point_cloud(_points(2, 1_000_000_000))
|
||||
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["evicted_points"] == 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