mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 10:51:13 +00:00
[AZ-331] C1 VioStrategy: Protocol + DTOs + factory + C5 migration
Freezes the c1_vio Public API per _docs/02_document/contracts/c1_vio/vio_strategy_protocol.md v1.0.0: - VioStrategy Protocol (4 methods: process_frame, reset_to_warm_start, health_snapshot, current_strategy_label) in components/c1_vio/interface.py. - DTOs (VioOutput, VioHealth, FeatureQuality, WarmStartPose) + VioState enum in _types/nav.py — L1 placement so C5 + C13 consume them without crossing the components.* boundary (AZ-270 AC-6). The new VioOutput shape (frame_id: str, relative_pose_T: gtsam.Pose3, pose_covariance_6x6, imu_bias, feature_quality, emitted_at_ns) replaces the AZ-263 scaffolding in _types/vio.py, which is now deleted. - VioError family (VioInitializingError / VioDegradedError / VioFatalError) in components/c1_vio/errors.py. Documented rationale: the degraded-operation path returns a VioOutput with inflated covariance + VioHealth.state=DEGRADED rather than raising VioDegradedError — the error type exists only for the rare degraded->fatal transition. - C1VioConfig per-component config block (strategy enum, lost_frame_threshold default 9, warm_start_max_frames default 5) with constructor-time validation rejecting unknown strategy labels. - StrategyNotAvailableError added to runtime_root/errors.py; composition-time error distinct from the VioError family. - Composition-root factory build_vio_strategy in runtime_root/vio_factory.py with three BUILD_* gates (BUILD_OKVIS2, BUILD_VINS_MONO, BUILD_KLT_RANSAC). Concrete strategy modules are imported lazily via __import__ AFTER the flag check — Tier-0 workstation builds with the flag OFF MUST NOT load the strategy module (Risk-2 / I-5; verifiable via sys.modules). - 36 conformance tests cover all 9 ACs + NFR-perf-factory (p99 build under 200 ms x 1000 calls) + NFR-reliability-error-family. AC-8 introspects the contract file's Shape table and asserts method parity against the runtime Protocol; AC-9 asserts the frame_id annotation is 'str' (PEP-563 stringified). C5 migration (consumers of the new VioOutput shape): - gtsam_isam2_estimator.py + eskf_baseline.py: replaced vio.timestamp -> vio.emitted_at_ns (drops _datetime_to_ns on the VIO path), vio.pose_se3 -> vio.relative_pose_T (gtsam.Pose3 direct; drops _pose_se3_to_gtsam / _pose_se3_to_array), vio.covariance_6x6 -> vio.pose_covariance_6x6 (rename). - key_for_frame signature widened to UUID | int | str to accept the new str frame_id. - 4 C5 test files migrated to the new VioOutput shape with helper fixtures producing ImuBias + FeatureQuality + str frame_id. - c5_state/interface.py TYPE_CHECKING import path updated. Bootstrap healthcheck + test_types_importable updated to drop the deleted _types/vio module and pick up _types/inference (AZ-297) in the same sweep. Full unit-test sweep: 884 passed, 2 pre-existing environment skips (cmake, actionlint). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -32,14 +32,19 @@ import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.nav import ImuSample, ImuWindow
|
||||
from gps_denied_onboard._types.nav import (
|
||||
FeatureQuality,
|
||||
ImuBias,
|
||||
ImuSample,
|
||||
ImuWindow,
|
||||
VioOutput,
|
||||
)
|
||||
from gps_denied_onboard._types.pose import (
|
||||
CovarianceMode,
|
||||
PoseEstimate,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
from gps_denied_onboard.components.c5_state.config import C5StateConfig
|
||||
from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError
|
||||
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
|
||||
@@ -73,12 +78,21 @@ def _build_estimator(*, with_stub_handle: bool = True) -> GtsamIsam2StateEstimat
|
||||
return estimator
|
||||
|
||||
|
||||
_ZERO_BIAS = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
|
||||
_NEUTRAL_FQ = FeatureQuality(
|
||||
tracked=20, new=2, lost=1, mean_parallax=5.0, mre_px=1.0,
|
||||
)
|
||||
|
||||
|
||||
def _make_vio(*, frame_id: int, t_seconds: float, pose: np.ndarray | None = None) -> VioOutput:
|
||||
matrix = pose if pose is not None else np.eye(4)
|
||||
return VioOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
|
||||
pose_se3=pose if pose is not None else np.eye(4),
|
||||
covariance_6x6=np.eye(6) * 0.01,
|
||||
frame_id=str(frame_id),
|
||||
relative_pose_T=gtsam.Pose3(matrix),
|
||||
pose_covariance_6x6=np.eye(6) * 0.01,
|
||||
imu_bias=_ZERO_BIAS,
|
||||
feature_quality=_NEUTRAL_FQ,
|
||||
emitted_at_ns=int(t_seconds * 1_000_000_000),
|
||||
)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user