mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 08:41:13 +00:00
[AZ-228] [AZ-229] Add VIO and satellite sync boundaries
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -1 +1,24 @@
|
||||
"""Offline satellite retrieval and synchronization component."""
|
||||
|
||||
from .interfaces import SatelliteService, SatelliteSyncBoundary
|
||||
from .types import (
|
||||
GeneratedTileUploadRecord,
|
||||
MissionCacheImportResult,
|
||||
MissionCachePackage,
|
||||
RuntimePhase,
|
||||
SatelliteSyncResult,
|
||||
SatelliteSyncStatus,
|
||||
UploadOutcome,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"GeneratedTileUploadRecord",
|
||||
"MissionCacheImportResult",
|
||||
"MissionCachePackage",
|
||||
"RuntimePhase",
|
||||
"SatelliteService",
|
||||
"SatelliteSyncBoundary",
|
||||
"SatelliteSyncResult",
|
||||
"SatelliteSyncStatus",
|
||||
"UploadOutcome",
|
||||
]
|
||||
|
||||
@@ -1,7 +1,21 @@
|
||||
"""Public satellite service interfaces."""
|
||||
|
||||
from collections.abc import Callable
|
||||
from typing import Any, Protocol
|
||||
|
||||
from shared.errors import ErrorEnvelope
|
||||
from tile_manager import GeneratedTileSyncPackage
|
||||
|
||||
from .types import (
|
||||
GeneratedTileUploadRecord,
|
||||
MissionCacheImportResult,
|
||||
MissionCachePackage,
|
||||
RuntimePhase,
|
||||
SatelliteSyncResult,
|
||||
SatelliteSyncStatus,
|
||||
UploadOutcome,
|
||||
)
|
||||
|
||||
|
||||
class SatelliteService(Protocol):
|
||||
"""Retrieves offline VPR candidates from mission cache data."""
|
||||
@@ -11,3 +25,87 @@ class SatelliteService(Protocol):
|
||||
|
||||
def retrieve(self, frame: Any) -> list[Any]:
|
||||
"""Return candidate anchor records for one frame."""
|
||||
|
||||
|
||||
class SatelliteSyncBoundary:
|
||||
"""Owns pre-flight and post-flight package exchange only."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
uploader: Callable[[GeneratedTileSyncPackage], UploadOutcome] | None = None,
|
||||
) -> None:
|
||||
self._uploader = uploader or self._default_uploader
|
||||
self._imports: dict[str, MissionCachePackage] = {}
|
||||
self._upload_records: list[GeneratedTileUploadRecord] = []
|
||||
|
||||
def import_mission_cache(
|
||||
self,
|
||||
package: MissionCachePackage,
|
||||
phase: RuntimePhase = "pre_flight",
|
||||
) -> MissionCacheImportResult:
|
||||
if phase != "pre_flight":
|
||||
return MissionCacheImportResult(
|
||||
package_id=package.package_id,
|
||||
mission_id=package.mission_id,
|
||||
ready_for_tile_validation=False,
|
||||
error=self._phase_error("mission cache import", phase),
|
||||
)
|
||||
|
||||
self._imports[package.package_id] = package
|
||||
return MissionCacheImportResult(
|
||||
package_id=package.package_id,
|
||||
mission_id=package.mission_id,
|
||||
ready_for_tile_validation=True,
|
||||
manifest_entries=package.manifest_entries,
|
||||
)
|
||||
|
||||
def upload_generated_tiles(
|
||||
self,
|
||||
package: GeneratedTileSyncPackage,
|
||||
phase: RuntimePhase = "post_flight",
|
||||
) -> SatelliteSyncResult:
|
||||
if phase != "post_flight":
|
||||
return SatelliteSyncResult(error=self._phase_error("generated tile upload", phase))
|
||||
|
||||
if not package.sidecars:
|
||||
record = GeneratedTileUploadRecord(
|
||||
package_ref=package.package_ref,
|
||||
mission_id=package.mission_id,
|
||||
status="rejected",
|
||||
reason="empty_generated_tile_package",
|
||||
retained_for_retry=False,
|
||||
)
|
||||
else:
|
||||
outcome = self._uploader(package)
|
||||
record = GeneratedTileUploadRecord(
|
||||
package_ref=package.package_ref,
|
||||
mission_id=package.mission_id,
|
||||
status=outcome,
|
||||
reason=outcome,
|
||||
retained_for_retry=outcome == "retryable_failure",
|
||||
)
|
||||
|
||||
self._upload_records.append(record)
|
||||
return SatelliteSyncResult(upload_record=record)
|
||||
|
||||
def status(self) -> SatelliteSyncStatus:
|
||||
return SatelliteSyncStatus(
|
||||
imported_package_ids=tuple(self._imports),
|
||||
upload_records=tuple(self._upload_records),
|
||||
retry_package_refs=tuple(
|
||||
record.package_ref for record in self._upload_records if record.retained_for_retry
|
||||
),
|
||||
)
|
||||
|
||||
def _phase_error(self, operation: str, phase: RuntimePhase) -> ErrorEnvelope:
|
||||
return ErrorEnvelope(
|
||||
component="satellite_service",
|
||||
category="security",
|
||||
message=f"{operation} is not allowed during {phase}",
|
||||
severity="warning",
|
||||
retryable=False,
|
||||
cause="mid_flight_network_blocked" if phase == "in_flight" else "phase_not_allowed",
|
||||
)
|
||||
|
||||
def _default_uploader(self, package: GeneratedTileSyncPackage) -> UploadOutcome:
|
||||
return "success"
|
||||
|
||||
@@ -1,5 +1,49 @@
|
||||
"""Public satellite service type aliases."""
|
||||
"""Public satellite service models."""
|
||||
|
||||
from typing import Any
|
||||
from typing import Literal
|
||||
|
||||
VprCandidateLike = Any
|
||||
from pydantic import BaseModel, ConfigDict, Field
|
||||
|
||||
from shared.errors import ErrorEnvelope
|
||||
from tile_manager import TileManifestEntry
|
||||
|
||||
|
||||
class SatelliteServiceModel(BaseModel):
|
||||
model_config = ConfigDict(extra="forbid", frozen=True)
|
||||
|
||||
|
||||
class MissionCachePackage(SatelliteServiceModel):
|
||||
package_id: str = Field(min_length=1)
|
||||
mission_id: str = Field(min_length=1)
|
||||
manifest_entries: tuple[TileManifestEntry, ...] = Field(min_length=1)
|
||||
|
||||
|
||||
class MissionCacheImportResult(SatelliteServiceModel):
|
||||
package_id: str = Field(min_length=1)
|
||||
mission_id: str = Field(min_length=1)
|
||||
ready_for_tile_validation: bool
|
||||
manifest_entries: tuple[TileManifestEntry, ...] = ()
|
||||
error: ErrorEnvelope | None = None
|
||||
|
||||
|
||||
class GeneratedTileUploadRecord(SatelliteServiceModel):
|
||||
package_ref: str = Field(min_length=1)
|
||||
mission_id: str = Field(min_length=1)
|
||||
status: Literal["uploaded", "rejected", "retryable_failure"]
|
||||
reason: str
|
||||
retained_for_retry: bool
|
||||
|
||||
|
||||
class SatelliteSyncStatus(SatelliteServiceModel):
|
||||
imported_package_ids: tuple[str, ...]
|
||||
upload_records: tuple[GeneratedTileUploadRecord, ...]
|
||||
retry_package_refs: tuple[str, ...]
|
||||
|
||||
|
||||
class SatelliteSyncResult(SatelliteServiceModel):
|
||||
upload_record: GeneratedTileUploadRecord | None = None
|
||||
error: ErrorEnvelope | None = None
|
||||
|
||||
|
||||
RuntimePhase = Literal["pre_flight", "in_flight", "post_flight"]
|
||||
UploadOutcome = Literal["success", "retryable_failure", "rejected"]
|
||||
|
||||
@@ -1 +1,15 @@
|
||||
"""Replaceable VIO adapter component."""
|
||||
|
||||
from .interfaces import DeterministicVioBackend, LocalVioAdapter, VioAdapter, VioBackend
|
||||
from .types import VioBackendEstimate, VioHealthReport, VioInputPacket, VioProcessingResult
|
||||
|
||||
__all__ = [
|
||||
"DeterministicVioBackend",
|
||||
"LocalVioAdapter",
|
||||
"VioAdapter",
|
||||
"VioBackend",
|
||||
"VioBackendEstimate",
|
||||
"VioHealthReport",
|
||||
"VioInputPacket",
|
||||
"VioProcessingResult",
|
||||
]
|
||||
|
||||
@@ -2,6 +2,17 @@
|
||||
|
||||
from typing import Any, Protocol
|
||||
|
||||
from shared.contracts import VioStatePacket
|
||||
from shared.errors import ErrorEnvelope
|
||||
from shared.time_sync import select_time_window
|
||||
|
||||
from .types import (
|
||||
VioBackendEstimate,
|
||||
VioHealthReport,
|
||||
VioInputPacket,
|
||||
VioProcessingResult,
|
||||
)
|
||||
|
||||
|
||||
class VioAdapter(Protocol):
|
||||
"""Processes frame and telemetry inputs into relative VIO state."""
|
||||
@@ -9,5 +20,129 @@ class VioAdapter(Protocol):
|
||||
def initialize(self) -> None:
|
||||
"""Initialize adapter resources."""
|
||||
|
||||
def process(self, frame: Any, telemetry: Any) -> Any:
|
||||
def process(self, packet: VioInputPacket) -> VioProcessingResult:
|
||||
"""Process one synchronized frame/telemetry pair."""
|
||||
|
||||
def health(self) -> VioHealthReport:
|
||||
"""Return current readiness and degradation state."""
|
||||
|
||||
|
||||
class VioBackend(Protocol):
|
||||
"""Backend-neutral native bridge boundary."""
|
||||
|
||||
def initialize(self) -> None:
|
||||
"""Initialize native backend resources."""
|
||||
|
||||
def estimate(self, frame: Any, telemetry_window: tuple[Any, ...]) -> VioBackendEstimate:
|
||||
"""Return one relative VIO estimate."""
|
||||
|
||||
|
||||
class DeterministicVioBackend:
|
||||
"""Small deterministic backend used until a native bridge is attached."""
|
||||
|
||||
def initialize(self) -> None:
|
||||
return None
|
||||
|
||||
def estimate(self, frame: Any, telemetry_window: tuple[Any, ...]) -> VioBackendEstimate:
|
||||
quality = float(getattr(frame, "quality", 1.0))
|
||||
tracking_quality = max(0.0, min(1.0, quality))
|
||||
return VioBackendEstimate(
|
||||
timestamp_ns=frame.timestamp_ns,
|
||||
relative_pose={
|
||||
"x_m": tracking_quality,
|
||||
"y_m": 0.0,
|
||||
"z_m": 0.0,
|
||||
"yaw_rad": 0.0,
|
||||
},
|
||||
velocity_mps=(tracking_quality, 0.0, 0.0),
|
||||
tracking_quality=tracking_quality,
|
||||
bias_estimate={"sample_count": float(len(telemetry_window))},
|
||||
covariance_hint=[
|
||||
[1.0 / max(tracking_quality, 0.1), 0.0, 0.0],
|
||||
[0.0, 1.0 / max(tracking_quality, 0.1), 0.0],
|
||||
[0.0, 0.0, 1.0 / max(tracking_quality, 0.1)],
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
class LocalVioAdapter:
|
||||
"""Backend-neutral adapter that exposes explicit health and mismatch behavior."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
backend: VioBackend | None = None,
|
||||
timestamp_tolerance_ns: int = 5_000_000,
|
||||
degraded_quality_threshold: float = 0.35,
|
||||
) -> None:
|
||||
self._backend = backend or DeterministicVioBackend()
|
||||
self._timestamp_tolerance_ns = timestamp_tolerance_ns
|
||||
self._degraded_quality_threshold = degraded_quality_threshold
|
||||
self._initialized = False
|
||||
self._health = VioHealthReport(
|
||||
initialized=False,
|
||||
state="not_initialized",
|
||||
tracking_quality=0.0,
|
||||
)
|
||||
|
||||
def initialize(self) -> None:
|
||||
self._backend.initialize()
|
||||
self._initialized = True
|
||||
self._health = VioHealthReport(
|
||||
initialized=True,
|
||||
state="ready",
|
||||
tracking_quality=1.0,
|
||||
)
|
||||
|
||||
def process(self, packet: VioInputPacket) -> VioProcessingResult:
|
||||
if not self._initialized:
|
||||
self.initialize()
|
||||
|
||||
telemetry_timestamps = [sample.timestamp_ns for sample in packet.telemetry_samples]
|
||||
time_window = select_time_window(
|
||||
packet.frame.timestamp_ns,
|
||||
telemetry_timestamps,
|
||||
self._timestamp_tolerance_ns,
|
||||
)
|
||||
if not time_window.ok:
|
||||
error = ErrorEnvelope(
|
||||
component="vio_adapter",
|
||||
category="validation",
|
||||
message="frame and telemetry timestamps are outside the VIO sync window",
|
||||
severity="warning",
|
||||
retryable=False,
|
||||
cause=time_window.violations[0].category,
|
||||
)
|
||||
self._health = VioHealthReport(
|
||||
initialized=True,
|
||||
state="degraded",
|
||||
tracking_quality=0.0,
|
||||
error=error,
|
||||
)
|
||||
return VioProcessingResult(health=self._health, error=error)
|
||||
|
||||
telemetry_window = tuple(
|
||||
sample
|
||||
for sample in packet.telemetry_samples
|
||||
if sample.timestamp_ns in set(time_window.sample_timestamps_ns)
|
||||
)
|
||||
estimate = self._backend.estimate(packet.frame, telemetry_window)
|
||||
state_packet = VioStatePacket(
|
||||
timestamp_ns=estimate.timestamp_ns,
|
||||
relative_pose=estimate.relative_pose,
|
||||
velocity_mps=estimate.velocity_mps,
|
||||
bias_estimate=estimate.bias_estimate,
|
||||
tracking_quality=estimate.tracking_quality,
|
||||
covariance_hint=estimate.covariance_hint,
|
||||
)
|
||||
health_state = (
|
||||
"degraded" if estimate.tracking_quality < self._degraded_quality_threshold else "ready"
|
||||
)
|
||||
self._health = VioHealthReport(
|
||||
initialized=True,
|
||||
state=health_state,
|
||||
tracking_quality=estimate.tracking_quality,
|
||||
)
|
||||
return VioProcessingResult(state_packet=state_packet, health=self._health)
|
||||
|
||||
def health(self) -> VioHealthReport:
|
||||
return self._health
|
||||
|
||||
@@ -1,5 +1,39 @@
|
||||
"""Public VIO type aliases."""
|
||||
"""Public VIO adapter models."""
|
||||
|
||||
from typing import Any
|
||||
from typing import Literal
|
||||
|
||||
VioStatePacketLike = Any
|
||||
from pydantic import BaseModel, ConfigDict, Field, NonNegativeInt
|
||||
|
||||
from shared.contracts import FramePacket, TelemetrySample, VioStatePacket
|
||||
from shared.errors import ErrorEnvelope
|
||||
|
||||
|
||||
class VioAdapterModel(BaseModel):
|
||||
model_config = ConfigDict(extra="forbid", frozen=True)
|
||||
|
||||
|
||||
class VioInputPacket(VioAdapterModel):
|
||||
frame: FramePacket
|
||||
telemetry_samples: tuple[TelemetrySample, ...] = Field(min_length=1)
|
||||
|
||||
|
||||
class VioHealthReport(VioAdapterModel):
|
||||
initialized: bool
|
||||
state: Literal["not_initialized", "ready", "degraded", "failed"]
|
||||
tracking_quality: float = Field(ge=0.0, le=1.0)
|
||||
error: ErrorEnvelope | None = None
|
||||
|
||||
|
||||
class VioProcessingResult(VioAdapterModel):
|
||||
state_packet: VioStatePacket | None = None
|
||||
health: VioHealthReport
|
||||
error: ErrorEnvelope | None = None
|
||||
|
||||
|
||||
class VioBackendEstimate(VioAdapterModel):
|
||||
timestamp_ns: NonNegativeInt
|
||||
relative_pose: dict[str, float]
|
||||
velocity_mps: tuple[float, float, float]
|
||||
tracking_quality: float = Field(ge=0.0, le=1.0)
|
||||
bias_estimate: dict[str, float] | None = None
|
||||
covariance_hint: list[list[float]] | None = None
|
||||
|
||||
Reference in New Issue
Block a user