mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 01:51:14 +00:00
[AZ-355] C4 PoseEstimator Protocol + factory + DTOs + composition
Land the foundational C4 surface AZ-358 (Marginals) and AZ-361 (Hybrid) build on top of: - PoseEstimator Protocol (@runtime_checkable): estimate(...) + current_covariance_mode(). - Error hierarchy: PoseEstimatorError, PnpFailureError, PoseEstimatorConfigError; CovarianceDegradedWarning as a Warning subclass (warnings.warn path, not raised). - ISam2GraphHandle Protocol stub (READ-ONLY view, get_pose_key only) decoupled from C5's concrete ISam2GraphHandleImpl. - C4PoseConfig (frozen dataclass) + register on c4_pose import. - runtime_root/pose_factory.build_pose_estimator with lazy-import fallback; INFO log c4.pose.strategy_loaded; shares ingest-thread binding with C5 per ADR-003. DTO restructuring (cross-cutting): retire the legacy raw-4x4 PoseEstimate(int frame_id, datetime timestamp, pose_se3, ...) and ship the contract shape PoseEstimate(UUID, LatLonAlt, Quat, np.ndarray, CovarianceMode, PoseSourceLabel, last_satellite_anchor_age_ms, emitted_at). C5 add_pose_anchor in both gtsam_isam2 + eskf_baseline migrated in lockstep via WGS84->ENU + Quat->R helpers; test fixtures updated. VIO output stays on the raw shape until AZ-331 (C1 protocol) lands. LatLonAlt upgraded to slots=True per AC-2. ThermalState stub added to _types/thermal.py so the Protocol typechecks pre-AZ-302. Tests: 25 new in tests/unit/c4_pose/test_az355_pose_protocol.py covering AC-1..AC-10 + factory wiring + config validation; full repo: 685 passed, 2 pre-existing CI-only skips. Jira transition deferred: MCP "Not connected"; leftover entry in _docs/_process_leftovers/2026-05-11_jira_transition_az355_deferred.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -25,13 +25,20 @@ from __future__ import annotations
|
||||
import logging
|
||||
from datetime import datetime, timezone
|
||||
from unittest import mock
|
||||
from uuid import UUID
|
||||
|
||||
import gtsam
|
||||
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.pose import PoseEstimate
|
||||
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
|
||||
@@ -79,15 +86,22 @@ def _make_pose(
|
||||
*,
|
||||
frame_id: int,
|
||||
t_seconds: float,
|
||||
covariance_mode: str = "marginals",
|
||||
pose: np.ndarray | None = None,
|
||||
covariance_mode: CovarianceMode | str = CovarianceMode.MARGINALS,
|
||||
) -> PoseEstimate:
|
||||
mode = (
|
||||
covariance_mode
|
||||
if isinstance(covariance_mode, CovarianceMode)
|
||||
else CovarianceMode(covariance_mode.lower())
|
||||
)
|
||||
return PoseEstimate(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
|
||||
pose_se3=pose if pose is not None else np.eye(4),
|
||||
frame_id=UUID(int=frame_id),
|
||||
position_wgs84=LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
covariance_6x6=np.eye(6) * 0.01,
|
||||
covariance_mode=covariance_mode,
|
||||
covariance_mode=mode,
|
||||
source_label=PoseSourceLabel.SATELLITE_ANCHORED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
emitted_at=int(t_seconds * 1_000_000_000),
|
||||
)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user