mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 16:11: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:
@@ -36,10 +36,15 @@ import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import GpsHealth, GpsStatus
|
||||
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, Quat
|
||||
from gps_denied_onboard._types.state import IsamState, PoseSourceLabel
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
from gps_denied_onboard.components.c5_state import (
|
||||
C5StateConfig,
|
||||
EstimatorFatalError,
|
||||
@@ -104,18 +109,26 @@ def _pose_translated(x: float, y: float, z: float) -> np.ndarray:
|
||||
return p
|
||||
|
||||
|
||||
_ESKF_ZERO_BIAS = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
|
||||
_ESKF_NEUTRAL_FQ = FeatureQuality(
|
||||
tracked=20, new=2, lost=1, mean_parallax=5.0, mre_px=1.0,
|
||||
)
|
||||
|
||||
|
||||
def _vio(
|
||||
frame_id: int,
|
||||
ts_ns: int,
|
||||
pose: np.ndarray,
|
||||
cov: np.ndarray | None = None,
|
||||
) -> VioOutput:
|
||||
# Default sigma ≈ 0.1 m / 0.1 rad — realistic VIO uncertainty.
|
||||
import gtsam
|
||||
return VioOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.fromtimestamp(ts_ns / 1_000_000_000, tz=timezone.utc),
|
||||
pose_se3=pose,
|
||||
covariance_6x6=cov if cov is not None else np.eye(6) * 0.01,
|
||||
frame_id=str(frame_id),
|
||||
relative_pose_T=gtsam.Pose3(pose),
|
||||
pose_covariance_6x6=cov if cov is not None else np.eye(6) * 0.01,
|
||||
imu_bias=_ESKF_ZERO_BIAS,
|
||||
feature_quality=_ESKF_NEUTRAL_FQ,
|
||||
emitted_at_ns=int(ts_ns),
|
||||
)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user