mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:11: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:
@@ -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
|
retry_count: 0
|
||||||
cycle: 1
|
cycle: 1
|
||||||
tracker: jira
|
tracker: jira
|
||||||
last_completed_batch: 55
|
last_completed_batch: 56
|
||||||
last_cumulative_review: batches_52-54
|
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.
|
default 9 per ``vio_strategy_protocol.md`` v1.0.0.
|
||||||
|
|
||||||
``warm_start_max_frames`` is the convergence budget after
|
``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
|
``okvis2`` carries OKVIS2-specific knobs (AZ-332); consulted only
|
||||||
when ``strategy == "okvis2"``.
|
when ``strategy == "okvis2"``.
|
||||||
@@ -288,6 +308,9 @@ class C1VioConfig:
|
|||||||
strategy: str = "klt_ransac"
|
strategy: str = "klt_ransac"
|
||||||
lost_frame_threshold: int = 9
|
lost_frame_threshold: int = 9
|
||||||
warm_start_max_frames: int = 5
|
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)
|
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
|
||||||
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
||||||
klt_ransac: KltRansacConfig = field(default_factory=KltRansacConfig)
|
klt_ransac: KltRansacConfig = field(default_factory=KltRansacConfig)
|
||||||
@@ -305,3 +328,19 @@ class C1VioConfig:
|
|||||||
raise ConfigError(
|
raise ConfigError(
|
||||||
f"C1VioConfig.warm_start_max_frames must be >= 1; got {self.warm_start_max_frames}"
|
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(
|
"vio.health": frozenset(
|
||||||
{"state", "consecutive_lost", "bias_norm", "strategy_label", "frame_id"}
|
{"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"}),
|
"state.tick": frozenset({"frame_id", "fused_pose", "covariance_2x2", "estimator_label"}),
|
||||||
"tile_match": frozenset({"frame_id", "tile_id", "score", "match_count", "ransac_inliers"}),
|
"tile_match": frozenset({"frame_id", "tile_id", "score", "match_count", "ransac_inliers"}),
|
||||||
"overrun": frozenset({"producer_id", "dropped_count"}),
|
"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",
|
"strategy_label": "okvis2",
|
||||||
"frame_id": "frame-0001",
|
"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":
|
if kind == "c7.thermal_transition":
|
||||||
return {
|
return {
|
||||||
"previous_state": False,
|
"previous_state": False,
|
||||||
|
|||||||
Reference in New Issue
Block a user