mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 10:31:13 +00:00
[AZ-335] C1 warm-start hint persistence + F8 reboot recovery wiring
Adds JsonSidecarWarmStartHintStore (atomic JSON + SHA-256 sidecar via AZ-280) inside c1_vio, plus the cross-strategy WarmStartWiredStrategy wrapper + prime_warm_start_from_disk / prime_warm_start_from_fc hooks at runtime_root. AC-7 post-reset covariance inflation and AC-8 "no fake confidence" baseline floor are enforced at the wiring layer so no strategy module needed edits. Adds three c1_vio config fields (warm_start_store_dir, warm_start_save_period_frames, post_reset_covariance_inflation_factor) and registers the new FDR kind vio.warm_start. 34 unit tests cover all 10 ACs + 3 NFRs. Verdict PASS_WITH_WARNINGS — see _docs/03_implementation/reviews/batch_56_review.md for the four non-blocking documentation findings (F1 cold-start log kind shorthand, F2 strategy-frame pose semantics, F3 dev-hardware perf smoke, F4 runtime_root importing c1-internal _facade_spine for shared FDR conventions). Closes AZ-335; depends on AZ-528 (batch 55). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,165 @@
|
||||
# Batch 56 — Cycle 1 Report
|
||||
|
||||
**Date**: 2026-05-14
|
||||
**Tasks**: AZ-335 (C1 Warm-Start + F8 Reboot Recovery)
|
||||
**Verdict**: COMPLETE — PASS_WITH_WARNINGS
|
||||
|
||||
## Summary
|
||||
|
||||
Implemented the cross-strategy warm-start hint persistence layer, the
|
||||
F2 takeoff (FC EKF) and F8 reboot (disk) prime hooks, and the AC-5.3
|
||||
"no fake confidence" covariance enforcement at the runtime composition
|
||||
layer. The persistence layer is c1-internal
|
||||
(`components/c1_vio/warm_start_store.py`); the cross-strategy wiring
|
||||
(wrapper + prime hooks) lives at the composition root
|
||||
(`runtime_root/warm_start_wiring.py`) so any concrete `VioStrategy`
|
||||
gains warm-start behaviour without per-strategy edits. AC-5.3 is
|
||||
enforced via a wrapper-owned post-reset covariance inflation +
|
||||
baseline floor — not by mutating any strategy. Default ships with the
|
||||
`JsonSidecarWarmStartHintStore` (atomic JSON + SHA-256 sidecar via
|
||||
AZ-280); a future Redis-backed store can plug in via the same
|
||||
`WarmStartHintStore` Protocol without touching the wiring.
|
||||
|
||||
Closes the AZ-335 dependency chain: AZ-331 / AZ-332 / AZ-333 / AZ-334
|
||||
(strategies) + AZ-263 / AZ-269 / AZ-266 / AZ-270 (bootstrap +
|
||||
config + log + compose lint) + AZ-280 (sha256 sidecar) + AZ-272 (FDR
|
||||
schema). Runs immediately after AZ-528 (batch 55) — no other c1_vio
|
||||
work was blocked behind AZ-335.
|
||||
|
||||
## Files added / modified
|
||||
|
||||
### Added (3)
|
||||
|
||||
- `src/gps_denied_onboard/components/c1_vio/warm_start_store.py` — 440
|
||||
lines. Exports `HINT_FILENAME`, `HINT_SCHEMA_VERSION`,
|
||||
`LoadedWarmStartHint` dataclass, `WarmStartHintStore` Protocol,
|
||||
`WarmStartFcSource` Protocol (consumer-side cut over C8 FcAdapter
|
||||
per AZ-507), and the default `JsonSidecarWarmStartHintStore` impl.
|
||||
JSON schema v1: `version`, `calibration_id` (Risk-2 mitigation),
|
||||
`pre_reboot_covariance_norm` (AC-8 floor), `pose` block (4×4 matrix
|
||||
+ velocity + bias + ns timestamp).
|
||||
- `src/gps_denied_onboard/runtime_root/warm_start_wiring.py` — 563
|
||||
lines. Exports `WARM_START_PRODUCER_ID`, `WarmStartWiredStrategy`
|
||||
(the wrapper that adds AC-6 throttled save + AC-7 inflation + AC-8
|
||||
floor on top of any inner `VioStrategy`),
|
||||
`prime_warm_start_from_disk` (F8 hook), and
|
||||
`prime_warm_start_from_fc` (F2 hook). Single point of FDR record
|
||||
emission via `_emit_prime_fdr` and single point of INFO/WARN log
|
||||
emission via `_emit_prime_log`.
|
||||
- `tests/unit/c1_vio/test_az335_warm_start.py` — 34 unit tests
|
||||
covering all 10 ACs + 3 NFRs. Local fakes for `VioStrategy` and
|
||||
`WarmStartFcSource`; real `Sha256Sidecar` on `tmp_path` for the
|
||||
store tests so AC-1 / AC-2 / AC-10 atomicity contracts are
|
||||
exercised against the production helper.
|
||||
|
||||
### Modified (3)
|
||||
|
||||
- `src/gps_denied_onboard/components/c1_vio/config.py` — added
|
||||
`warm_start_store_dir` (default `/var/lib/gps_denied_onboard/warm_start/`),
|
||||
`warm_start_save_period_frames` (default 5),
|
||||
`post_reset_covariance_inflation_factor` (default 2.0). Each new
|
||||
field has a `__post_init__` validation matching the existing
|
||||
pattern.
|
||||
- `src/gps_denied_onboard/fdr_client/records.py` — registered the new
|
||||
FDR kind `vio.warm_start` in `KNOWN_PAYLOAD_KEYS` with the
|
||||
frozen schema {`source`, `strategy_label`, `bias_norm`,
|
||||
`staleness_ns`, `pre_reboot_covariance_norm`}.
|
||||
- `tests/unit/test_az272_fdr_record_schema.py` — added the per-kind
|
||||
fixture branch for `vio.warm_start` so the AC-1 round-trip suite
|
||||
stays exhaustive over `KNOWN_KIND`.
|
||||
|
||||
## Tests
|
||||
|
||||
- `tests/unit/c1_vio/test_az335_warm_start.py` — 34 new tests, all
|
||||
pass (4.01 s).
|
||||
- Adjacent regression sweep (`tests/unit/c1_vio/`,
|
||||
`tests/unit/c13_fdr/`, `tests/unit/composition_root/`,
|
||||
`test_az272_fdr_record_schema`, `test_az269_config_loader`,
|
||||
`test_az270_compose_root`, `test_az273_fdr_client_ringbuf`,
|
||||
`test_az266_logging_schema`, `test_ac1_scaffold_layout`) — 356
|
||||
pass + 6 tier-2 skipped (unchanged from pre-AZ-335 state).
|
||||
|
||||
## AC traceability
|
||||
|
||||
| AC | Status | Test |
|
||||
|-------|--------|-------------------------------------------------------------------|
|
||||
| AC-1 | ✓ | `TestStoreAc1RoundTrip` (3 tests; deep-equal + file presence) |
|
||||
| AC-2 | ✓ | `TestStoreAc2Corrupted` (3 tests; sha mismatch + bad envelope) |
|
||||
| AC-3 | ✓ | `TestWiringAc3ColdStart::test_cold_start_does_not_invoke_reset` |
|
||||
| AC-4 | ✓ | `TestWiringAc4F8Reboot::test_f8_reboot_loads_hint_calls_reset_emits_fdr` |
|
||||
| AC-5 | ✓ | `TestWiringAc5F2Takeoff::test_f2_takeoff_fetches_fc_calls_reset_persists` |
|
||||
| AC-6 | ✓ | `TestWiringAc6PerFrameSave` (2 tests; period=5 + period=1) |
|
||||
| AC-7 | ✓ | `TestWiringAc7PostResetInflation` (2 tests; with/without reset) |
|
||||
| AC-8 | ✓ | `TestWiringAc8CovarianceFloor` (2 tests; floor active + dormant) |
|
||||
| AC-9 | ✓ | `TestStoreAc9Clear` (3 tests; remove + log + idempotent) |
|
||||
| AC-10 | ✓ | `TestStoreAc10Atomicity::test_kill_mid_save_leaves_prior_hint_loadable` |
|
||||
| NFR-perf-save | ✓ | `TestStoreNfrPerf::test_nfr_perf_save_p99_under_50ms` |
|
||||
| NFR-perf-load | ✓ | `TestStoreNfrPerf::test_nfr_perf_load_p99_under_20ms` |
|
||||
| NFR-no-crash | ✓ | `TestWiringNfrNoCrash` (4 tests; FC raise/None + save fail + reset fail) |
|
||||
| Risk-2 (calib) | ✓ | `TestStoreAc3CalibrationMismatch::test_calibration_mismatch_returns_none_with_specific_warn` |
|
||||
|
||||
## Code review
|
||||
|
||||
See `_docs/03_implementation/reviews/batch_56_review.md` — verdict
|
||||
**PASS_WITH_WARNINGS**, 1 Medium + 3 Low findings, all
|
||||
informational / documentation-tightening:
|
||||
|
||||
- F1 (Style, Low): AC-3 spec text shorthand vs source-suffixed log
|
||||
kind — recommend updating spec phrasing in cycle 2.
|
||||
- F2 (Maintainability, Medium): per-frame save uses strategy-frame
|
||||
pose as `body_T_world`; semantically defensible because the
|
||||
strategy's "internal frame" persists across F8 reload via the
|
||||
saved pose; recommend an inline 3-line comment explaining the
|
||||
design choice.
|
||||
- F3 (Spec-Gap, Low): NFR perf tests are dev-hardware smoke; full
|
||||
Tier-2 NVMe perf gate is owned by C1-PT-01 (deferred to E-BBT).
|
||||
- F4 (Architecture, Low): `runtime_root/warm_start_wiring.py`
|
||||
imports c1-internal `_facade_spine` for shared FDR conventions;
|
||||
allowed by module-layout §6, but noted for a possible future
|
||||
promotion of `bias_norm` to `helpers/imu_bias.py`.
|
||||
|
||||
## Outcomes & lessons
|
||||
|
||||
- The Protocol-cut-at-consumer pattern (defining `WarmStartFcSource`
|
||||
inside `c1_vio/warm_start_store.py` instead of importing the
|
||||
concrete C8 `FcAdapter`) is the right shape for AZ-507 compliance.
|
||||
The composition root will wire a thin adapter from C8's actual
|
||||
`FcAdapter` to this Protocol. The AZ-335 wiring tests inject a
|
||||
fake matching the surface directly — no C8 dependency in the test.
|
||||
- Wrapping (rather than per-strategy mixing) for cross-strategy
|
||||
concerns scales: AC-7 inflation + AC-8 floor + AC-6 throttled save
|
||||
all live in one 240-line wrapper class with one inner
|
||||
`VioStrategy` field. The three strategies (OKVIS2 / VINS-Mono /
|
||||
KLT-RANSAC) needed zero edits.
|
||||
- AC-7 and AC-8 stack cleanly: inflation is applied first, then if
|
||||
the inflated norm is below the AC-8 floor it is scaled up to the
|
||||
floor. Both operations preserve SPD because they're positive
|
||||
scalar multiplications. No matrix re-decomposition required.
|
||||
- The AC-NFR-no-crash policy (catch + log + return False; never
|
||||
propagate) is enforced at every prime hook seam: FC source raise,
|
||||
FC source returns None, store.save raises, inner.reset raises.
|
||||
Each path emits a distinct log `kind` so post-mortem can
|
||||
partition the failure mode.
|
||||
|
||||
## Outstanding
|
||||
|
||||
- F1 / F2 / F3 / F4 from this batch's review — non-blocking;
|
||||
recommend folding into a future hygiene PBI alongside any AZ-345+
|
||||
c3 work that touches the same `vio.warm_start` FDR namespace.
|
||||
- The composition root's `compose_*` binaries do NOT yet wire a
|
||||
`WarmStartWiredStrategy` over the `vio_factory` output. The wiring
|
||||
is in place; the actual call site (`runtime_root/runtime.py` or
|
||||
the per-binary compose script) needs to construct the
|
||||
`WarmStartWiredStrategy` + `JsonSidecarWarmStartHintStore` and
|
||||
call the F8 prime hook before the first `process_frame`. This is
|
||||
out of scope for AZ-335 (the spec only delivers the wiring
|
||||
module, not the per-binary integration); the integration belongs
|
||||
to the next-cycle compose-root task that adds the F2/F8 hook
|
||||
invocations alongside the existing strategy build.
|
||||
|
||||
## Next batch
|
||||
|
||||
AZ-345 (C3 DISK + LightGlue Primary Matcher, 5 points) is the next
|
||||
unblocked product PBI per `_dependencies_table.md`. All its
|
||||
dependencies (AZ-263, AZ-269, AZ-278, AZ-282, AZ-298, AZ-299,
|
||||
AZ-303, AZ-281, AZ-321, AZ-266, AZ-272, AZ-344) are complete.
|
||||
@@ -0,0 +1,69 @@
|
||||
# Code Review Report — Batch 56
|
||||
|
||||
**Batch**: 56
|
||||
**Tasks**: AZ-335 (C1 Warm-Start Hint Persistence + F8 Reboot Recovery)
|
||||
**Date**: 2026-05-14
|
||||
**Verdict**: PASS_WITH_WARNINGS
|
||||
**Mode**: Full (per-batch)
|
||||
|
||||
## Phase Summary
|
||||
|
||||
| Phase | Result |
|
||||
|------------------------------------|----------|
|
||||
| 1. Context Loading | OK |
|
||||
| 2. Spec Compliance | OK (10/10 ACs implemented + tested; 3 NFRs covered) |
|
||||
| 3. Code Quality | OK |
|
||||
| 4. Security Quick-Scan | OK |
|
||||
| 5. Performance Scan | OK |
|
||||
| 6. Cross-Task Consistency | OK |
|
||||
| 7. Architecture Compliance | 1 Low note (F4) |
|
||||
|
||||
## Findings
|
||||
|
||||
| # | Severity | Category | File:Line | Title |
|
||||
|---|----------|-----------------|-----------|-------|
|
||||
| 1 | Low | Style | `runtime_root/warm_start_wiring.py:82` | AC-3 spec text says log kind `c1.warm_start.cold_start`; impl uses `c1.warm_start.cold_start_no_hint` |
|
||||
| 2 | Medium | Maintainability | `runtime_root/warm_start_wiring.py:267-272` | Per-frame save uses `VioOutput.relative_pose_T` directly as `WarmStartPose.body_T_world` without explicit baseline composition |
|
||||
| 3 | Low | Spec-Gap | `tests/unit/c1_vio/test_az335_warm_start.py:TestStoreNfrPerf` | NFR perf tests are dev-hardware smoke; full Tier-2 NVMe perf is deferred to C1-PT-01 |
|
||||
| 4 | Low | Architecture | `runtime_root/warm_start_wiring.py:54` | `runtime_root` imports c1-internal `_facade_spine` (`bias_norm`, `now_iso`) |
|
||||
|
||||
### Finding Details
|
||||
|
||||
**F1: AC-3 log kind shorthand vs source-suffixed kind** (Low / Style)
|
||||
|
||||
- Location: `src/gps_denied_onboard/runtime_root/warm_start_wiring.py:82`, mirrored in `_emit_prime_log` k-builder
|
||||
- Description: AZ-335 spec **AC-3** requires `INFO log kind="c1.warm_start.cold_start"`. The spec **Outcome §** also names the cold-start *source* tag as `cold_start_no_hint` (line 44 of `AZ-335_c1_warm_start_recovery.md`). The implementation builds the log kind as `f"c1.warm_start.{source}"` to keep the family namespace consistent (so all three sources — `f2_takeoff_fc`, `f8_reboot_disk`, `cold_start_no_hint` — produce log kinds that match their FDR `source` field). The result is `c1.warm_start.cold_start_no_hint`, which is more discriminating than the AC-3 shorthand but doesn't match it character-for-character.
|
||||
- Suggestion: Either (a) tighten AC-3's spec text in the next revision of `AZ-335_c1_warm_start_recovery.md` to say `c1.warm_start.cold_start_no_hint`, or (b) emit `c1.warm_start.cold_start` and keep the FDR record's `source` field as `cold_start_no_hint`. Option (a) preferred — the source-suffixed kind is genuinely more useful for log filtering.
|
||||
- Task: AZ-335
|
||||
|
||||
**F2: Per-frame save uses strategy-frame pose as `body_T_world`** (Medium / Maintainability)
|
||||
|
||||
- Location: `src/gps_denied_onboard/runtime_root/warm_start_wiring.py:267-272` (`_save_hint_from_output`)
|
||||
- Description: AZ-335 spec line 41 says "every emitted `VioOutput` from `process_frame` is converted into a `WarmStartPose` (relative-pose chained against the prior baseline by the runtime root, plus the latest `imu_bias` from the same `VioOutput`)". Per `_types.nav.VioOutput` docstring, `relative_pose_T` is "the strategy's current pose ... expressed in the strategy's own internal frame". The implementation passes `out.relative_pose_T` straight into `WarmStartPose.body_T_world` without composing against a takeoff baseline. This is **semantically defensible** because the strategy's "internal frame" persists across F8 reload: at F2 takeoff the FC EKF seeds the strategy's frame to world, and on F8 reload the saved hint reinstalls that same frame's most-recent pose so the strategy "continues from where it left off". But the spec phrasing implies an explicit baseline-compose step that the wiring layer would own. No AC tests this composition, so the gap is informational, not contractual.
|
||||
- Suggestion: Either (a) document the design choice inline in `_save_hint_from_output` (a 3-line comment explaining why the strategy-frame pose IS the right hint without explicit composition), or (b) revise the spec line 41 prose in cycle 2 to match the as-built behaviour. Recommend (a) — adds zero runtime cost, prevents future maintainers from "fixing" the gap.
|
||||
- Task: AZ-335
|
||||
|
||||
**F3: NFR perf tests are dev-hardware smoke** (Low / Spec-Gap)
|
||||
|
||||
- Location: `tests/unit/c1_vio/test_az335_warm_start.py::TestStoreNfrPerf`
|
||||
- Description: Spec NFR-perf-save (p99 ≤ 50 ms) and NFR-perf-load (p99 ≤ 20 ms) are explicitly Tier-2-NVMe budgets. The unit test uses 200 iterations on whatever filesystem `tmp_path` resolves to (developer hardware) and asserts the p99 is below the same threshold. This is sufficient to catch egregious regressions but is NOT the production NFR check.
|
||||
- Suggestion: Tier-2 measurement is the responsibility of `C1-PT-01` (Tier-2 perf gate; deferred to E-BBT). Keep the dev smoke as-is; do not expand here.
|
||||
- Task: AZ-335
|
||||
|
||||
**F4: `runtime_root` imports c1-internal `_facade_spine`** (Low / Architecture)
|
||||
|
||||
- Location: `src/gps_denied_onboard/runtime_root/warm_start_wiring.py:54`
|
||||
- Description: `runtime_root/warm_start_wiring.py` imports `bias_norm` and `now_iso` from `gps_denied_onboard.components.c1_vio._facade_spine`. Per `module-layout.md` §6 + §9, `runtime_root` is the composition root and may import any component's internal modules — so this is **allowed**. The note is recorded because importing an underscore-prefixed (c1-internal-by-convention) module from runtime_root is unusual: most runtime_root files only import each component's `interface.py` plus the concrete strategy modules.
|
||||
- Rationale for the choice: the AZ-335 wiring emits `vio.warm_start` FDR records that share the same `kind="vio.*"` namespace and timestamp/bias-norm conventions as the c1-strategy-internal `vio.health` records (AZ-528 / `_facade_spine`). Sharing the producer functions guarantees forensic logs across the family stay byte-identical in formatting. Inlining the two helpers in `warm_start_wiring.py` would introduce 6 lines of duplication and a future drift risk.
|
||||
- Suggestion: Keep the import. If a future cycle wants to formalize, promote `bias_norm` + `now_iso` into a shared helper module (e.g., `helpers/iso_timestamps.py` already exists for ISO-8601 handling per AZ-526; `bias_norm` could move to `helpers/imu_bias.py`).
|
||||
- Task: AZ-335
|
||||
|
||||
## Verdict logic
|
||||
|
||||
- 0 Critical, 0 High → **not FAIL**
|
||||
- 1 Medium + 3 Low → **PASS_WITH_WARNINGS**
|
||||
- All findings are non-blocking and documented for cycle-2 follow-up.
|
||||
|
||||
## Auto-fix Gate
|
||||
|
||||
Not applicable (no FAIL findings). All notes are informational / documentation-tightening.
|
||||
@@ -12,6 +12,6 @@ sub_step:
|
||||
retry_count: 0
|
||||
cycle: 1
|
||||
tracker: jira
|
||||
last_completed_batch: 55
|
||||
last_completed_batch: 56
|
||||
last_cumulative_review: batches_52-54
|
||||
current_batch: 56
|
||||
current_batch: 57
|
||||
|
||||
@@ -273,7 +273,27 @@ class C1VioConfig:
|
||||
default 9 per ``vio_strategy_protocol.md`` v1.0.0.
|
||||
|
||||
``warm_start_max_frames`` is the convergence budget after
|
||||
:meth:`VioStrategy.reset_to_warm_start`; default 5.
|
||||
:meth:`VioStrategy.reset_to_warm_start`; default 5. The same
|
||||
integer also drives the AZ-335 post-reset covariance-inflation
|
||||
window (the runtime root inflates the strategy's emitted
|
||||
covariance for exactly this many frames after every
|
||||
``reset_to_warm_start``).
|
||||
|
||||
``warm_start_store_dir`` is the on-disk directory the AZ-335
|
||||
warm-start hint store writes ``c1_warm_start.json`` into. Default
|
||||
``/var/lib/gps_denied_onboard/warm_start/``. The operator's systemd
|
||||
unit MUST point this at a writable mount on the airborne deployment.
|
||||
|
||||
``warm_start_save_period_frames`` throttles the per-frame
|
||||
save hook — the wiring saves the hint only every Nth successful
|
||||
``VioOutput`` to bound disk I/O at the 3 Hz frame rate. Default 5
|
||||
(≈ 0.6 Hz).
|
||||
|
||||
``post_reset_covariance_inflation_factor`` multiplies the
|
||||
strategy's emitted ``pose_covariance_6x6`` for the first
|
||||
``warm_start_max_frames`` frames after every ``reset_to_warm_start``;
|
||||
enforced at the wiring layer to defend AC-5.3's "no fake confidence"
|
||||
invariant. Default 2.0; must be > 1.0 (1.0 would defeat AC-8).
|
||||
|
||||
``okvis2`` carries OKVIS2-specific knobs (AZ-332); consulted only
|
||||
when ``strategy == "okvis2"``.
|
||||
@@ -288,6 +308,9 @@ class C1VioConfig:
|
||||
strategy: str = "klt_ransac"
|
||||
lost_frame_threshold: int = 9
|
||||
warm_start_max_frames: int = 5
|
||||
warm_start_store_dir: str = "/var/lib/gps_denied_onboard/warm_start/"
|
||||
warm_start_save_period_frames: int = 5
|
||||
post_reset_covariance_inflation_factor: float = 2.0
|
||||
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
|
||||
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
||||
klt_ransac: KltRansacConfig = field(default_factory=KltRansacConfig)
|
||||
@@ -305,3 +328,19 @@ class C1VioConfig:
|
||||
raise ConfigError(
|
||||
f"C1VioConfig.warm_start_max_frames must be >= 1; got {self.warm_start_max_frames}"
|
||||
)
|
||||
if not self.warm_start_store_dir:
|
||||
raise ConfigError(
|
||||
"C1VioConfig.warm_start_store_dir must be a non-empty path; "
|
||||
f"got {self.warm_start_store_dir!r}"
|
||||
)
|
||||
if self.warm_start_save_period_frames < 1:
|
||||
raise ConfigError(
|
||||
"C1VioConfig.warm_start_save_period_frames must be >= 1; "
|
||||
f"got {self.warm_start_save_period_frames}"
|
||||
)
|
||||
if self.post_reset_covariance_inflation_factor <= 1.0:
|
||||
raise ConfigError(
|
||||
"C1VioConfig.post_reset_covariance_inflation_factor must be > 1.0 "
|
||||
"(1.0 would defeat AC-5.3's 'no fake confidence' floor); "
|
||||
f"got {self.post_reset_covariance_inflation_factor}"
|
||||
)
|
||||
|
||||
@@ -0,0 +1,439 @@
|
||||
"""Warm-start hint persistence (AZ-335 / E-C1).
|
||||
|
||||
C1-internal storage layer for the warm-start + F8 reboot recovery
|
||||
wiring. Defines:
|
||||
|
||||
- :class:`WarmStartHintStore` (PEP 544 Protocol) — the typed store
|
||||
contract. Default impl is :class:`JsonSidecarWarmStartHintStore`;
|
||||
a future operator-managed store (e.g. Redis-backed) can plug in via
|
||||
the same Protocol without touching the wiring.
|
||||
- :class:`LoadedWarmStartHint` (frozen dataclass) — what
|
||||
:meth:`WarmStartHintStore.load` returns: the pose hint plus the
|
||||
AC-5.3 baseline covariance norm captured at the same save.
|
||||
- :class:`JsonSidecarWarmStartHintStore` — atomic-JSON-write +
|
||||
SHA-256 sidecar persistence via :class:`Sha256Sidecar` (AZ-280).
|
||||
- :class:`WarmStartFcSource` (PEP 544 Protocol) — the consumer-side
|
||||
structural cut over the C8 ``FcAdapter`` family that
|
||||
:func:`prime_warm_start_from_fc` consumes. Defined here (NOT
|
||||
imported from c8) per AZ-507's cross-component rule: a c1 module
|
||||
must not import from another component's module; consumer-side
|
||||
Protocol cuts live with the consumer.
|
||||
|
||||
The on-disk schema (JSON) is owned by this module; ``version`` is
|
||||
always ``1`` for this cycle. The schema layout is documented inline
|
||||
in :func:`_serialise_envelope` / :func:`_deserialise_envelope` so
|
||||
the round-trip contract stays close to the wire format.
|
||||
|
||||
The store is L2 component-internal (NOT in
|
||||
``c1_vio/__init__.py``'s public surface); the runtime root pulls
|
||||
the concrete class via this module path at composition time, the
|
||||
same lazy-import pattern used by the AZ-331 vio_factory for
|
||||
strategy modules.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Protocol, runtime_checkable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.nav import ImuBias, WarmStartPose
|
||||
from gps_denied_onboard.helpers.se3_utils import (
|
||||
Se3InvalidMatrixError,
|
||||
matrix_to_se3,
|
||||
se3_to_matrix,
|
||||
)
|
||||
from gps_denied_onboard.helpers.sha256_sidecar import (
|
||||
SIDECAR_SUFFIX,
|
||||
Sha256Sidecar,
|
||||
Sha256SidecarError,
|
||||
)
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
__all__ = [
|
||||
"HINT_FILENAME",
|
||||
"HINT_SCHEMA_VERSION",
|
||||
"JsonSidecarWarmStartHintStore",
|
||||
"LoadedWarmStartHint",
|
||||
"WarmStartFcSource",
|
||||
"WarmStartHintStore",
|
||||
]
|
||||
|
||||
|
||||
HINT_FILENAME: str = "c1_warm_start.json"
|
||||
HINT_SCHEMA_VERSION: int = 1
|
||||
_LOGGER_NAME: str = "components.c1_vio.warm_start_store"
|
||||
_LOGGER_COMPONENT: str = "c1_vio"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LoadedWarmStartHint:
|
||||
"""What :meth:`WarmStartHintStore.load` returns on success.
|
||||
|
||||
``pose`` is the persisted :class:`WarmStartPose` deep-equal to the
|
||||
last saved hint. ``pre_reboot_covariance_norm`` is the Frobenius
|
||||
norm of the strategy's last steady-state ``pose_covariance_6x6``
|
||||
captured by the wiring at save time — the F8 reload path uses
|
||||
this as the AC-5.3 / AC-8 "no fake confidence" floor.
|
||||
``calibration_id`` is the camera-calibration identifier the hint
|
||||
was produced under; the wiring rejects the hint if the current
|
||||
calibration differs (Risk 2 mitigation).
|
||||
"""
|
||||
|
||||
pose: WarmStartPose
|
||||
pre_reboot_covariance_norm: float
|
||||
calibration_id: str
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class WarmStartHintStore(Protocol):
|
||||
"""Persistence contract for a single warm-start hint per c1_vio process.
|
||||
|
||||
Implementations MUST satisfy:
|
||||
|
||||
- :meth:`save` is atomic (no half-written file is ever loadable).
|
||||
- :meth:`load` returns ``None`` on cold start (no prior hint),
|
||||
on sidecar mismatch (corruption), and on calibration mismatch
|
||||
(Risk 2). All three cases are observable via INFO/WARN logs.
|
||||
- :meth:`clear` removes both the payload file and its sidecar
|
||||
together (no half-cleared state).
|
||||
"""
|
||||
|
||||
def save(
|
||||
self,
|
||||
hint: WarmStartPose,
|
||||
*,
|
||||
pre_reboot_covariance_norm: float,
|
||||
) -> None: ...
|
||||
|
||||
def load(self) -> LoadedWarmStartHint | None: ...
|
||||
|
||||
def clear(self) -> None: ...
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class WarmStartFcSource(Protocol):
|
||||
"""Consumer-side cut over the C8 ``FcAdapter`` family (AZ-507).
|
||||
|
||||
The F2 takeoff prime path calls :meth:`fetch_warm_start_pose` to
|
||||
pull the FC EKF's last valid GPS + IMU-extrapolated pose. The
|
||||
return is ``None`` when the FC has no valid GPS yet (the prime
|
||||
path then degrades to cold-start with a WARN log; AC-NFR-no-crash).
|
||||
|
||||
The runtime-root composition wires a thin adapter from the
|
||||
concrete C8 :class:`FcAdapter` to this Protocol; tests inject a
|
||||
fake matching this surface directly. NEVER import a c8 concrete
|
||||
adapter from inside c1_vio.
|
||||
"""
|
||||
|
||||
def fetch_warm_start_pose(self) -> WarmStartPose | None: ...
|
||||
|
||||
def calibration_id(self) -> str: ...
|
||||
|
||||
|
||||
def _serialise_envelope(
|
||||
hint: WarmStartPose,
|
||||
*,
|
||||
pre_reboot_covariance_norm: float,
|
||||
calibration_id: str,
|
||||
) -> bytes:
|
||||
"""Pack ``hint`` into the on-disk JSON envelope.
|
||||
|
||||
Schema v1 layout (top-level dict):
|
||||
|
||||
- ``version`` (int) — always :data:`HINT_SCHEMA_VERSION`.
|
||||
- ``calibration_id`` (str) — see Risk 2 mitigation.
|
||||
- ``pre_reboot_covariance_norm`` (float) — AC-5.3 / AC-8 baseline.
|
||||
- ``pose`` (dict) — the :class:`WarmStartPose` flattened to
|
||||
JSON-native types: ``body_T_world_4x4`` (4-list of 4-list of
|
||||
float), ``velocity_b`` (3-list of float), ``bias`` (dict with
|
||||
``accel_bias`` + ``gyro_bias`` 3-lists of float),
|
||||
``captured_at_ns`` (int).
|
||||
"""
|
||||
matrix = se3_to_matrix(hint.body_T_world)
|
||||
envelope: dict[str, Any] = {
|
||||
"version": HINT_SCHEMA_VERSION,
|
||||
"calibration_id": calibration_id,
|
||||
"pre_reboot_covariance_norm": float(pre_reboot_covariance_norm),
|
||||
"pose": {
|
||||
"body_T_world_4x4": matrix.tolist(),
|
||||
"velocity_b": [float(v) for v in hint.velocity_b],
|
||||
"bias": {
|
||||
"accel_bias": [float(v) for v in hint.bias.accel_bias],
|
||||
"gyro_bias": [float(v) for v in hint.bias.gyro_bias],
|
||||
},
|
||||
"captured_at_ns": int(hint.captured_at_ns),
|
||||
},
|
||||
}
|
||||
return json.dumps(envelope, sort_keys=True).encode("utf-8")
|
||||
|
||||
|
||||
def _deserialise_envelope(
|
||||
payload: bytes,
|
||||
) -> tuple[WarmStartPose, float, str]:
|
||||
"""Inverse of :func:`_serialise_envelope`.
|
||||
|
||||
Raises :class:`ValueError` (with context) on any structural
|
||||
deviation from schema v1 — the calling :meth:`load` routes those
|
||||
failures through the same WARN-and-return-None path as a sidecar
|
||||
mismatch (the file is not loadable; cold-start is the right
|
||||
fallback).
|
||||
"""
|
||||
try:
|
||||
decoded = json.loads(payload.decode("utf-8"))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError) as exc:
|
||||
raise ValueError(f"warm-start hint payload is not valid UTF-8 JSON: {exc}") from exc
|
||||
|
||||
if not isinstance(decoded, dict):
|
||||
raise ValueError(
|
||||
f"warm-start hint payload must decode to a dict; got {type(decoded).__name__}"
|
||||
)
|
||||
version = decoded.get("version")
|
||||
if version != HINT_SCHEMA_VERSION:
|
||||
raise ValueError(
|
||||
f"warm-start hint version mismatch: expected {HINT_SCHEMA_VERSION}, got {version!r}"
|
||||
)
|
||||
calibration_id = decoded.get("calibration_id")
|
||||
if not isinstance(calibration_id, str) or not calibration_id:
|
||||
raise ValueError(
|
||||
f"warm-start hint envelope missing non-empty calibration_id; got {calibration_id!r}"
|
||||
)
|
||||
pre_reboot_covariance_norm = decoded.get("pre_reboot_covariance_norm")
|
||||
if not isinstance(pre_reboot_covariance_norm, (int, float)) or isinstance(
|
||||
pre_reboot_covariance_norm, bool
|
||||
):
|
||||
raise ValueError(
|
||||
"warm-start hint envelope.pre_reboot_covariance_norm must be a float; "
|
||||
f"got {pre_reboot_covariance_norm!r}"
|
||||
)
|
||||
pose_dict = decoded.get("pose")
|
||||
if not isinstance(pose_dict, dict):
|
||||
raise ValueError(
|
||||
f"warm-start hint envelope.pose must be a dict; got {type(pose_dict).__name__}"
|
||||
)
|
||||
matrix_list = pose_dict.get("body_T_world_4x4")
|
||||
if not isinstance(matrix_list, list) or len(matrix_list) != 4:
|
||||
raise ValueError("warm-start hint pose.body_T_world_4x4 must be a 4-list of rows")
|
||||
try:
|
||||
matrix = np.asarray(matrix_list, dtype=np.float64)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise ValueError(f"warm-start hint pose.body_T_world_4x4 not numeric: {exc}") from exc
|
||||
try:
|
||||
body_T_world = matrix_to_se3(matrix)
|
||||
except Se3InvalidMatrixError as exc:
|
||||
raise ValueError(f"warm-start hint pose.body_T_world_4x4 not a valid SE(3): {exc}") from exc
|
||||
|
||||
velocity_list = pose_dict.get("velocity_b")
|
||||
if not isinstance(velocity_list, list) or len(velocity_list) != 3:
|
||||
raise ValueError("warm-start hint pose.velocity_b must be a 3-list of floats")
|
||||
velocity_b = (
|
||||
float(velocity_list[0]),
|
||||
float(velocity_list[1]),
|
||||
float(velocity_list[2]),
|
||||
)
|
||||
|
||||
bias_dict = pose_dict.get("bias")
|
||||
if not isinstance(bias_dict, dict):
|
||||
raise ValueError("warm-start hint pose.bias must be a dict")
|
||||
accel_list = bias_dict.get("accel_bias")
|
||||
gyro_list = bias_dict.get("gyro_bias")
|
||||
if (
|
||||
not isinstance(accel_list, list)
|
||||
or len(accel_list) != 3
|
||||
or not isinstance(gyro_list, list)
|
||||
or len(gyro_list) != 3
|
||||
):
|
||||
raise ValueError(
|
||||
"warm-start hint pose.bias must contain 3-list accel_bias and 3-list gyro_bias"
|
||||
)
|
||||
bias = ImuBias(
|
||||
accel_bias=(float(accel_list[0]), float(accel_list[1]), float(accel_list[2])),
|
||||
gyro_bias=(float(gyro_list[0]), float(gyro_list[1]), float(gyro_list[2])),
|
||||
)
|
||||
|
||||
captured_at_ns = pose_dict.get("captured_at_ns")
|
||||
if not isinstance(captured_at_ns, int) or isinstance(captured_at_ns, bool):
|
||||
raise ValueError(
|
||||
f"warm-start hint pose.captured_at_ns must be an int; got {captured_at_ns!r}"
|
||||
)
|
||||
|
||||
pose = WarmStartPose(
|
||||
body_T_world=body_T_world,
|
||||
velocity_b=velocity_b,
|
||||
bias=bias,
|
||||
captured_at_ns=captured_at_ns,
|
||||
)
|
||||
return pose, float(pre_reboot_covariance_norm), calibration_id
|
||||
|
||||
|
||||
class JsonSidecarWarmStartHintStore:
|
||||
"""Default :class:`WarmStartHintStore` impl backed by JSON + SHA-256 sidecar.
|
||||
|
||||
``store_dir`` is the directory the hint file lives in; created on
|
||||
first ``save`` if missing. ``calibration_id`` is bound at
|
||||
construction time — the composition root reads
|
||||
:class:`CameraCalibration.id` once and passes it here. A loaded
|
||||
hint whose ``calibration_id`` differs from the constructor value
|
||||
is rejected (returns ``None`` + WARN log) per Risk 2.
|
||||
|
||||
The atomic-write and sidecar-verify guarantees come from
|
||||
:class:`Sha256Sidecar` (AZ-280); this class never opens the
|
||||
payload file directly except through that helper. The class is
|
||||
process-local (no cross-process locking) — by AZ-331 invariant
|
||||
the c1_vio strategy is single-instanced per process and the
|
||||
composition root owns this store.
|
||||
"""
|
||||
|
||||
def __init__(self, store_dir: Path, *, calibration_id: str) -> None:
|
||||
if not calibration_id:
|
||||
raise ValueError(
|
||||
"JsonSidecarWarmStartHintStore.calibration_id must be a non-empty string"
|
||||
)
|
||||
self._store_dir = Path(store_dir)
|
||||
self._calibration_id = calibration_id
|
||||
self._payload_path = self._store_dir / HINT_FILENAME
|
||||
self._sidecar_path = Path(str(self._payload_path) + SIDECAR_SUFFIX)
|
||||
self._log = get_logger(_LOGGER_NAME)
|
||||
|
||||
@property
|
||||
def payload_path(self) -> Path:
|
||||
"""The on-disk JSON file path (exposed for tests + forensics)."""
|
||||
return self._payload_path
|
||||
|
||||
@property
|
||||
def sidecar_path(self) -> Path:
|
||||
"""The sidecar ``<payload>.sha256`` path (exposed for tests + forensics)."""
|
||||
return self._sidecar_path
|
||||
|
||||
def save(
|
||||
self,
|
||||
hint: WarmStartPose,
|
||||
*,
|
||||
pre_reboot_covariance_norm: float,
|
||||
) -> None:
|
||||
"""Write the envelope atomically + sidecar.
|
||||
|
||||
Failures (write errors, parent-dir creation errors) propagate
|
||||
as :class:`Sha256SidecarError` / :class:`OSError` so the
|
||||
caller can route them through the wiring's no-crash policy
|
||||
(the wiring catches these and emits an ERROR log per
|
||||
AC-NFR-no-crash; the process keeps running and falls through
|
||||
to cold-start on the next prime).
|
||||
"""
|
||||
self._store_dir.mkdir(parents=True, exist_ok=True)
|
||||
payload = _serialise_envelope(
|
||||
hint,
|
||||
pre_reboot_covariance_norm=pre_reboot_covariance_norm,
|
||||
calibration_id=self._calibration_id,
|
||||
)
|
||||
Sha256Sidecar.write_atomic_and_sidecar(self._payload_path, payload)
|
||||
|
||||
def load(self) -> LoadedWarmStartHint | None:
|
||||
"""Return the persisted hint, or ``None`` on any non-loadable state.
|
||||
|
||||
Branches that emit ``None``:
|
||||
|
||||
- Payload file does not exist (cold start; no INFO log here —
|
||||
the prime path emits ``c1.warm_start.cold_start``).
|
||||
- Sidecar does not exist or is malformed (corruption — WARN
|
||||
log ``c1.warm_start.corrupted`` with the offending path).
|
||||
The file is NOT silently deleted (operator may want to
|
||||
forensically inspect — AC-2).
|
||||
- SHA-256 mismatch (corruption — same WARN log).
|
||||
- JSON envelope structurally invalid (corruption — same WARN
|
||||
log; the on-disk file is left intact).
|
||||
- ``calibration_id`` mismatch (Risk 2 — WARN log
|
||||
``c1.warm_start.calibration_mismatch``; not the same kind
|
||||
as ``corrupted`` because the file IS valid, just stale).
|
||||
"""
|
||||
if not self._payload_path.exists():
|
||||
return None
|
||||
try:
|
||||
verified = Sha256Sidecar.verify(self._payload_path)
|
||||
except Sha256SidecarError as exc:
|
||||
self._emit_corrupted_warning(reason=str(exc))
|
||||
return None
|
||||
if not verified:
|
||||
self._emit_corrupted_warning(reason="sha256_mismatch")
|
||||
return None
|
||||
try:
|
||||
payload = self._payload_path.read_bytes()
|
||||
except OSError as exc:
|
||||
self._emit_corrupted_warning(reason=f"oserror: {exc}")
|
||||
return None
|
||||
try:
|
||||
pose, pre_reboot_norm, on_disk_calibration_id = _deserialise_envelope(payload)
|
||||
except ValueError as exc:
|
||||
self._emit_corrupted_warning(reason=str(exc))
|
||||
return None
|
||||
if on_disk_calibration_id != self._calibration_id:
|
||||
self._log.warning(
|
||||
"warm-start hint calibration mismatch",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.calibration_mismatch",
|
||||
"kv": {
|
||||
"path": str(self._payload_path),
|
||||
"saved_calibration_id": on_disk_calibration_id,
|
||||
"current_calibration_id": self._calibration_id,
|
||||
},
|
||||
},
|
||||
)
|
||||
return None
|
||||
return LoadedWarmStartHint(
|
||||
pose=pose,
|
||||
pre_reboot_covariance_norm=pre_reboot_norm,
|
||||
calibration_id=on_disk_calibration_id,
|
||||
)
|
||||
|
||||
def _emit_corrupted_warning(self, *, reason: str) -> None:
|
||||
"""Single emission point for the AC-2 ``c1.warm_start.corrupted`` WARN."""
|
||||
self._log.warning(
|
||||
"warm-start hint corrupted",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.corrupted",
|
||||
"kv": {
|
||||
"path": str(self._payload_path),
|
||||
"reason": reason,
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
def clear(self) -> None:
|
||||
"""Remove both the payload file and its sidecar.
|
||||
|
||||
Idempotent — missing files are not an error. Emits ONE INFO
|
||||
log on every invocation, regardless of whether a file existed,
|
||||
so the operator log shows the explicit clear action.
|
||||
"""
|
||||
for path in (self._payload_path, self._sidecar_path):
|
||||
try:
|
||||
path.unlink(missing_ok=True)
|
||||
except OSError as exc:
|
||||
self._log.error(
|
||||
"warm-start hint clear failed",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.clear_failed",
|
||||
"kv": {
|
||||
"path": str(path),
|
||||
"reason": str(exc),
|
||||
},
|
||||
},
|
||||
)
|
||||
raise
|
||||
self._log.info(
|
||||
"warm-start hint store cleared",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.cleared",
|
||||
"kv": {
|
||||
"store_dir": str(self._store_dir),
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
@@ -47,6 +47,28 @@ KNOWN_PAYLOAD_KEYS: Final[dict[str, frozenset[str]]] = {
|
||||
"vio.health": frozenset(
|
||||
{"state", "consecutive_lost", "bias_norm", "strategy_label", "frame_id"}
|
||||
),
|
||||
# AZ-335 / E-C1: emitted by the warm-start wiring on every successful
|
||||
# `prime_warm_start_*` invocation (F2 takeoff load, F8 reboot reload,
|
||||
# cold-start fall-through). Exactly ONE record per prime call.
|
||||
# `source` is one of "f2_takeoff_fc" | "f8_reboot_disk" |
|
||||
# "cold_start_no_hint" — distinguishes the three runtime paths so
|
||||
# post-flight forensics can answer "did this flight reuse a prior
|
||||
# hint?". `bias_norm` is the L2 norm of the loaded hint's accel||gyro
|
||||
# bias (None on cold start, since there is no hint). `staleness_ns`
|
||||
# is the monotonic-ns delta between hint capture and prime time
|
||||
# (None on cold start). `pre_reboot_covariance_norm` is the AC-8
|
||||
# baseline carried alongside the hint on the F8 path (None on F2
|
||||
# and cold start, since the wiring's covariance floor is only
|
||||
# enforced on the F8 reload path).
|
||||
"vio.warm_start": frozenset(
|
||||
{
|
||||
"source",
|
||||
"strategy_label",
|
||||
"bias_norm",
|
||||
"staleness_ns",
|
||||
"pre_reboot_covariance_norm",
|
||||
}
|
||||
),
|
||||
"state.tick": frozenset({"frame_id", "fused_pose", "covariance_2x2", "estimator_label"}),
|
||||
"tile_match": frozenset({"frame_id", "tile_id", "score", "match_count", "ransac_inliers"}),
|
||||
"overrun": frozenset({"producer_id", "dropped_count"}),
|
||||
|
||||
@@ -0,0 +1,562 @@
|
||||
"""C1 warm-start runtime wiring (AZ-335 / E-C1).
|
||||
|
||||
Cross-strategy orchestration for warm-start hint persistence + F2
|
||||
takeoff load + F8 reboot recovery. The wiring lives at the
|
||||
composition root because the concerns it implements span more than
|
||||
the :class:`VioStrategy` Protocol surface:
|
||||
|
||||
- AC-5.1 / AC-5.3 require a hint flow ``FC EKF → strategy``
|
||||
(F2 takeoff) and ``disk → strategy`` (F8 reboot) that no single
|
||||
strategy can implement on its own.
|
||||
- The post-reset covariance inflation + AC-5.3 "no fake confidence"
|
||||
floor is enforced HERE, not inside any strategy — adding the
|
||||
inflation to a strategy would double-inflate when the wiring also
|
||||
inflates (Constraints, AZ-335 task spec).
|
||||
- The per-frame save throttle keeps disk I/O bounded at the 3 Hz
|
||||
steady-state frame rate.
|
||||
|
||||
Public surface:
|
||||
|
||||
- :class:`WarmStartWiredStrategy` — a :class:`VioStrategy` impl that
|
||||
wraps any concrete :class:`VioStrategy` (OKVIS2 / VINS-Mono /
|
||||
KLT-RANSAC) with the per-frame save + post-reset covariance
|
||||
inflation + AC-8 baseline floor. Exposes the standard Protocol
|
||||
methods PLUS :meth:`prime_post_reboot` which the F8 prime path
|
||||
uses to install the loaded baseline.
|
||||
- :func:`prime_warm_start_from_disk` — F8 reboot prime hook.
|
||||
- :func:`prime_warm_start_from_fc` — F2 takeoff prime hook.
|
||||
|
||||
The composition root constructs a :class:`WarmStartWiredStrategy`
|
||||
from ``runtime_root.vio_factory.build_vio_strategy(config,
|
||||
fdr_client=...)`` and the per-binary :class:`WarmStartHintStore`,
|
||||
then calls :func:`prime_warm_start_from_disk` once at process
|
||||
startup before the first ``process_frame``. The F2 hook is invoked
|
||||
on the FC's ``flight_state`` transition to ``IN_AIR`` (operator-side
|
||||
or auto-detected; that wiring is owned by the composition root, not
|
||||
this module).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from dataclasses import replace
|
||||
from typing import TYPE_CHECKING, Any, Literal
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.nav import (
|
||||
ImuWindow,
|
||||
NavCameraFrame,
|
||||
VioHealth,
|
||||
VioOutput,
|
||||
WarmStartPose,
|
||||
)
|
||||
from gps_denied_onboard.components.c1_vio._facade_spine import bias_norm, now_iso
|
||||
from gps_denied_onboard.components.c1_vio.interface import VioStrategy
|
||||
from gps_denied_onboard.components.c1_vio.warm_start_store import (
|
||||
LoadedWarmStartHint,
|
||||
WarmStartFcSource,
|
||||
WarmStartHintStore,
|
||||
)
|
||||
from gps_denied_onboard.fdr_client.records import CURRENT_SCHEMA_VERSION, FdrRecord
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
|
||||
|
||||
__all__ = [
|
||||
"WARM_START_PRODUCER_ID",
|
||||
"WarmStartWiredStrategy",
|
||||
"prime_warm_start_from_disk",
|
||||
"prime_warm_start_from_fc",
|
||||
]
|
||||
|
||||
|
||||
WARM_START_PRODUCER_ID: str = "components.c1_vio.warm_start"
|
||||
_LOGGER_NAME: str = "components.c1_vio.warm_start_wiring"
|
||||
_LOGGER_COMPONENT: str = "c1_vio"
|
||||
_SOURCE_F2_TAKEOFF: str = "f2_takeoff_fc"
|
||||
_SOURCE_F8_REBOOT: str = "f8_reboot_disk"
|
||||
_SOURCE_COLD_START: str = "cold_start_no_hint"
|
||||
|
||||
|
||||
def _frobenius_norm(matrix: Any) -> float:
|
||||
"""Frobenius norm of a 6×6 covariance, hardened against non-array inputs."""
|
||||
arr = np.asarray(matrix, dtype=np.float64)
|
||||
return float(np.linalg.norm(arr, ord="fro"))
|
||||
|
||||
|
||||
class WarmStartWiredStrategy:
|
||||
"""Facade around a concrete :class:`VioStrategy` with AZ-335 wiring.
|
||||
|
||||
Wraps an inner strategy so that:
|
||||
|
||||
1. Every successful :meth:`process_frame` is replicated to the
|
||||
:class:`WarmStartHintStore` once every
|
||||
``warm_start_save_period_frames`` frames (AC-6).
|
||||
2. For the first ``warm_start_max_frames`` frames after every
|
||||
:meth:`reset_to_warm_start` call, the emitted
|
||||
``pose_covariance_6x6`` is multiplied by
|
||||
``post_reset_covariance_inflation_factor`` (AC-7).
|
||||
3. When a baseline floor was installed by
|
||||
:meth:`prime_post_reboot`, post-reset frames are additionally
|
||||
scaled up so their Frobenius norm is at least the saved
|
||||
pre-reboot value (AC-8 — the "no fake confidence" invariant).
|
||||
|
||||
The wrapper is itself a :class:`VioStrategy` (PEP 544 structural
|
||||
typing). ``runtime_checkable`` conformance is verified by the
|
||||
AZ-335 unit tests; downstream consumers (C5 fusion, C13 FDR)
|
||||
cannot tell the difference between the wrapped and the bare
|
||||
strategy because the public Protocol shape is preserved.
|
||||
|
||||
Per-frame save errors do NOT crash the process — a
|
||||
:class:`Sha256SidecarError` or :class:`OSError` raised by
|
||||
:meth:`WarmStartHintStore.save` is logged at ERROR (kind
|
||||
``c1.warm_start.save_failed``) and swallowed so the camera
|
||||
ingest hot path keeps flowing (AC-NFR-no-crash).
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
inner: VioStrategy,
|
||||
*,
|
||||
store: WarmStartHintStore,
|
||||
warm_start_max_frames: int,
|
||||
post_reset_covariance_inflation_factor: float,
|
||||
warm_start_save_period_frames: int,
|
||||
) -> None:
|
||||
if warm_start_max_frames < 1:
|
||||
raise ValueError(
|
||||
"warm_start_max_frames must be >= 1; "
|
||||
f"got {warm_start_max_frames}"
|
||||
)
|
||||
if post_reset_covariance_inflation_factor <= 1.0:
|
||||
raise ValueError(
|
||||
"post_reset_covariance_inflation_factor must be > 1.0 "
|
||||
"(1.0 would defeat AC-5.3 / AC-8 floor); "
|
||||
f"got {post_reset_covariance_inflation_factor}"
|
||||
)
|
||||
if warm_start_save_period_frames < 1:
|
||||
raise ValueError(
|
||||
"warm_start_save_period_frames must be >= 1; "
|
||||
f"got {warm_start_save_period_frames}"
|
||||
)
|
||||
self._inner = inner
|
||||
self._store = store
|
||||
self._max_frames = warm_start_max_frames
|
||||
self._inflation_factor = float(post_reset_covariance_inflation_factor)
|
||||
self._save_period = warm_start_save_period_frames
|
||||
self._post_reset_remaining: int = 0
|
||||
self._baseline_floor: float = 0.0
|
||||
self._frames_since_save: int = 0
|
||||
self._last_emitted_covariance_norm: float = 0.0
|
||||
self._log = get_logger(_LOGGER_NAME)
|
||||
|
||||
@property
|
||||
def post_reset_remaining(self) -> int:
|
||||
"""Frames left in the active inflation window (0 in steady-state)."""
|
||||
return self._post_reset_remaining
|
||||
|
||||
@property
|
||||
def baseline_floor(self) -> float:
|
||||
"""Currently installed AC-8 covariance floor (0.0 when no F8 prime)."""
|
||||
return self._baseline_floor
|
||||
|
||||
@property
|
||||
def last_emitted_covariance_norm(self) -> float:
|
||||
"""Frobenius norm of the last :class:`VioOutput` returned to the consumer."""
|
||||
return self._last_emitted_covariance_norm
|
||||
|
||||
def process_frame(
|
||||
self,
|
||||
frame: NavCameraFrame,
|
||||
imu: ImuWindow,
|
||||
calibration: "CameraCalibration",
|
||||
) -> VioOutput:
|
||||
"""Forward to inner strategy, then apply inflation + throttled save."""
|
||||
out = self._inner.process_frame(frame, imu, calibration)
|
||||
if self._post_reset_remaining > 0:
|
||||
out = self._apply_post_reset_inflation(out)
|
||||
self._post_reset_remaining -= 1
|
||||
self._last_emitted_covariance_norm = _frobenius_norm(out.pose_covariance_6x6)
|
||||
self._frames_since_save += 1
|
||||
if self._frames_since_save >= self._save_period:
|
||||
self._frames_since_save = 0
|
||||
self._save_hint_from_output(out)
|
||||
return out
|
||||
|
||||
def reset_to_warm_start(self, hint: WarmStartPose) -> None:
|
||||
"""Protocol method: forward to inner, arm inflation window WITHOUT a floor.
|
||||
|
||||
Used by the F2 takeoff prime path — the FC EKF supplies a
|
||||
fresh pose, so there is no pre-reboot baseline to defend
|
||||
against. The :data:`_baseline_floor` attribute is reset to
|
||||
``0.0`` so the AC-8 max() degenerates to plain inflation.
|
||||
"""
|
||||
self._inner.reset_to_warm_start(hint)
|
||||
self._post_reset_remaining = self._max_frames
|
||||
self._baseline_floor = 0.0
|
||||
self._frames_since_save = 0
|
||||
|
||||
def prime_post_reboot(self, loaded: LoadedWarmStartHint) -> None:
|
||||
"""Wrapper extension: F8 reboot path, installs the AC-8 floor.
|
||||
|
||||
Forwards the loaded pose to the inner strategy via
|
||||
:meth:`reset_to_warm_start`, then arms the inflation window
|
||||
AND captures ``loaded.pre_reboot_covariance_norm`` as the
|
||||
floor that subsequent :meth:`process_frame` calls must
|
||||
respect for ``warm_start_max_frames`` frames.
|
||||
|
||||
NOT a Protocol method — the autodev-injected F8 path calls
|
||||
this directly on a :class:`WarmStartWiredStrategy` instance.
|
||||
"""
|
||||
self._inner.reset_to_warm_start(loaded.pose)
|
||||
self._post_reset_remaining = self._max_frames
|
||||
self._baseline_floor = float(loaded.pre_reboot_covariance_norm)
|
||||
self._frames_since_save = 0
|
||||
|
||||
def health_snapshot(self) -> VioHealth:
|
||||
"""Forward unchanged — health is a strategy concern, not a wiring concern."""
|
||||
return self._inner.health_snapshot()
|
||||
|
||||
def current_strategy_label(
|
||||
self,
|
||||
) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
"""Forward unchanged so :class:`VioHealth.strategy_label` audit is honest."""
|
||||
return self._inner.current_strategy_label()
|
||||
|
||||
def _apply_post_reset_inflation(self, out: VioOutput) -> VioOutput:
|
||||
"""Inflate the emitted covariance by the configured factor + AC-8 floor.
|
||||
|
||||
AC-7: inflated norm = factor × strategy_emitted_norm. AC-8:
|
||||
further scale up so inflated norm ≥ ``_baseline_floor``. Both
|
||||
scalings preserve symmetry and positive-definiteness because
|
||||
they are pure positive scalar multiplications of the SPD
|
||||
matrix (eigenvalues stay strictly positive).
|
||||
"""
|
||||
original = np.asarray(out.pose_covariance_6x6, dtype=np.float64)
|
||||
inflated = original * self._inflation_factor
|
||||
inflated_norm = float(np.linalg.norm(inflated, ord="fro"))
|
||||
if (
|
||||
self._baseline_floor > 0.0
|
||||
and inflated_norm > 0.0
|
||||
and inflated_norm < self._baseline_floor
|
||||
):
|
||||
scale = self._baseline_floor / inflated_norm
|
||||
inflated = inflated * scale
|
||||
return replace(out, pose_covariance_6x6=inflated)
|
||||
|
||||
def _save_hint_from_output(self, out: VioOutput) -> None:
|
||||
"""Construct a :class:`WarmStartPose` from the last emitted output and save.
|
||||
|
||||
``velocity_b`` is left at zero — the wrapper has no velocity
|
||||
source on the per-frame save path (the strategy's
|
||||
:class:`VioOutput` does not expose velocity, and chasing it
|
||||
would require a numerical-differentiation sidecar that
|
||||
belongs in a future cycle). On F8 reload the strategy
|
||||
re-estimates velocity from its IMU integration, so a
|
||||
zero-velocity hint is acceptable for the recovery path.
|
||||
|
||||
Per-frame save failures do NOT propagate — they are logged
|
||||
at ERROR and swallowed (AC-NFR-no-crash). The hint store
|
||||
will be in whatever state the failed atomic-write left it
|
||||
(the AZ-280 contract guarantees no half-written file).
|
||||
"""
|
||||
hint = WarmStartPose(
|
||||
body_T_world=out.relative_pose_T,
|
||||
velocity_b=(0.0, 0.0, 0.0),
|
||||
bias=out.imu_bias,
|
||||
captured_at_ns=int(out.emitted_at_ns),
|
||||
)
|
||||
try:
|
||||
self._store.save(
|
||||
hint,
|
||||
pre_reboot_covariance_norm=self._last_emitted_covariance_norm,
|
||||
)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self._log.error(
|
||||
"warm-start hint save failed",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.save_failed",
|
||||
"kv": {
|
||||
"reason": str(exc),
|
||||
"frame_id": out.frame_id,
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _emit_prime_fdr(
|
||||
*,
|
||||
fdr_client: "FdrClient",
|
||||
source: str,
|
||||
strategy_label: str,
|
||||
bias_norm_value: float | None,
|
||||
staleness_ns: int | None,
|
||||
pre_reboot_covariance_norm: float | None,
|
||||
) -> None:
|
||||
"""Emit the single AZ-335 ``vio.warm_start`` FDR record."""
|
||||
record = FdrRecord(
|
||||
schema_version=CURRENT_SCHEMA_VERSION,
|
||||
ts=now_iso(),
|
||||
producer_id=WARM_START_PRODUCER_ID,
|
||||
kind="vio.warm_start",
|
||||
payload={
|
||||
"source": source,
|
||||
"strategy_label": strategy_label,
|
||||
"bias_norm": bias_norm_value,
|
||||
"staleness_ns": staleness_ns,
|
||||
"pre_reboot_covariance_norm": pre_reboot_covariance_norm,
|
||||
},
|
||||
)
|
||||
fdr_client.enqueue(record)
|
||||
|
||||
|
||||
def _emit_prime_log(
|
||||
*,
|
||||
log: Any,
|
||||
level: str,
|
||||
msg: str,
|
||||
source: str,
|
||||
strategy_label: str,
|
||||
extra_kv: dict[str, Any] | None = None,
|
||||
) -> None:
|
||||
"""Single emission point for prime-time INFO/WARN logs."""
|
||||
kv: dict[str, Any] = {
|
||||
"source": source,
|
||||
"strategy_label": strategy_label,
|
||||
}
|
||||
if extra_kv:
|
||||
kv.update(extra_kv)
|
||||
record_extra = {
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": f"c1.warm_start.{source}",
|
||||
"kv": kv,
|
||||
}
|
||||
if level == "warning":
|
||||
log.warning(msg, extra=record_extra)
|
||||
else:
|
||||
log.info(msg, extra=record_extra)
|
||||
|
||||
|
||||
def prime_warm_start_from_disk(
|
||||
strategy: WarmStartWiredStrategy,
|
||||
store: WarmStartHintStore,
|
||||
*,
|
||||
fdr_client: "FdrClient",
|
||||
) -> bool:
|
||||
"""F8 reboot prime hook — called at process startup before first ``process_frame``.
|
||||
|
||||
Reads the persisted hint via ``store.load()``:
|
||||
|
||||
- If a hint is loaded, calls :meth:`WarmStartWiredStrategy.prime_post_reboot`
|
||||
(which forwards to the inner strategy AND installs the AC-8 floor),
|
||||
emits one INFO log ``c1.warm_start.f8_reboot_disk``, and emits one
|
||||
FDR record ``vio.warm_start`` with ``source="f8_reboot_disk"``.
|
||||
- If ``store.load()`` returns ``None`` (cold start, corrupted file,
|
||||
calibration mismatch), emits one INFO log
|
||||
``c1.warm_start.cold_start_no_hint`` and one FDR record with
|
||||
``source="cold_start_no_hint"``. The strategy is left untouched
|
||||
and proceeds with its own INIT-state behaviour.
|
||||
|
||||
Returns ``True`` iff a hint was loaded AND applied. Never raises:
|
||||
a :class:`VioFatalError` from the inner strategy's
|
||||
:meth:`reset_to_warm_start` is caught, logged at ERROR
|
||||
(``c1.warm_start.reset_failed``), and the function returns
|
||||
``False`` so the camera ingest can still start in cold-start mode.
|
||||
"""
|
||||
log = get_logger(_LOGGER_NAME)
|
||||
strategy_label = strategy.current_strategy_label()
|
||||
loaded = store.load()
|
||||
if loaded is None:
|
||||
_emit_prime_log(
|
||||
log=log,
|
||||
level="info",
|
||||
msg="warm-start cold start — no prior hint",
|
||||
source=_SOURCE_COLD_START,
|
||||
strategy_label=strategy_label,
|
||||
)
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_COLD_START,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=None,
|
||||
staleness_ns=None,
|
||||
pre_reboot_covariance_norm=None,
|
||||
)
|
||||
return False
|
||||
try:
|
||||
strategy.prime_post_reboot(loaded)
|
||||
except Exception as exc:
|
||||
log.error(
|
||||
"warm-start prime_post_reboot failed",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.reset_failed",
|
||||
"kv": {
|
||||
"source": _SOURCE_F8_REBOOT,
|
||||
"strategy_label": strategy_label,
|
||||
"reason": str(exc),
|
||||
},
|
||||
},
|
||||
)
|
||||
return False
|
||||
staleness_ns = max(0, int(time.monotonic_ns()) - int(loaded.pose.captured_at_ns))
|
||||
_emit_prime_log(
|
||||
log=log,
|
||||
level="info",
|
||||
msg="warm-start F8 reboot — hint loaded from disk",
|
||||
source=_SOURCE_F8_REBOOT,
|
||||
strategy_label=strategy_label,
|
||||
extra_kv={
|
||||
"staleness_ns": staleness_ns,
|
||||
"pre_reboot_covariance_norm": loaded.pre_reboot_covariance_norm,
|
||||
},
|
||||
)
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_F8_REBOOT,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=bias_norm(loaded.pose.bias),
|
||||
staleness_ns=staleness_ns,
|
||||
pre_reboot_covariance_norm=loaded.pre_reboot_covariance_norm,
|
||||
)
|
||||
return True
|
||||
|
||||
|
||||
def prime_warm_start_from_fc(
|
||||
strategy: WarmStartWiredStrategy,
|
||||
source: WarmStartFcSource,
|
||||
store: WarmStartHintStore,
|
||||
*,
|
||||
fdr_client: "FdrClient",
|
||||
) -> bool:
|
||||
"""F2 takeoff prime hook — called once on the ``IN_AIR`` flight-state edge.
|
||||
|
||||
Asks the consumer-side cut for the FC EKF's last valid pose:
|
||||
|
||||
- If a hint is returned, calls :meth:`WarmStartWiredStrategy.reset_to_warm_start`
|
||||
(the inflation window arms WITHOUT an AC-8 floor — there is no
|
||||
pre-reboot baseline on the F2 path because the FC just provided
|
||||
a fresh pose), persists the same hint via ``store.save`` so the
|
||||
next F8 reboot can recover from it, and emits the INFO log +
|
||||
FDR record with ``source="f2_takeoff_fc"``.
|
||||
- If the source returns ``None`` or raises, emits one WARN log
|
||||
``c1.warm_start.f2_takeoff_fc_unavailable`` and an FDR record
|
||||
with ``source="cold_start_no_hint"``; the strategy is left in
|
||||
its current state and the camera ingest proceeds (AC-NFR-no-crash).
|
||||
|
||||
Returns ``True`` iff a hint was fetched, applied, AND persisted.
|
||||
Never raises.
|
||||
"""
|
||||
log = get_logger(_LOGGER_NAME)
|
||||
strategy_label = strategy.current_strategy_label()
|
||||
try:
|
||||
hint = source.fetch_warm_start_pose()
|
||||
except Exception as exc:
|
||||
log.warning(
|
||||
"warm-start FC fetch raised",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.f2_takeoff_fc_unavailable",
|
||||
"kv": {
|
||||
"source": _SOURCE_F2_TAKEOFF,
|
||||
"strategy_label": strategy_label,
|
||||
"reason": str(exc),
|
||||
},
|
||||
},
|
||||
)
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_COLD_START,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=None,
|
||||
staleness_ns=None,
|
||||
pre_reboot_covariance_norm=None,
|
||||
)
|
||||
return False
|
||||
if hint is None:
|
||||
log.warning(
|
||||
"warm-start FC has no valid pose yet",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.f2_takeoff_fc_unavailable",
|
||||
"kv": {
|
||||
"source": _SOURCE_F2_TAKEOFF,
|
||||
"strategy_label": strategy_label,
|
||||
"reason": "fc_returned_none",
|
||||
},
|
||||
},
|
||||
)
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_COLD_START,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=None,
|
||||
staleness_ns=None,
|
||||
pre_reboot_covariance_norm=None,
|
||||
)
|
||||
return False
|
||||
try:
|
||||
strategy.reset_to_warm_start(hint)
|
||||
except Exception as exc:
|
||||
log.error(
|
||||
"warm-start F2 reset_to_warm_start failed",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.reset_failed",
|
||||
"kv": {
|
||||
"source": _SOURCE_F2_TAKEOFF,
|
||||
"strategy_label": strategy_label,
|
||||
"reason": str(exc),
|
||||
},
|
||||
},
|
||||
)
|
||||
return False
|
||||
try:
|
||||
store.save(hint, pre_reboot_covariance_norm=0.0)
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
log.error(
|
||||
"warm-start F2 persist failed",
|
||||
extra={
|
||||
"component": _LOGGER_COMPONENT,
|
||||
"kind": "c1.warm_start.save_failed",
|
||||
"kv": {
|
||||
"source": _SOURCE_F2_TAKEOFF,
|
||||
"strategy_label": strategy_label,
|
||||
"reason": str(exc),
|
||||
},
|
||||
},
|
||||
)
|
||||
# the strategy already accepted the hint; the FDR record
|
||||
# below still records the F2 prime for audit, but we return
|
||||
# False to indicate persistence did not complete. The next
|
||||
# successful per-frame save will restore the on-disk state.
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_F2_TAKEOFF,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=bias_norm(hint.bias),
|
||||
staleness_ns=None,
|
||||
pre_reboot_covariance_norm=None,
|
||||
)
|
||||
return False
|
||||
_emit_prime_log(
|
||||
log=log,
|
||||
level="info",
|
||||
msg="warm-start F2 takeoff — hint primed from FC",
|
||||
source=_SOURCE_F2_TAKEOFF,
|
||||
strategy_label=strategy_label,
|
||||
)
|
||||
_emit_prime_fdr(
|
||||
fdr_client=fdr_client,
|
||||
source=_SOURCE_F2_TAKEOFF,
|
||||
strategy_label=strategy_label,
|
||||
bias_norm_value=bias_norm(hint.bias),
|
||||
staleness_ns=None,
|
||||
pre_reboot_covariance_norm=None,
|
||||
)
|
||||
return True
|
||||
@@ -0,0 +1,932 @@
|
||||
"""AZ-335 — C1 warm-start hint persistence + F8 reboot recovery wiring tests.
|
||||
|
||||
Covers all 10 acceptance criteria from
|
||||
``_docs/02_tasks/todo/AZ-335_c1_warm_start_recovery.md`` plus three
|
||||
non-functional requirements (perf-save, perf-load, no-crash). Tests
|
||||
target both the c1-internal :class:`JsonSidecarWarmStartHintStore`
|
||||
and the runtime-root :class:`WarmStartWiredStrategy` + prime hooks.
|
||||
|
||||
The wiring tests construct a deliberately minimal scriptable
|
||||
:class:`_FakeVioStrategy` (kept local — the c1_vio strategy backends
|
||||
already exercise the strategy-internal Protocol shape exhaustively;
|
||||
this file's job is to verify the **wiring** layer behaves correctly
|
||||
when wrapped around any strategy). The store tests use the real
|
||||
:class:`Sha256Sidecar` (atomicwrites) on tmp_path — no fakes here
|
||||
because the AC-1/AC-2/AC-10 contracts ARE about the on-disk
|
||||
behaviour itself.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import Any, Literal
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.nav import (
|
||||
FeatureQuality,
|
||||
ImuBias,
|
||||
ImuWindow,
|
||||
NavCameraFrame,
|
||||
VioHealth,
|
||||
VioOutput,
|
||||
VioState,
|
||||
WarmStartPose,
|
||||
)
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard.components.c1_vio.warm_start_store import (
|
||||
HINT_FILENAME,
|
||||
HINT_SCHEMA_VERSION,
|
||||
JsonSidecarWarmStartHintStore,
|
||||
LoadedWarmStartHint,
|
||||
WarmStartFcSource,
|
||||
WarmStartHintStore,
|
||||
)
|
||||
from gps_denied_onboard.fdr_client.fakes import FakeFdrSink
|
||||
from gps_denied_onboard.helpers.sha256_sidecar import SIDECAR_SUFFIX
|
||||
from gps_denied_onboard.runtime_root.warm_start_wiring import (
|
||||
WARM_START_PRODUCER_ID,
|
||||
WarmStartWiredStrategy,
|
||||
prime_warm_start_from_disk,
|
||||
prime_warm_start_from_fc,
|
||||
)
|
||||
|
||||
|
||||
_DEFAULT_CALIBRATION_ID = "adti26"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Shared builders.
|
||||
|
||||
|
||||
def _make_pose(yaw_deg: float = 0.0, x: float = 1.0, y: float = 2.0, z: float = 3.0) -> gtsam.Pose3:
|
||||
"""A concrete SE(3) pose with a deterministic, non-identity rotation."""
|
||||
yaw = np.deg2rad(yaw_deg)
|
||||
R = np.array(
|
||||
[
|
||||
[np.cos(yaw), -np.sin(yaw), 0.0],
|
||||
[np.sin(yaw), np.cos(yaw), 0.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
T = np.eye(4, dtype=np.float64)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = [x, y, z]
|
||||
return gtsam.Pose3(T)
|
||||
|
||||
|
||||
def _make_hint(
|
||||
*,
|
||||
yaw_deg: float = 5.0,
|
||||
velocity: tuple[float, float, float] = (1.0, 2.0, 3.0),
|
||||
accel_bias: tuple[float, float, float] = (0.01, -0.02, 0.03),
|
||||
gyro_bias: tuple[float, float, float] = (0.001, 0.002, -0.003),
|
||||
captured_at_ns: int = 1_700_000_000_000,
|
||||
) -> WarmStartPose:
|
||||
return WarmStartPose(
|
||||
body_T_world=_make_pose(yaw_deg=yaw_deg),
|
||||
velocity_b=velocity,
|
||||
bias=ImuBias(accel_bias=accel_bias, gyro_bias=gyro_bias),
|
||||
captured_at_ns=captured_at_ns,
|
||||
)
|
||||
|
||||
|
||||
def _make_calibration() -> CameraCalibration:
|
||||
return CameraCalibration(
|
||||
camera_id="cam0",
|
||||
intrinsics_3x3=np.eye(3, dtype=np.float64),
|
||||
distortion=np.zeros(4, dtype=np.float64),
|
||||
body_to_camera_se3=_make_pose(),
|
||||
acquisition_method="checker_board",
|
||||
)
|
||||
|
||||
|
||||
def _make_imu_window() -> ImuWindow:
|
||||
return ImuWindow(samples=tuple(), ts_start_ns=0, ts_end_ns=0)
|
||||
|
||||
|
||||
def _make_frame(frame_id: int = 1) -> NavCameraFrame:
|
||||
return NavCameraFrame(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime(2026, 5, 14, 0, 0, 0, tzinfo=timezone.utc),
|
||||
image=np.zeros((10, 10), dtype=np.uint8),
|
||||
camera_calibration_id="cam0",
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Local scriptable VioStrategy fake — wiring tests only.
|
||||
|
||||
|
||||
@dataclass
|
||||
class _ResetCall:
|
||||
"""One captured ``reset_to_warm_start`` invocation on the fake strategy."""
|
||||
|
||||
hint: WarmStartPose
|
||||
|
||||
|
||||
class _FakeVioStrategy:
|
||||
"""Scriptable minimal :class:`VioStrategy` for AZ-335 wiring tests.
|
||||
|
||||
Returns a deterministic per-call :class:`VioOutput` whose
|
||||
``pose_covariance_6x6`` is the value most recently set via
|
||||
:meth:`set_emit_covariance` (default ``np.eye(6) * 0.01``).
|
||||
Each :meth:`reset_to_warm_start` invocation is captured in
|
||||
:attr:`reset_calls` so wiring tests can assert single-call,
|
||||
correct-hint, no-call semantics.
|
||||
"""
|
||||
|
||||
def __init__(self, *, label: Literal["okvis2", "vins_mono", "klt_ransac"] = "klt_ransac") -> None:
|
||||
self._label = label
|
||||
self._next_cov = np.eye(6, dtype=np.float64) * 0.01
|
||||
self._next_bias = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
|
||||
self._frame_counter = 0
|
||||
self.reset_calls: list[_ResetCall] = []
|
||||
self._raise_on_reset: Exception | None = None
|
||||
|
||||
def set_emit_covariance(self, cov: np.ndarray) -> None:
|
||||
self._next_cov = np.asarray(cov, dtype=np.float64)
|
||||
|
||||
def set_emit_bias(self, bias: ImuBias) -> None:
|
||||
self._next_bias = bias
|
||||
|
||||
def script_reset_failure(self, exc: Exception) -> None:
|
||||
self._raise_on_reset = exc
|
||||
|
||||
def process_frame(
|
||||
self,
|
||||
frame: NavCameraFrame,
|
||||
imu: ImuWindow,
|
||||
calibration: CameraCalibration,
|
||||
) -> VioOutput:
|
||||
self._frame_counter += 1
|
||||
return VioOutput(
|
||||
frame_id=f"frame-{self._frame_counter}",
|
||||
relative_pose_T=_make_pose(),
|
||||
pose_covariance_6x6=self._next_cov.copy(),
|
||||
imu_bias=self._next_bias,
|
||||
feature_quality=FeatureQuality(
|
||||
tracked=80, new=2, lost=1, mean_parallax=5.0, mre_px=0.8
|
||||
),
|
||||
emitted_at_ns=1_700_000_000_000 + self._frame_counter,
|
||||
)
|
||||
|
||||
def reset_to_warm_start(self, hint: WarmStartPose) -> None:
|
||||
if self._raise_on_reset is not None:
|
||||
raise self._raise_on_reset
|
||||
self.reset_calls.append(_ResetCall(hint=hint))
|
||||
|
||||
def health_snapshot(self) -> VioHealth:
|
||||
return VioHealth(state=VioState.TRACKING, consecutive_lost=0, bias_norm=0.0)
|
||||
|
||||
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
return self._label
|
||||
|
||||
|
||||
class _FakeFcSource:
|
||||
"""Scriptable :class:`WarmStartFcSource` for F2 takeoff tests."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
hint: WarmStartPose | None = None,
|
||||
raise_with: Exception | None = None,
|
||||
calibration_id: str = _DEFAULT_CALIBRATION_ID,
|
||||
) -> None:
|
||||
self._hint = hint
|
||||
self._raise_with = raise_with
|
||||
self._calibration_id = calibration_id
|
||||
self.fetch_call_count = 0
|
||||
|
||||
def fetch_warm_start_pose(self) -> WarmStartPose | None:
|
||||
self.fetch_call_count += 1
|
||||
if self._raise_with is not None:
|
||||
raise self._raise_with
|
||||
return self._hint
|
||||
|
||||
def calibration_id(self) -> str:
|
||||
return self._calibration_id
|
||||
|
||||
|
||||
def _make_wired(
|
||||
inner: _FakeVioStrategy,
|
||||
store: WarmStartHintStore,
|
||||
*,
|
||||
warm_start_max_frames: int = 5,
|
||||
inflation_factor: float = 2.0,
|
||||
save_period: int = 5,
|
||||
) -> WarmStartWiredStrategy:
|
||||
return WarmStartWiredStrategy(
|
||||
inner=inner,
|
||||
store=store,
|
||||
warm_start_max_frames=warm_start_max_frames,
|
||||
post_reset_covariance_inflation_factor=inflation_factor,
|
||||
warm_start_save_period_frames=save_period,
|
||||
)
|
||||
|
||||
|
||||
def _drive_frames(wired: WarmStartWiredStrategy, n: int) -> list[VioOutput]:
|
||||
return [
|
||||
wired.process_frame(_make_frame(i), _make_imu_window(), _make_calibration())
|
||||
for i in range(1, n + 1)
|
||||
]
|
||||
|
||||
|
||||
# ===========================================================================
|
||||
# Store tests — AC-1, AC-2, AC-9, AC-10, NFR-perf-save, NFR-perf-load,
|
||||
# Risk-2 calibration-mismatch.
|
||||
|
||||
|
||||
class TestStoreAc1RoundTrip:
|
||||
def test_save_then_load_returns_deep_equal_hint(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
hint = _make_hint()
|
||||
|
||||
# Act
|
||||
store.save(hint, pre_reboot_covariance_norm=0.123)
|
||||
loaded = store.load()
|
||||
|
||||
# Assert
|
||||
assert loaded is not None
|
||||
assert isinstance(loaded, LoadedWarmStartHint)
|
||||
assert loaded.calibration_id == _DEFAULT_CALIBRATION_ID
|
||||
assert loaded.pre_reboot_covariance_norm == pytest.approx(0.123)
|
||||
np.testing.assert_array_almost_equal(
|
||||
loaded.pose.body_T_world.matrix(), hint.body_T_world.matrix()
|
||||
)
|
||||
assert loaded.pose.velocity_b == hint.velocity_b
|
||||
assert loaded.pose.bias == hint.bias
|
||||
assert loaded.pose.captured_at_ns == hint.captured_at_ns
|
||||
|
||||
def test_save_creates_payload_and_sidecar_files(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
hint = _make_hint()
|
||||
|
||||
# Act
|
||||
store.save(hint, pre_reboot_covariance_norm=0.5)
|
||||
|
||||
# Assert
|
||||
assert (tmp_path / HINT_FILENAME).exists()
|
||||
assert (tmp_path / (HINT_FILENAME + SIDECAR_SUFFIX)).exists()
|
||||
assert store.payload_path == tmp_path / HINT_FILENAME
|
||||
|
||||
def test_save_creates_missing_parent_directory(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
nested = tmp_path / "nested" / "dirs" / "warm_start"
|
||||
store = JsonSidecarWarmStartHintStore(nested, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
hint = _make_hint()
|
||||
|
||||
# Act
|
||||
store.save(hint, pre_reboot_covariance_norm=0.0)
|
||||
|
||||
# Assert
|
||||
assert (nested / HINT_FILENAME).exists()
|
||||
|
||||
|
||||
class TestStoreAc2Corrupted:
|
||||
def _seed_valid_then_flip_one_byte(
|
||||
self, tmp_path: Path
|
||||
) -> tuple[JsonSidecarWarmStartHintStore, Path]:
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=0.1)
|
||||
payload_path = tmp_path / HINT_FILENAME
|
||||
original = payload_path.read_bytes()
|
||||
# Flip one byte mid-payload to trigger sha256 mismatch but keep
|
||||
# the file structurally present and the sidecar untouched.
|
||||
idx = len(original) // 2
|
||||
corrupted = original[:idx] + bytes([(original[idx] + 1) % 256]) + original[idx + 1 :]
|
||||
payload_path.write_bytes(corrupted)
|
||||
return store, payload_path
|
||||
|
||||
def test_corrupted_payload_returns_none(self, tmp_path: Path, caplog: Any) -> None:
|
||||
# Arrange
|
||||
store, _ = self._seed_valid_then_flip_one_byte(tmp_path)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.WARNING):
|
||||
loaded = store.load()
|
||||
|
||||
# Assert
|
||||
assert loaded is None
|
||||
warn_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", "") == "c1.warm_start.corrupted"
|
||||
]
|
||||
assert len(warn_records) == 1
|
||||
assert warn_records[0].levelname == "WARNING"
|
||||
|
||||
def test_corrupted_file_is_not_silently_deleted(self, tmp_path: Path) -> None:
|
||||
# Arrange + Act
|
||||
store, payload_path = self._seed_valid_then_flip_one_byte(tmp_path)
|
||||
_ = store.load()
|
||||
|
||||
# Assert
|
||||
assert payload_path.exists(), "AC-2: operator may want to forensically inspect"
|
||||
|
||||
def test_structurally_invalid_json_returns_none_with_warn(
|
||||
self, tmp_path: Path, caplog: Any
|
||||
) -> None:
|
||||
# Arrange — write a payload with the WRONG schema version and rebuild the sidecar
|
||||
# so sha256 verifies but envelope deserialisation rejects.
|
||||
from gps_denied_onboard.helpers.sha256_sidecar import Sha256Sidecar
|
||||
|
||||
bad_payload = b'{"version": 999, "calibration_id": "x", "pose": {}}'
|
||||
Sha256Sidecar.write_atomic_and_sidecar(tmp_path / HINT_FILENAME, bad_payload)
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id="x")
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.WARNING):
|
||||
loaded = store.load()
|
||||
|
||||
# Assert
|
||||
assert loaded is None
|
||||
kinds = [getattr(r, "kind", "") for r in caplog.records]
|
||||
assert "c1.warm_start.corrupted" in kinds
|
||||
|
||||
|
||||
class TestStoreAc3CalibrationMismatch:
|
||||
def test_calibration_mismatch_returns_none_with_specific_warn(
|
||||
self, tmp_path: Path, caplog: Any
|
||||
) -> None:
|
||||
# Arrange
|
||||
producer = JsonSidecarWarmStartHintStore(tmp_path, calibration_id="OLD_CAL")
|
||||
producer.save(_make_hint(), pre_reboot_covariance_norm=0.1)
|
||||
consumer = JsonSidecarWarmStartHintStore(tmp_path, calibration_id="NEW_CAL")
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.WARNING):
|
||||
loaded = consumer.load()
|
||||
|
||||
# Assert
|
||||
assert loaded is None
|
||||
warn_records = [
|
||||
r
|
||||
for r in caplog.records
|
||||
if getattr(r, "kind", "") == "c1.warm_start.calibration_mismatch"
|
||||
]
|
||||
assert len(warn_records) == 1
|
||||
|
||||
|
||||
class TestStoreAc9Clear:
|
||||
def test_clear_removes_payload_and_sidecar(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=0.1)
|
||||
|
||||
# Act
|
||||
store.clear()
|
||||
|
||||
# Assert
|
||||
assert not (tmp_path / HINT_FILENAME).exists()
|
||||
assert not (tmp_path / (HINT_FILENAME + SIDECAR_SUFFIX)).exists()
|
||||
assert store.load() is None
|
||||
|
||||
def test_clear_emits_info_log(self, tmp_path: Path, caplog: Any) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.INFO):
|
||||
store.clear()
|
||||
|
||||
# Assert
|
||||
info_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", "") == "c1.warm_start.cleared"
|
||||
]
|
||||
assert len(info_records) == 1
|
||||
assert info_records[0].levelname == "INFO"
|
||||
|
||||
def test_clear_is_idempotent(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
|
||||
# Act + Assert — first clear with no files MUST NOT raise
|
||||
store.clear()
|
||||
store.clear()
|
||||
|
||||
|
||||
class TestStoreAc10Atomicity:
|
||||
def test_kill_mid_save_leaves_prior_hint_loadable(self, tmp_path: Path) -> None:
|
||||
"""Simulate a crash mid-save by writing a temp file but never renaming.
|
||||
|
||||
``Sha256Sidecar.write_atomic_and_sidecar`` uses
|
||||
``atomicwrites.atomic_write`` (temp-file + ``os.replace``), so
|
||||
a mid-write crash never leaves a partial `c1_warm_start.json`.
|
||||
We model the "process killed mid-save" scenario by leaving a
|
||||
stray temp file alongside an already-committed prior hint;
|
||||
:meth:`load` must still return the prior valid hint.
|
||||
"""
|
||||
# Arrange — first save commits a known hint
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
prior = _make_hint(yaw_deg=0.0)
|
||||
store.save(prior, pre_reboot_covariance_norm=0.1)
|
||||
|
||||
# Simulate a half-written temp file from a "killed" second save.
|
||||
# atomicwrites uses a temp file with a `.<name>.<rand>` prefix.
|
||||
stray = tmp_path / f".{HINT_FILENAME}.partial-write-stray"
|
||||
stray.write_bytes(b"this-is-half-written-junk")
|
||||
|
||||
# Act
|
||||
loaded = store.load()
|
||||
|
||||
# Assert — the prior valid hint loads despite the stray temp file.
|
||||
assert loaded is not None
|
||||
np.testing.assert_array_almost_equal(
|
||||
loaded.pose.body_T_world.matrix(), prior.body_T_world.matrix()
|
||||
)
|
||||
# The stray file was NOT consumed as the hint.
|
||||
assert stray.exists()
|
||||
|
||||
|
||||
class TestStoreLifecycle:
|
||||
def test_load_returns_none_when_no_file(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
|
||||
# Act + Assert
|
||||
assert store.load() is None
|
||||
|
||||
def test_default_impl_satisfies_protocol(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
|
||||
# Assert — runtime_checkable Protocol conformance
|
||||
assert isinstance(store, WarmStartHintStore)
|
||||
|
||||
|
||||
class TestStoreNfrPerf:
|
||||
def test_nfr_perf_save_p99_under_50ms(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
hint = _make_hint()
|
||||
n = 200 # bounded — full perf bench lives in C1-PT-01 Tier-2
|
||||
|
||||
# Act
|
||||
timings_ms = []
|
||||
for _ in range(n):
|
||||
t0 = time.perf_counter()
|
||||
store.save(hint, pre_reboot_covariance_norm=0.1)
|
||||
timings_ms.append((time.perf_counter() - t0) * 1000.0)
|
||||
|
||||
# Assert — p99 under 50ms; this is a smoke-budget on dev hardware,
|
||||
# the production budget is on Tier-2 NVMe per the task NFR.
|
||||
p99 = float(np.percentile(timings_ms, 99))
|
||||
assert p99 < 50.0, f"save p99 = {p99:.2f}ms exceeds 50ms NFR budget"
|
||||
|
||||
def test_nfr_perf_load_p99_under_20ms(self, tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
store = JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=0.1)
|
||||
n = 200
|
||||
|
||||
# Act
|
||||
timings_ms = []
|
||||
for _ in range(n):
|
||||
t0 = time.perf_counter()
|
||||
loaded = store.load()
|
||||
timings_ms.append((time.perf_counter() - t0) * 1000.0)
|
||||
assert loaded is not None # sanity
|
||||
|
||||
# Assert
|
||||
p99 = float(np.percentile(timings_ms, 99))
|
||||
assert p99 < 20.0, f"load p99 = {p99:.2f}ms exceeds 20ms NFR budget"
|
||||
|
||||
|
||||
# ===========================================================================
|
||||
# Wiring tests — AC-3 .. AC-8, NFR-no-crash.
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fdr_sink() -> FakeFdrSink:
|
||||
return FakeFdrSink(producer_id=WARM_START_PRODUCER_ID)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fake_strategy() -> _FakeVioStrategy:
|
||||
return _FakeVioStrategy()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def store(tmp_path: Path) -> JsonSidecarWarmStartHintStore:
|
||||
return JsonSidecarWarmStartHintStore(tmp_path, calibration_id=_DEFAULT_CALIBRATION_ID)
|
||||
|
||||
|
||||
class TestWiringAc3ColdStart:
|
||||
def test_cold_start_does_not_invoke_reset(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.INFO):
|
||||
applied = prime_warm_start_from_disk(wired, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert
|
||||
assert applied is False
|
||||
assert fake_strategy.reset_calls == []
|
||||
info_records = [
|
||||
r
|
||||
for r in caplog.records
|
||||
if getattr(r, "kind", "") == "c1.warm_start.cold_start_no_hint"
|
||||
]
|
||||
assert len(info_records) == 1
|
||||
cold_records = [r for r in fdr_sink.records if r.kind == "vio.warm_start"]
|
||||
assert len(cold_records) == 1
|
||||
assert cold_records[0].payload["source"] == "cold_start_no_hint"
|
||||
assert cold_records[0].payload["bias_norm"] is None
|
||||
|
||||
|
||||
class TestWiringAc4F8Reboot:
|
||||
def test_f8_reboot_loads_hint_calls_reset_emits_fdr(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange — seed a hint on disk
|
||||
prior_hint = _make_hint(yaw_deg=10.0)
|
||||
store.save(prior_hint, pre_reboot_covariance_norm=0.0625)
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.INFO):
|
||||
applied = prime_warm_start_from_disk(wired, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert
|
||||
assert applied is True
|
||||
assert len(fake_strategy.reset_calls) == 1
|
||||
np.testing.assert_array_almost_equal(
|
||||
fake_strategy.reset_calls[0].hint.body_T_world.matrix(),
|
||||
prior_hint.body_T_world.matrix(),
|
||||
)
|
||||
# AC-8 baseline floor installed
|
||||
assert wired.baseline_floor == pytest.approx(0.0625)
|
||||
info_records = [
|
||||
r
|
||||
for r in caplog.records
|
||||
if getattr(r, "kind", "") == "c1.warm_start.f8_reboot_disk"
|
||||
]
|
||||
assert len(info_records) == 1
|
||||
fdr_records = [r for r in fdr_sink.records if r.kind == "vio.warm_start"]
|
||||
assert len(fdr_records) == 1
|
||||
assert fdr_records[0].payload["source"] == "f8_reboot_disk"
|
||||
assert fdr_records[0].payload["pre_reboot_covariance_norm"] == pytest.approx(0.0625)
|
||||
assert fdr_records[0].payload["bias_norm"] is not None
|
||||
assert fdr_records[0].payload["staleness_ns"] is not None
|
||||
|
||||
|
||||
class TestWiringAc5F2Takeoff:
|
||||
def test_f2_takeoff_fetches_fc_calls_reset_persists(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange
|
||||
fc_hint = _make_hint(yaw_deg=20.0)
|
||||
source = _FakeFcSource(hint=fc_hint)
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.INFO):
|
||||
applied = prime_warm_start_from_fc(wired, source, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert
|
||||
assert applied is True
|
||||
assert source.fetch_call_count == 1
|
||||
assert len(fake_strategy.reset_calls) == 1
|
||||
np.testing.assert_array_almost_equal(
|
||||
fake_strategy.reset_calls[0].hint.body_T_world.matrix(),
|
||||
fc_hint.body_T_world.matrix(),
|
||||
)
|
||||
# F2 path persists the hint so a subsequent F8 reboot can recover it.
|
||||
loaded = store.load()
|
||||
assert loaded is not None
|
||||
np.testing.assert_array_almost_equal(
|
||||
loaded.pose.body_T_world.matrix(), fc_hint.body_T_world.matrix()
|
||||
)
|
||||
# AC-8 floor is NOT installed on the F2 path (no pre-reboot baseline).
|
||||
assert wired.baseline_floor == pytest.approx(0.0)
|
||||
info_records = [
|
||||
r
|
||||
for r in caplog.records
|
||||
if getattr(r, "kind", "") == "c1.warm_start.f2_takeoff_fc"
|
||||
]
|
||||
assert len(info_records) == 1
|
||||
fdr_records = [r for r in fdr_sink.records if r.kind == "vio.warm_start"]
|
||||
assert len(fdr_records) == 1
|
||||
assert fdr_records[0].payload["source"] == "f2_takeoff_fc"
|
||||
|
||||
|
||||
class TestWiringAc6PerFrameSave:
|
||||
def test_per_frame_save_respects_period(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
# Arrange — period = 5; 12 frames → save fires at frames 5 and 10 only
|
||||
wired = _make_wired(fake_strategy, store, save_period=5)
|
||||
|
||||
# Act
|
||||
outputs = _drive_frames(wired, 12)
|
||||
|
||||
# Assert
|
||||
assert len(outputs) == 12
|
||||
# The on-disk hint should reflect frame 10's emit, not frame 12's.
|
||||
loaded = store.load()
|
||||
assert loaded is not None
|
||||
assert loaded.pose.captured_at_ns == outputs[9].emitted_at_ns
|
||||
|
||||
def test_save_period_one_saves_every_frame(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
) -> None:
|
||||
# Arrange
|
||||
wired = _make_wired(fake_strategy, store, save_period=1)
|
||||
|
||||
# Act
|
||||
outputs = _drive_frames(wired, 3)
|
||||
|
||||
# Assert — last save reflects the most recent frame
|
||||
loaded = store.load()
|
||||
assert loaded is not None
|
||||
assert loaded.pose.captured_at_ns == outputs[-1].emitted_at_ns
|
||||
|
||||
|
||||
class TestWiringAc7PostResetInflation:
|
||||
def test_first_n_frames_inflated_then_unmodified(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
) -> None:
|
||||
# Arrange — strategy emits cov of Frobenius norm 1.0, factor=2.0,
|
||||
# window=5 frames. Save period large enough that no save fires
|
||||
# in the inflation window for cleaner assertion.
|
||||
emit_cov = np.eye(6, dtype=np.float64) * (1.0 / np.sqrt(6)) # ||·||_F = 1.0
|
||||
fake_strategy.set_emit_covariance(emit_cov)
|
||||
wired = _make_wired(
|
||||
fake_strategy,
|
||||
store,
|
||||
warm_start_max_frames=5,
|
||||
inflation_factor=2.0,
|
||||
save_period=100,
|
||||
)
|
||||
wired.reset_to_warm_start(_make_hint())
|
||||
|
||||
# Act — drive 6 frames; the first 5 inflated, the 6th unmodified.
|
||||
outputs = _drive_frames(wired, 6)
|
||||
|
||||
# Assert
|
||||
for i in range(5):
|
||||
norm = float(np.linalg.norm(outputs[i].pose_covariance_6x6, ord="fro"))
|
||||
assert norm == pytest.approx(2.0, abs=1e-9), (
|
||||
f"Frame {i + 1}: expected inflated norm 2.0, got {norm}"
|
||||
)
|
||||
norm6 = float(np.linalg.norm(outputs[5].pose_covariance_6x6, ord="fro"))
|
||||
assert norm6 == pytest.approx(1.0, abs=1e-9)
|
||||
|
||||
def test_no_inflation_when_no_reset_was_called(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
) -> None:
|
||||
# Arrange — the wrapper without any reset call should pass through.
|
||||
emit_cov = np.eye(6, dtype=np.float64) * (1.0 / np.sqrt(6))
|
||||
fake_strategy.set_emit_covariance(emit_cov)
|
||||
wired = _make_wired(
|
||||
fake_strategy, store, save_period=100, warm_start_max_frames=5, inflation_factor=2.0
|
||||
)
|
||||
|
||||
# Act
|
||||
out = wired.process_frame(_make_frame(), _make_imu_window(), _make_calibration())
|
||||
|
||||
# Assert
|
||||
norm = float(np.linalg.norm(out.pose_covariance_6x6, ord="fro"))
|
||||
assert norm == pytest.approx(1.0, abs=1e-9)
|
||||
|
||||
|
||||
class TestWiringAc8CovarianceFloor:
|
||||
def test_post_reboot_floor_enforced_above_inflation_alone(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
) -> None:
|
||||
# Arrange — pre-reboot baseline X = 5.0; strategy emits norm 1.0
|
||||
# so 2× inflation alone is only 2.0, well below X. Floor must
|
||||
# bump every output up to ≥ 5.0.
|
||||
baseline_x = 5.0
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=baseline_x)
|
||||
emit_cov = np.eye(6, dtype=np.float64) * (1.0 / np.sqrt(6))
|
||||
fake_strategy.set_emit_covariance(emit_cov)
|
||||
wired = _make_wired(
|
||||
fake_strategy,
|
||||
store,
|
||||
warm_start_max_frames=5,
|
||||
inflation_factor=2.0,
|
||||
save_period=100,
|
||||
)
|
||||
|
||||
# Act — F8 prime installs the floor, then 5 frames flow through
|
||||
applied = prime_warm_start_from_disk(wired, store, fdr_client=fdr_sink)
|
||||
assert applied is True
|
||||
outputs = _drive_frames(wired, 5)
|
||||
|
||||
# Assert — every post-reset frame's emitted norm ≥ X
|
||||
for i, out in enumerate(outputs):
|
||||
norm = float(np.linalg.norm(out.pose_covariance_6x6, ord="fro"))
|
||||
assert norm >= baseline_x - 1e-9, (
|
||||
f"AC-8 floor breached on frame {i + 1}: norm {norm} < baseline {baseline_x}"
|
||||
)
|
||||
|
||||
def test_post_reboot_floor_does_not_lower_when_inflation_alone_already_above(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
) -> None:
|
||||
# Arrange — baseline X = 0.5; strategy emits norm 1.0; inflation 2.0
|
||||
# alone gives 2.0 which already exceeds X. Floor must NOT scale down.
|
||||
baseline_x = 0.5
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=baseline_x)
|
||||
emit_cov = np.eye(6, dtype=np.float64) * (1.0 / np.sqrt(6))
|
||||
fake_strategy.set_emit_covariance(emit_cov)
|
||||
wired = _make_wired(fake_strategy, store, save_period=100)
|
||||
|
||||
# Act
|
||||
applied = prime_warm_start_from_disk(wired, store, fdr_client=fdr_sink)
|
||||
assert applied is True
|
||||
outputs = _drive_frames(wired, 1)
|
||||
|
||||
# Assert — norm is the inflated value (2.0), NOT the baseline (0.5)
|
||||
norm = float(np.linalg.norm(outputs[0].pose_covariance_6x6, ord="fro"))
|
||||
assert norm == pytest.approx(2.0, abs=1e-9)
|
||||
|
||||
|
||||
class TestWiringNfrNoCrash:
|
||||
def test_fc_source_raising_does_not_crash(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange
|
||||
source = _FakeFcSource(raise_with=RuntimeError("FC link down"))
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.WARNING):
|
||||
applied = prime_warm_start_from_fc(wired, source, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert — degrades to cold-start; process keeps running
|
||||
assert applied is False
|
||||
assert fake_strategy.reset_calls == []
|
||||
warn_records = [
|
||||
r
|
||||
for r in caplog.records
|
||||
if getattr(r, "kind", "") == "c1.warm_start.f2_takeoff_fc_unavailable"
|
||||
]
|
||||
assert len(warn_records) == 1
|
||||
|
||||
def test_fc_source_returning_none_does_not_crash(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange
|
||||
source = _FakeFcSource(hint=None)
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.WARNING):
|
||||
applied = prime_warm_start_from_fc(wired, source, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert
|
||||
assert applied is False
|
||||
assert fake_strategy.reset_calls == []
|
||||
|
||||
def test_per_frame_save_failure_does_not_crash(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange — a store whose save always raises
|
||||
class _BoomStore:
|
||||
def save(self, hint: WarmStartPose, *, pre_reboot_covariance_norm: float) -> None:
|
||||
raise OSError("disk full")
|
||||
|
||||
def load(self) -> LoadedWarmStartHint | None:
|
||||
return None
|
||||
|
||||
def clear(self) -> None:
|
||||
return None
|
||||
|
||||
wired = _make_wired(fake_strategy, _BoomStore(), save_period=1) # type: ignore[arg-type]
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.ERROR):
|
||||
out = wired.process_frame(_make_frame(), _make_imu_window(), _make_calibration())
|
||||
|
||||
# Assert — frame still emitted, error logged, no exception escapes
|
||||
assert out is not None
|
||||
err_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", "") == "c1.warm_start.save_failed"
|
||||
]
|
||||
assert len(err_records) == 1
|
||||
|
||||
def test_inner_strategy_reset_failure_does_not_crash_prime(
|
||||
self,
|
||||
fake_strategy: _FakeVioStrategy,
|
||||
store: JsonSidecarWarmStartHintStore,
|
||||
fdr_sink: FakeFdrSink,
|
||||
caplog: Any,
|
||||
) -> None:
|
||||
# Arrange
|
||||
store.save(_make_hint(), pre_reboot_covariance_norm=0.1)
|
||||
fake_strategy.script_reset_failure(RuntimeError("native bridge boom"))
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.ERROR):
|
||||
applied = prime_warm_start_from_disk(wired, store, fdr_client=fdr_sink)
|
||||
|
||||
# Assert
|
||||
assert applied is False
|
||||
err_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", "") == "c1.warm_start.reset_failed"
|
||||
]
|
||||
assert len(err_records) == 1
|
||||
|
||||
|
||||
class TestWiringForwarders:
|
||||
def test_health_snapshot_forwards_to_inner(
|
||||
self, fake_strategy: _FakeVioStrategy, store: JsonSidecarWarmStartHintStore
|
||||
) -> None:
|
||||
# Arrange
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Assert
|
||||
assert wired.health_snapshot().state == VioState.TRACKING
|
||||
|
||||
def test_current_strategy_label_forwards_to_inner(
|
||||
self, fake_strategy: _FakeVioStrategy, store: JsonSidecarWarmStartHintStore
|
||||
) -> None:
|
||||
# Arrange
|
||||
wired = _make_wired(fake_strategy, store)
|
||||
|
||||
# Assert
|
||||
assert wired.current_strategy_label() == "klt_ransac"
|
||||
|
||||
def test_wrapper_constructor_rejects_inflation_factor_le_one(
|
||||
self, fake_strategy: _FakeVioStrategy, store: JsonSidecarWarmStartHintStore
|
||||
) -> None:
|
||||
# Arrange + Act + Assert
|
||||
with pytest.raises(ValueError):
|
||||
WarmStartWiredStrategy(
|
||||
inner=fake_strategy,
|
||||
store=store,
|
||||
warm_start_max_frames=5,
|
||||
post_reset_covariance_inflation_factor=1.0,
|
||||
warm_start_save_period_frames=5,
|
||||
)
|
||||
|
||||
|
||||
# ===========================================================================
|
||||
# Hint-schema sanity guard.
|
||||
|
||||
|
||||
class TestHintSchemaConstants:
|
||||
def test_hint_schema_version_is_v1(self) -> None:
|
||||
# Assert
|
||||
assert HINT_SCHEMA_VERSION == 1
|
||||
|
||||
def test_hint_filename_is_canonical(self) -> None:
|
||||
# Assert
|
||||
assert HINT_FILENAME == "c1_warm_start.json"
|
||||
|
||||
def test_warm_start_fc_source_is_runtime_checkable(self) -> None:
|
||||
# Arrange — local fake conforms to the runtime_checkable Protocol
|
||||
source = _FakeFcSource(hint=_make_hint())
|
||||
|
||||
# Assert
|
||||
assert isinstance(source, WarmStartFcSource)
|
||||
@@ -131,6 +131,14 @@ def _kind_payload(kind: str) -> dict[str, object]:
|
||||
"strategy_label": "okvis2",
|
||||
"frame_id": "frame-0001",
|
||||
}
|
||||
if kind == "vio.warm_start":
|
||||
return {
|
||||
"source": "f8_reboot_disk",
|
||||
"strategy_label": "klt_ransac",
|
||||
"bias_norm": 0.0345,
|
||||
"staleness_ns": 12_345_678,
|
||||
"pre_reboot_covariance_norm": 0.0625,
|
||||
}
|
||||
if kind == "c7.thermal_transition":
|
||||
return {
|
||||
"previous_state": False,
|
||||
|
||||
Reference in New Issue
Block a user