138 lines
4.4 KiB
Python
138 lines
4.4 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import replace
|
|
from pathlib import Path
|
|
|
|
from k1link.perception.contracts import (
|
|
ClockBasis,
|
|
GridCell,
|
|
HistorySample,
|
|
ModalityOutcome,
|
|
ModalityStatus,
|
|
MotionState,
|
|
SourceEnvelope,
|
|
TemporalObstacle,
|
|
TemporalState,
|
|
TimestampBundle,
|
|
)
|
|
from k1link.perception.providers import SourcePacket
|
|
from k1link.perception.rolling_map import (
|
|
RollingLocalObstacleMapProvider,
|
|
load_rolling_map_profile,
|
|
)
|
|
|
|
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
|
PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-rolling-local-map-v1.json"
|
|
|
|
|
|
class _Pose:
|
|
def pose_values_for_frame(
|
|
self,
|
|
frame_id: str,
|
|
) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]:
|
|
return (0.0, 0.0, 1.25), (0.0, 0.0, 0.0, 1.0)
|
|
|
|
|
|
def _status() -> ModalityStatus:
|
|
return ModalityStatus(True, ModalityOutcome.AVAILABLE, "test-available")
|
|
|
|
|
|
def _packet(sequence: int, seconds: float) -> SourcePacket:
|
|
return SourcePacket(
|
|
envelope=SourceEnvelope(
|
|
source_id="RAVNOVES00",
|
|
session_id="20260720T065719Z_viewer_live",
|
|
frame_id=f"frame-{sequence:06d}",
|
|
sequence=sequence,
|
|
timestamps=TimestampBundle(
|
|
utc_ns=round(seconds * 1_000_000_000),
|
|
monotonic_ns=round(seconds * 1_000_000_000),
|
|
source_ns=round(seconds * 1_000_000_000),
|
|
clock_basis=ClockBasis.RECORDED_HOST,
|
|
),
|
|
source_age_ns=0,
|
|
binding_reason="test-source",
|
|
calibration_id="test-calibration",
|
|
representation_id="registered-map-increment-v1",
|
|
image=_status(),
|
|
registered_point_increment=_status(),
|
|
pose=_status(),
|
|
),
|
|
image_payload="image",
|
|
registered_point_increment_payload="points",
|
|
pose_payload="pose",
|
|
)
|
|
|
|
|
|
def _current(packet: SourcePacket) -> TemporalObstacle:
|
|
return TemporalObstacle(
|
|
component_id=f"temporal-{packet.envelope.sequence:08d}",
|
|
identity_scope="ephemeral",
|
|
state=TemporalState.CURRENT,
|
|
ttl_ns=750_000_000,
|
|
last_hit_ns=packet.envelope.timestamps.source_ns,
|
|
age_ns=0,
|
|
association_basis="new-spatial-hit",
|
|
history=(
|
|
HistorySample(
|
|
frame_id=packet.envelope.frame_id,
|
|
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
|
centroid_xyz_m=(0.675, 0.225, 0.225),
|
|
),
|
|
),
|
|
cells=(GridCell(1, 0, 0), GridCell(2, 0, 0)),
|
|
coordinate_frame="map",
|
|
last_centroid_xyz_m=(0.675, 0.225, 0.225),
|
|
motion=MotionState.UNKNOWN,
|
|
motion_confidence=0.0,
|
|
motion_reason="motion-not-estimated",
|
|
)
|
|
|
|
|
|
def test_registered_increment_is_retained_without_claiming_current_motion() -> None:
|
|
provider = RollingLocalObstacleMapProvider(
|
|
pose_resolver=_Pose(),
|
|
profile=load_rolling_map_profile(PROFILE_PATH),
|
|
)
|
|
first = _packet(0, 0.0)
|
|
assert provider.update(first, (_current(first),)) == ()
|
|
|
|
second = _packet(1, 1.0)
|
|
retained = provider.update(second, ())
|
|
|
|
assert len(retained) == 1
|
|
assert retained[0].state is TemporalState.RETAINED
|
|
assert retained[0].age_ns == 1_000_000_000
|
|
assert retained[0].cells == (GridCell(1, 0, 0), GridCell(2, 0, 0))
|
|
assert retained[0].motion is MotionState.UNKNOWN
|
|
assert retained[0].association_basis == "registered-map-increment-retention"
|
|
|
|
|
|
def test_current_republication_is_not_duplicated_as_retained_occupancy() -> None:
|
|
provider = RollingLocalObstacleMapProvider(
|
|
pose_resolver=_Pose(),
|
|
profile=load_rolling_map_profile(PROFILE_PATH),
|
|
)
|
|
first = _packet(0, 0.0)
|
|
provider.update(first, (_current(first),))
|
|
second = _packet(1, 1.0)
|
|
|
|
assert provider.update(
|
|
second,
|
|
(replace(_current(second), component_id="temporal-00000099"),),
|
|
) == ()
|
|
|
|
|
|
def test_retained_occupancy_expires_only_at_explicit_time_bound() -> None:
|
|
provider = RollingLocalObstacleMapProvider(
|
|
pose_resolver=_Pose(),
|
|
profile=load_rolling_map_profile(PROFILE_PATH),
|
|
)
|
|
first = _packet(0, 0.0)
|
|
provider.update(first, (_current(first),))
|
|
assert provider.update(_packet(1, 3.0), ())
|
|
assert provider.update(_packet(2, 3.1), ()) == ()
|
|
snapshot = provider.snapshot()
|
|
assert snapshot.time_evicted_cells == 2
|
|
assert snapshot.capacity_evicted_cells == 0
|