From 962e61ce7959042292ce1be64c23e8673865abec Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 17:32:53 +0000 Subject: [PATCH 1/9] progress: issue review for #121 --- .agent/work-plans/issue-121/progress.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .agent/work-plans/issue-121/progress.md diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md new file mode 100644 index 0000000..9f3d304 --- /dev/null +++ b/.agent/work-plans/issue-121/progress.md @@ -0,0 +1,21 @@ +--- +issue: 121 +--- + +# Issue #121 — Unified raster render abstraction + CAMP live tile cache (anti-entropy) + +## Issue Review +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Code Agent (Claude Sonnet) + +**Issue**: #121 +**Comment**: (best-effort post follows this entry; not recorded inline) +**Scope verdict**: needs-splitting + +### Actions +- [ ] Split into two PRs: Part A (RasterFieldSource interface + adapters) and Part B (live tile cache) — Part B depends on #230 (anti-entropy transport) and #44/#68 (discover/opt-in); verify those are merged before starting Part B. +- [ ] Verify that the `RasterFieldSource` interface doesn't merely rename existing `GggsTileLayer` internals — the abstraction should emerge from needing a second implementation (live cache), not be introduced speculatively ahead of it. Consider whether the interface belongs in the same PR as Part A or only in Part B's PR. +- [ ] Write a camp project ADR for Part B's persistence contract: in-memory render + write-through to disk + anti-entropy reconcile + "display-grade preview vs. data of record" distinction. This is a significant design decision for the project. +- [ ] Clarify how the live cache layer presents to the operator: does it appear in the Layers tree? How is its "live/preview" status visually distinguished? Align with ADR-0005's browse/compose split. +- [ ] For Part B acceptance: add a test or documented scenario for the simulated-downtime gap (request + prune) that can run without a live boat connection. From edf994673621e199d1a3b1e3532ec3ff5acaac34 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 17:50:29 +0000 Subject: [PATCH 2/9] Add work plan for #121 CAMP-managed live tile cache (Part B): SonarLiveCacheLayer + anti-entropy reconciler + write-through persistence + downtime-gap test --- .agent/work-plans/issue-121/plan.md | 268 ++++++++++++++++++++++++++++ 1 file changed, 268 insertions(+) create mode 100644 .agent/work-plans/issue-121/plan.md diff --git a/.agent/work-plans/issue-121/plan.md b/.agent/work-plans/issue-121/plan.md new file mode 100644 index 0000000..8fbf75e --- /dev/null +++ b/.agent/work-plans/issue-121/plan.md @@ -0,0 +1,268 @@ +# Plan: CAMP Live Tile Cache (Part B — anti-entropy, in-memory render) + +## Issue + +https://github.com/rolker/camp/issues/121 + +## Context + +**Scope (operator-decided): Part B only.** Part A (RasterFieldSource abstraction) +is deferred to camp#134. + +The transport is fully in place: `marine_interfaces` has `SonarVisualizationTile`, +`TileCatalog`, `TileCatalogEntry`, `TileIndex`, `TileRequest`, and +`VisualizationBand`; `marine_tiled_raster_store` has `TileCatalogBuilder` and +`TileCatalogReconciler` (merged via uma#230/PR#234–235); the boat-side producer +`cube_bathymetry` publishes on `~/coverage_tiles` (best_effort), `~/coverage_catalog` +(transient_local+reliable), and subscribes to `~/coverage_requests` (reliable) +(merged via cube#78). + +Current CAMP state (post #108/#122/PR#124/PR#133): +- `src/camp_map/raster/gggs_tile_layer.{h,cpp}` — multi-band, colormap LUT, + per-band NoData discard, Nearest sampling, per-layer QSettings persistence keyed + by directory. +- `src/camp_map/raster/gggs_tile.{h,cpp}` — GDAL-backed tile; `loadPixels()` reads + one band as Float32 into `data_` (off-thread safe); `texture()` uploads to GL. +- Rendering: offscreen FBO + vertex/fragment shader (GL 2.x); auto-range + colormap + LUT; NoData discard in shader. + +The gap: CAMP has no live tile subscriber or anti-entropy reconciler. The live cache +must render through the existing GL path without re-introducing the read/write race +that write-through-then-rescan would create. + +## Approach + +### Step 1 — Camp ADR-0006: Live Tile Cache Persistence Contract + +Write `docs/decisions/0006-live-tile-cache-persistence.md`. Captures the +design decisions the review-issue entry flagged as needing an ADR: +- Display-grade preview vs. data of record (lossy/quantized; losing it ≠ losing + survey data). +- In-memory render (no GDAL read on the hot path after warm-load). +- Write-through to disk: atomic temp+rename for crash safety only. +- Warm-load: on startup, read the disk cache dir using `GggsTile::loadPixels()` + (GDAL reads dequantized Float32 GeoTIFFs) before subscribing. +- Anti-entropy: `TileCatalogReconciler::reconcile()` drives request+prune on each + received `TileCatalog`. +- Cache directory: `QStandardPaths::AppDataLocation + "/live_tile_cache//"`, + configurable via QSettings (`LiveTileCache/cache_dir`). Follows #117 + no-hardcoded-defaults: default is `AppDataLocation`, operator can override. +- "Live/preview" operator label: the layer appears in the Layers tree with a + `(live)` suffix in its display name; no store-browser entry (it is ROS-sourced, + not file-browsed). Aligns with ADR-0005 browse/compose split. + +### Step 2 — `SonarLiveTile`: in-memory tile carrying dequantized Float32 data + +New `src/camp_map/raster/sonar_live_tile.{h,cpp}`. + +Holds one GGGS tile worth of per-band Float32 data received over the network +(dequantized via `value = raw * scale + offset`). Does not use GDAL. Mirrors +`GggsTile`'s interface where the render path needs it: + +```cpp +struct SonarLiveBand { + std::string name; // "depth" | "uncertainty" | "backscatter" | "intensity" + std::vector data; // row-major Float32, tile_width*tile_height cells + float nodata; + bool has_nodata; + float data_min, data_max; // auto-range over non-nodata cells +}; + +class SonarLiveTile { +public: + explicit SonarLiveTile(const marine_interfaces::msg::TileIndex& index); + void applyPatch(const marine_interfaces::msg::SonarVisualizationTile& msg); + // Geographic extent from GGGS (gggs::levelSpec / GridIndex → lat/lon bounds) + double minLon(), maxLon(), minLat(), maxLat(); + int width(), height(); + const SonarLiveBand* band(const std::string& name) const; + int bandCount() const; + // version for newest-wins / reconciler + marine_tiled_raster_store::TileVersion version() const; +private: + gggs::GridIndex index_; + int width_, height_; + std::map bands_; + TileVersion version_{0}; +}; +``` + +`applyPatch()` dequantizes the dirty sub-window from `VisualizationBand::data` +(generic: dtype + scale + offset + nodata), patches `data` at +`[window_row..][window_col..]`, updates `data_min`/`data_max` incrementally, +and bumps `version_`. + +### Step 3 — `SonarLiveCacheLayer`: render + ROS subscribe + write-through + +New `src/camp_map/raster/sonar_live_cache_layer.{h,cpp}`. + +Extends `ros::Layer` (which extends `map::Layer`). Holds the in-memory tile +map and drives anti-entropy. + +**Rendering** — duplicates the GggsTileLayer GL pipeline (shader constants, +FBO, LUT texture) for the in-memory tiles. Duplication is explicit and +temporary: a `// NOTE: shader duplicated from GggsTileLayer; will unify +// via RasterFieldSource in camp#134` comment marks the intended consolidation. +Band selection and colormap controls (via context menu, same as GggsTileLayer) +apply. Selected band/colormap persist via QSettings, keyed on the source +namespace (settingsKey = "LiveTileCache/" + source_ns_). + +**ROS subscriptions** — created with the `rclcpp::Node` from `ros::Layer`'s +parent `ros::Node`: + +| Topic | Message | QoS | Direction | +|---|---|---|---| +| `/coverage_tiles` | `SonarVisualizationTile` | best_effort 10 | subscribe | +| `/coverage_catalog` | `TileCatalog` | transient_local reliable | subscribe | +| `/coverage_requests` | `TileRequest` | reliable | publish | + +`` is the remapped source namespace (e.g. `/cube_bathymetry`), passed at +construction from `SonarLiveCacheManager`. + +**Anti-entropy** — `TileCatalogReconciler reconciler_`. On each `TileCatalog`: +1. `reconciler_.reconcile(catalog)` → `{to_request, to_prune}` +2. Publish `TileRequest` for `to_request` tiles. +3. For each `to_prune`: delete in-memory tile, delete disk file, call + `reconciler_.drop()`. + +On each `SonarVisualizationTile`: +1. Newest-wins: check tile's `header.stamp` vs `reconciler_.versionOf(index)`; + discard if stale. +2. `SonarLiveTile::applyPatch()` — patch the in-memory tile. +3. `reconciler_.markHave(index, version)`. +4. Schedule async write-through (see below). +5. Invalidate cached render, repaint. + +**Write-through** — off the GUI thread via `QtConcurrent`: +- Serialize the updated tile's selected band(s) to Float32 GeoTIFF at a temp + path (`/__.tif.tmp`), using GDAL `GDALCreate` with + the GGGS georeferencing from `gggs::levelSpec` for the tile's level. Band(s) + written as `GDT_Float32` with per-band NoData set. +- Atomic rename to `__.tif`. +- Write is crash-safe only — the layer never reads from disk during operation. + In-memory state is authoritative. + +**Warm-load** — constructor reads existing `__.tif` files from +the cache directory using the existing `GggsTile::loadPixels()` (GDAL path). +Converts loaded `GggsTile` data to `SonarLiveTile` in-memory representation. +Seeds `reconciler_` with `markHave` for each loaded tile (version 0 = "have +something; reconciler will request update on next catalog"). + +**Operator presentation**: +- `objectName()` = `"Live Coverage"` (or `"Live Coverage []"` for + multi-source). +- `settingsKey()` = `"LiveTileCache/" + source_ns_hash`. +- `onRemovedFromMap()`: drops the source_ns from `QSettings LiveTileCache/sources`. +- Does NOT appear in the Stores tab catalog browser (it is ROS-discovered, not + file-browsed — ADR-0005 §2, opt-in via topic discovery). + +### Step 4 — `SonarLiveCacheManager`: topic discovery and layer lifecycle + +New `src/camp_map/ros/sonar_live_cache_manager.{h,cpp}`. + +Extends `tools::LayerManager`. Pattern: `TopicsManager` with +`setTypeFilter({"marine_interfaces/msg/SonarVisualizationTile"})`. + +On `namesUpdated()`: for each discovered topic with active publishers, infer +`` (strip `~/coverage_tiles` suffix), check if a layer for that base +already exists (dedup), spawn `SonarLiveCacheLayer` if not. + +On re-connect (topic reappears): if the layer exists, it is already subscribed — +no new spawn; the layer's subscription reconnects via DDS discovery. + +Wired in `src/camp_map/ros/node.cpp` alongside `GridManager` and `MarkersManager`. + +Persists active source namespaces under `QSettings LiveTileCache/sources` +(a `QStringList`). `createDefaultLayers()` restores a layer per still-active source +(mirrors `GggsStoreSource::instantiate` + `GggsTileLayers/dirs` pattern). + +**Discover vs. opt-in**: camp#44/#68 (shared discovery util / opt-in catalog +integration) are not yet implemented. For this PR, auto-spawn follows the +`GridManager` pattern (discover + immediately spawn). An open question flags +whether operator opt-in is required (see Open Questions). + +### Step 5 — `CMakeLists.txt` additions + +- Add `marine_tiled_raster_store` to `find_package` and `ament_target_dependencies`. +- Add new source files to the `camp_map` library target. +- Add `test_sonar_live_cache.cpp` to the test target. + +### Step 6 — Test: downtime-gap acceptance scenario + +New `test/test_sonar_live_cache.cpp`. + +Runs headless (no Qt event loop, no ROS node, no live boat): + +1. **Warm-load test**: write synthetic Float32 GeoTIFFs to a temp dir; construct a + `SonarLiveTile` from those tiles via the GDAL warm-load path; verify tile count + and data range. +2. **Patch-apply test**: call `SonarLiveTile::applyPatch()` with a synthetic + `SonarVisualizationTile` (sub-window patch); verify the patched region is + dequantized correctly and `data_min`/`data_max` updated. +3. **Downtime-gap reconcile test** (the acceptance scenario from the issue): + - Seed `TileCatalogReconciler` with tiles A, B, C (simulates "before downtime"). + - Receive a `TileCatalog` with tiles A, B, D (C replaced by D — boat was + down, some coverage changed). + - Call `reconcile()` → assert `to_request = {D}` (missing), `to_prune = {C}` + (absent and version < generation_time). + - Call `markHave(D, ...)`, `drop(C)` — model the consumer acting. + - Receive a second catalog with A, B, D — assert `to_request = {}`, + `to_prune = {}` (converged). +4. **Prune timestamp-gate test**: ensure a tile with version > catalog + `generation_time` is NOT pruned even if absent (ADR-0008 D4b). + +## Files to Change + +| File | Change | +|------|--------| +| `docs/decisions/0006-live-tile-cache-persistence.md` | New ADR | +| `src/camp_map/raster/sonar_live_tile.h` | New in-memory tile type | +| `src/camp_map/raster/sonar_live_tile.cpp` | Patch-apply, dequantize, auto-range | +| `src/camp_map/raster/sonar_live_cache_layer.h` | New ROS layer | +| `src/camp_map/raster/sonar_live_cache_layer.cpp` | GL render, subscribe, reconcile, write-through, warm-load | +| `src/camp_map/ros/sonar_live_cache_manager.h` | New topic-discovery manager | +| `src/camp_map/ros/sonar_live_cache_manager.cpp` | TopicsManager + layer lifecycle | +| `src/camp_map/ros/node.cpp` | Wire SonarLiveCacheManager (alongside GridManager) | +| `CMakeLists.txt` | Add `marine_tiled_raster_store`, new sources, new test | +| `test/test_sonar_live_cache.cpp` | Downtime-gap + reconcile tests | + +## Principles Self-Check + +| Principle | Consideration | +|---|---| +| Only what's needed | GL shader code is duplicated from GggsTileLayer rather than extracting a shared renderer; that extraction belongs to Part A / camp#134. | +| Capture decisions, not just implementations | ADR-0006 records the persistence contract and the preview-vs-data-of-record distinction. | +| A change includes its consequences | CMakeLists.txt gains `marine_tiled_raster_store`; node.cpp wires the manager; test covers the downtime-gap scenario. | +| Test what breaks | Tests target the reconciler logic (request + prune, timestamp gate) and dequantize path — the failure modes hardest to catch in the field. | +| Improve incrementally | Part B only; Part A's abstraction deferred; single PR. | +| Human control and transparency | Operator sees a `(live)` suffix on the layer; write-through and warm-load are transparent to the user. | + +## ADR Compliance + +| ADR | Triggered | How addressed | +|---|---|---| +| camp ADR-0005 (browse/compose split) | Yes | Live cache layer appears in Layers tree (compose side), NOT in the Stores browser; spawned via ROS topic discovery, not file browsing. | +| camp ADR-0001 (TopicBridge executor contract) | Yes | Subscription callbacks are lightweight (data copy + signal emit); GL render is always on the GUI thread. | +| camp ADR-0002 (Web-Mercator scene + layer model) | Yes | Layer renders into the Web-Mercator scene via the same FBO+shader path as GggsTileLayer. | +| uma ADR-0008 (live sonar transport + render) | Yes | Subscribes to the D1/D3/D4 messages; applies newest-wins patch ordering; prune-on-absence with timestamp gate. | +| workspace ADR-0013 (progress.md vocabulary) | Yes | This plan is committed; progress.md updated. | + +## Consequences + +| If we change... | Also update... | Included in plan? | +|---|---|---| +| Add `marine_tiled_raster_store` dep | `CMakeLists.txt` + `package.xml` | Yes | +| New layer type in Layers tree | `item_types.h` (add `SonarLiveCacheLayerType`) | Yes (in `sonar_live_cache_layer.h`) | +| `node.cpp` wires manager | `node.h` forward-declares the manager | Yes | +| GggsTile shader duplicated | Comment pointing to camp#134 consolidation | Yes | + +## Open Questions + +- [ ] **Discover vs. opt-in**: camp#44/#68 (shared topic-discovery + opt-in catalog) are not implemented. Should the live cache auto-spawn when the topic is detected (GridManager pattern), or require explicit operator opt-in (catalog browser entry)? Auto-spawn is proposed; confirm with operator before implementation. +- [ ] **Multi-band write-through**: The write-through step must decide which bands to persist (depth only? all received bands?). Writing all bands is safest for warm-load fidelity; writing only depth keeps the cache small. Propose: write all received bands (one Float32 band per VisualizationBand), so warm-load can reconstruct any operator-selected band. +- [ ] **Cache size limits**: `TileCatalogReconciler` has no built-in eviction; prune-on-absence handles normal shrinkage. A large survey could accumulate thousands of tiles. An eviction-by-area policy is possible follow-up. Flag if the operator expects this to be bounded. + +## Estimated Scope + +Single PR. ~600–900 lines net (new files: ADR, sonar_live_tile, sonar_live_cache_layer, +sonar_live_cache_manager, test; changes: node.cpp, CMakeLists.txt, item_types.h). From 076b6a81384bf28f52b317ad7295dd1eb43abfd1 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 17:50:42 +0000 Subject: [PATCH 3/9] progress: plan authored for #121 --- .agent/work-plans/issue-121/progress.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index 9f3d304..289a0a4 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -19,3 +19,17 @@ issue: 121 - [ ] Write a camp project ADR for Part B's persistence contract: in-memory render + write-through to disk + anti-entropy reconcile + "display-grade preview vs. data of record" distinction. This is a significant design decision for the project. - [ ] Clarify how the live cache layer presents to the operator: does it appear in the Layers tree? How is its "live/preview" status visually distinguished? Align with ADR-0005's browse/compose split. - [ ] For Part B acceptance: add a test or documented scenario for the simulated-downtime gap (request + prune) that can run without a live boat connection. + +## Plan Authored +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Code Agent (Claude Sonnet) + +**Plan**: `.agent/work-plans/issue-121/plan.md` at `edf9946` +**Branch**: feature/issue-121 at `edf9946` +**Phases**: single + +### Open questions +- [ ] Discover vs. opt-in: camp#44/#68 not yet implemented; auto-spawn (GridManager pattern) proposed — confirm with operator before implementing. +- [ ] Multi-band write-through: which bands to persist (all received vs. depth-only)? Propose: all received bands. +- [ ] Cache size limits: no built-in eviction beyond prune-on-absence; flag if operator expects a bounded cache. From c3f8029f684f7d95ab38e6b423d639b065318f04 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 17:56:31 +0000 Subject: [PATCH 4/9] progress: plan review for #121 --- .agent/work-plans/issue-121/progress.md | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index 289a0a4..73be097 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -33,3 +33,27 @@ issue: 121 - [ ] Discover vs. opt-in: camp#44/#68 not yet implemented; auto-spawn (GridManager pattern) proposed — confirm with operator before implementing. - [ ] Multi-band write-through: which bands to persist (all received vs. depth-only)? Propose: all received bands. - [ ] Cache size limits: no built-in eviction beyond prune-on-absence; flag if operator expects a bounded cache. + +## Plan Review +**Status**: complete +**When**: 2026-06-28 17:55 +00:00 +**By**: Claude Code Agent (Claude Opus) + +**Plan**: `.agent/work-plans/issue-121/plan.md` at `edf9946` +**PR**: PR-less (--issue mode) +**Verdict**: approve-with-suggestions + +The plan is well-grounded in the actual codebase (reconciler API, `ros::Layer`/`Node`, +`GggsTile::loadPixels`, manager wiring, QoS choices all verified) and addresses every +review-issue finding. The one must-fix is a completeness gap: the Files-to-Change table +omits two files (`package.xml`, `item_types.h`) that the plan's own Consequences table +says must change. Suggestions concern threading clarity, the discover-vs-opt-in blocker, +and minor convention/version-semantics points. + +### Findings +- [ ] (must-fix) Files-to-Change table omits `package.xml` (needs `marine_tiled_raster_store` — confirmed absent) and `item_types.h` (needs a new `SonarLiveCacheLayerType` in its `enum ItemType` — the Consequences row says "in sonar_live_cache_layer.h" but the enum lives in `src/camp_map/map/item_types.h`). Reconcile the two tables. — `plan.md:214-227, 252-257` +- [ ] (suggestion) Consequences row "node.h forward-declares the manager" is unnecessary: existing `MarkersManager`/`GridManager` are created in `node.cpp` (`new XManager(this)`) and are not forward-declared in `node.h`. Drop the row or confirm it's actually needed. — `plan.md:256` +- [ ] (suggestion) Resolve Open Question #1 (auto-spawn vs. operator opt-in) with the operator *before* implementation — it determines `SonarLiveCacheManager`'s spawn behavior, a structural decision, not a detail to defer. Also note this bypasses ADR-0005 §2's generic `CatalogSource` seam (which explicitly anticipated topic-discovery consumers like camp#44/#68); the deferral is defensible but should be stated as a conscious ADR-0005 tension in ADR-0006. — `plan.md:182, 261` +- [ ] (suggestion) `TileCatalogReconciler` is explicitly **not thread-safe** (tile_catalog.hpp:46). The plan should state that `reconcile()`/`markHave()`/`drop()`/`applyPatch()` all run on a single thread (warm-load constructor + ROS callbacks marshalled to the GUI thread via queued signal per ADR-0001), so the ROS callback must only copy+emit, never touch `reconciler_` directly. — `plan.md:122-135, 245` +- [ ] (suggestion) Specify version semantics consistency: newest-wins compares `header.stamp` against `versionOf()` (`TileVersion` = int64), and prune is gated on catalog `generation_time` (also int64). State the single conversion (e.g. stamp→nanoseconds) used for `markHave`, the stale check, and the timestamp gate so they're comparable. — `plan.md:128-130, 211-213` +- [ ] (suggestion) New manager file placement: existing managers live in `ros/` subdirectories (`ros/grids/`, `ros/markers/`, `ros/geometry/`); the plan puts `sonar_live_cache_manager.{h,cpp}` directly in `ros/`. Consider a subdir for consistency (cosmetic). — `plan.md:161, 223-224` From 470d2c99f999e2c97c605a33cc87545f62107f2a Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 18:29:30 +0000 Subject: [PATCH 5/9] camp#121: CAMP live tile cache (Part B, opt-in subscribe) Add the live sonar-tile cache consumer: discover the SonarVisualizationTile topic, render dequantized in-memory Float32 tiles through a GL path duplicated from GggsTileLayer (no GDAL on the hot path), and drive anti-entropy convergence with marine_tiled_raster_store::TileCatalogReconciler. Part A (RasterFieldSource) stays deferred to camp#134. Activation is opt-in (ADR-0006 D5, serves #71): discovery auto-spawns a discovered-but-inactive layer that may subscribe to the cheap transient-local coverage_catalog, but subscribes to the best-effort coverage_tiles stream and publishes TileRequest only when the operator enables it. The per-source enabled flag persists via QSettings, so an enabled source re-subscribes on warm restart while a never-enabled one stays passive. Persistence (ADR-0006): in-memory state is authoritative; write-through is atomic temp+rename, crash-safe only, all received bands (band name = GDAL band description); warm-load reads the cache dir before subscribing and seeds the reconciler at version 0 to close the downtime gap. One int64 version unit (ns since epoch) across newest-wins, markHave, and the prune timestamp-gate. Threading (ADR-0001): ROS callbacks only copy the message + marshal to the GUI thread (queued invokeMethod); the GUI-thread slot does all reconcile/markHave/ drop/applyPatch/write-through. The reconciler is never touched off-thread. New TUs live in ros/live_coverage/ (not raster/) so camp_map stays ROS-free; they compile into camp_map_ros, which gains marine_tiled_raster_store + GDAL. Files: - docs/decisions/0006-live-tile-cache-persistence.md (new ADR) - ros/live_coverage/sonar_live_tile.{h,cpp} (in-memory tile, dequantize, warm-load/write-through, wire<->gggs conversions) - ros/live_coverage/sonar_live_cache_layer.{h,cpp} (render + subscribe + reconcile + opt-in toggle) - ros/live_coverage/sonar_live_cache_manager.{h,cpp} (topic discovery) - map/item_types.h: + SonarLiveCacheLayerType - ros/node.cpp: wire the manager - CMakeLists.txt / package.xml: + marine_tiled_raster_store, GDAL, new test - test/test_sonar_live_cache.cpp: warm-load, patch/dequantize, downtime-gap reconcile, prune timestamp-gate (headless; no Qt loop / ROS node / boat) Tests: 129 tests, 0 failures, 4 skipped (offscreen-GL renders skip in-container). Co-Authored-By: Claude Opus 4.8 --- .agent/work-plans/issue-121/plan.md | 69 +- .agent/work-plans/issue-121/progress.md | 47 + CMakeLists.txt | 33 +- .../0006-live-tile-cache-persistence.md | 174 ++++ package.xml | 1 + src/camp_map/map/item_types.h | 1 + .../live_coverage/sonar_live_cache_layer.cpp | 819 ++++++++++++++++++ .../live_coverage/sonar_live_cache_layer.h | 195 +++++ .../sonar_live_cache_manager.cpp | 76 ++ .../live_coverage/sonar_live_cache_manager.h | 45 + .../ros/live_coverage/sonar_live_tile.cpp | 416 +++++++++ .../ros/live_coverage/sonar_live_tile.h | 152 ++++ src/camp_map/ros/node.cpp | 2 + test/test_sonar_live_cache.cpp | 263 ++++++ 14 files changed, 2270 insertions(+), 23 deletions(-) create mode 100644 docs/decisions/0006-live-tile-cache-persistence.md create mode 100644 src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp create mode 100644 src/camp_map/ros/live_coverage/sonar_live_cache_layer.h create mode 100644 src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp create mode 100644 src/camp_map/ros/live_coverage/sonar_live_cache_manager.h create mode 100644 src/camp_map/ros/live_coverage/sonar_live_tile.cpp create mode 100644 src/camp_map/ros/live_coverage/sonar_live_tile.h create mode 100644 test/test_sonar_live_cache.cpp diff --git a/.agent/work-plans/issue-121/plan.md b/.agent/work-plans/issue-121/plan.md index 8fbf75e..6cb8b9a 100644 --- a/.agent/work-plans/issue-121/plan.md +++ b/.agent/work-plans/issue-121/plan.md @@ -176,10 +176,17 @@ Persists active source namespaces under `QSettings LiveTileCache/sources` (a `QStringList`). `createDefaultLayers()` restores a layer per still-active source (mirrors `GggsStoreSource::instantiate` + `GggsTileLayers/dirs` pattern). -**Discover vs. opt-in**: camp#44/#68 (shared discovery util / opt-in catalog -integration) are not yet implemented. For this PR, auto-spawn follows the -`GridManager` pattern (discover + immediately spawn). An open question flags -whether operator opt-in is required (see Open Questions). +**Discover vs. opt-in (operator-decided — opt-in tile stream).** Discovery is +automatic (the `GridManager` pattern), but the *tile-stream subscription* is +opt-in. `SonarLiveCacheManager` auto-detects the topic and spawns a layer in a +**discovered-but-inactive** state; the layer MAY subscribe to the cheap +`coverage_catalog` (transient-local) to advertise availability, but it does NOT +subscribe to the best-effort `coverage_tiles` and does NOT publish `TileRequest` +until the operator enables it (context-menu "Enable live coverage"). The per-source +enabled flag persists via QSettings, so an enabled source re-subscribes on warm +restart while a never-enabled one stays passive. This serves #71 (no surprise +bandwidth on a slow link) and is recorded as a conscious ADR-0005 tension in +ADR-0006 (D5), pending the generic #44/#68 `CatalogSource` seam. ### Step 5 — `CMakeLists.txt` additions @@ -213,18 +220,26 @@ Runs headless (no Qt event loop, no ROS node, no live boat): ## Files to Change +All three new translation units live under `ros/live_coverage/` (not `raster/`): +they depend on `marine_interfaces` + `marine_tiled_raster_store`, so they compile +into the ROS-dependent `camp_map_ros` library — `camp_map` stays ROS-free, the +layering invariant ADR-0002 / the CMake comment relies on. (Refines the original +`raster/` placement.) + | File | Change | |------|--------| -| `docs/decisions/0006-live-tile-cache-persistence.md` | New ADR | -| `src/camp_map/raster/sonar_live_tile.h` | New in-memory tile type | -| `src/camp_map/raster/sonar_live_tile.cpp` | Patch-apply, dequantize, auto-range | -| `src/camp_map/raster/sonar_live_cache_layer.h` | New ROS layer | -| `src/camp_map/raster/sonar_live_cache_layer.cpp` | GL render, subscribe, reconcile, write-through, warm-load | -| `src/camp_map/ros/sonar_live_cache_manager.h` | New topic-discovery manager | -| `src/camp_map/ros/sonar_live_cache_manager.cpp` | TopicsManager + layer lifecycle | +| `docs/decisions/0006-live-tile-cache-persistence.md` | New ADR (opt-in activation + persistence contract) | +| `src/camp_map/ros/live_coverage/sonar_live_tile.h` | New in-memory tile type + node-boundary conversions | +| `src/camp_map/ros/live_coverage/sonar_live_tile.cpp` | Patch-apply/dequantize, auto-range, warm-load, write-through, wire↔gggs conversions | +| `src/camp_map/ros/live_coverage/sonar_live_cache_layer.h` | New ROS layer (opt-in toggle) | +| `src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp` | GL render (duplicated), subscribe, reconcile, write-through, warm-load | +| `src/camp_map/ros/live_coverage/sonar_live_cache_manager.h` | New topic-discovery manager | +| `src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp` | TopicsManager + layer lifecycle (discover-only spawn) | +| `src/camp_map/map/item_types.h` | Add `SonarLiveCacheLayerType` to `enum ItemType` (must-fix) | | `src/camp_map/ros/node.cpp` | Wire SonarLiveCacheManager (alongside GridManager) | -| `CMakeLists.txt` | Add `marine_tiled_raster_store`, new sources, new test | -| `test/test_sonar_live_cache.cpp` | Downtime-gap + reconcile tests | +| `CMakeLists.txt` | Add `marine_tiled_raster_store` (find_package + camp_map_ros dep) + GDAL to camp_map_ros, new sources, new test | +| `package.xml` | Add `marine_tiled_raster_store` (must-fix) | +| `test/test_sonar_live_cache.cpp` | Warm-load, patch-apply, downtime-gap reconcile + prune-gate tests | ## Principles Self-Check @@ -251,16 +266,26 @@ Runs headless (no Qt event loop, no ROS node, no live boat): | If we change... | Also update... | Included in plan? | |---|---|---| -| Add `marine_tiled_raster_store` dep | `CMakeLists.txt` + `package.xml` | Yes | -| New layer type in Layers tree | `item_types.h` (add `SonarLiveCacheLayerType`) | Yes (in `sonar_live_cache_layer.h`) | -| `node.cpp` wires manager | `node.h` forward-declares the manager | Yes | +| Add `marine_tiled_raster_store` dep | `CMakeLists.txt` (find_package + camp_map_ros) + `package.xml` | Yes (both) | +| New layer type in Layers tree | `map/item_types.h` (add `SonarLiveCacheLayerType` to `enum ItemType`) | Yes | +| `node.cpp` wires manager | (none — managers are `new XManager(this)` in node.cpp, not forward-declared in node.h, matching Markers/Grid) | n/a (row dropped per review) | | GggsTile shader duplicated | Comment pointing to camp#134 consolidation | Yes | - -## Open Questions - -- [ ] **Discover vs. opt-in**: camp#44/#68 (shared topic-discovery + opt-in catalog) are not implemented. Should the live cache auto-spawn when the topic is detected (GridManager pattern), or require explicit operator opt-in (catalog browser entry)? Auto-spawn is proposed; confirm with operator before implementation. -- [ ] **Multi-band write-through**: The write-through step must decide which bands to persist (depth only? all received bands?). Writing all bands is safest for warm-load fidelity; writing only depth keeps the cache small. Propose: write all received bands (one Float32 band per VisualizationBand), so warm-load can reconstruct any operator-selected band. -- [ ] **Cache size limits**: `TileCatalogReconciler` has no built-in eviction; prune-on-absence handles normal shrinkage. A large survey could accumulate thousands of tiles. An eviction-by-area policy is possible follow-up. Flag if the operator expects this to be bounded. +| Live-cache .cpp uses GDAL directly | camp_map_ros links `${GDAL_LIBRARY}` (camp_map links it PRIVATE) | Yes | + +## Open Questions (resolved by operator before implementation) + +- [x] **Discover vs. opt-in** → **Discover automatically, subscribe to the tile + stream only on opt-in.** Discovery auto-spawns a discovered-but-inactive layer + (catalog sub allowed); `coverage_tiles` + `TileRequest` are gated behind a + context-menu "Enable live coverage" toggle, persisted per source. Serves #71. + Recorded as an ADR-0005 tension in ADR-0006 D5. (Implemented.) +- [x] **Multi-band write-through** → **Write all received bands** (one Float32 + GeoTIFF band per VisualizationBand, band name carried as the GDAL band + description) so warm-load can reconstruct any operator-selected band. + (Implemented.) +- [x] **Cache size limits** → **No eviction beyond prune-on-absence** for this PR. + Eviction-by-area is a deferred follow-up (`// TODO(camp): eviction-by-area`), + not implemented. (Confirmed.) ## Estimated Scope diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index 73be097..552c547 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -57,3 +57,50 @@ and minor convention/version-semantics points. - [ ] (suggestion) `TileCatalogReconciler` is explicitly **not thread-safe** (tile_catalog.hpp:46). The plan should state that `reconcile()`/`markHave()`/`drop()`/`applyPatch()` all run on a single thread (warm-load constructor + ROS callbacks marshalled to the GUI thread via queued signal per ADR-0001), so the ROS callback must only copy+emit, never touch `reconciler_` directly. — `plan.md:122-135, 245` - [ ] (suggestion) Specify version semantics consistency: newest-wins compares `header.stamp` against `versionOf()` (`TileVersion` = int64), and prune is gated on catalog `generation_time` (also int64). State the single conversion (e.g. stamp→nanoseconds) used for `markHave`, the stale check, and the timestamp gate so they're comparable. — `plan.md:128-130, 211-213` - [ ] (suggestion) New manager file placement: existing managers live in `ros/` subdirectories (`ros/grids/`, `ros/markers/`, `ros/geometry/`); the plan puts `sonar_live_cache_manager.{h,cpp}` directly in `ros/`. Consider a subdir for consistency (cosmetic). — `plan.md:161, 223-224` + +## Implementation +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Opus + +**Issue**: #121 (Part B — CAMP live tile cache; Part A / RasterFieldSource deferred to camp#134) +**Branch**: feature/issue-121 +**Scope**: Part B only, with the operator-decided **opt-in tile-stream** activation model. + +### Files added +- `docs/decisions/0006-live-tile-cache-persistence.md` — ADR: in-memory render + crash-safe write-through + warm-load + anti-entropy, and the **discovery-automatic / tile-stream-opt-in** decision (D5) stated as a conscious ADR-0005 tension. +- `src/camp_map/ros/live_coverage/sonar_live_tile.{h,cpp}` — in-memory dequantized Float32 tile (`applyPatch` generic dequantize `value = raw*scale+offset`, GGGS-cell→north-up flip, incremental auto-range), GDAL warm-load (`loadFromGeoTiff`/`loadCacheDir`, all bands + names + GridIndex-from-geotransform) and write-through (`writeToGeoTiff`), plus the node-boundary conversions (`toNanoseconds`, `gridIndexFromTileIndex`/`tileIndexFromGridIndex`, `toReconcilerCatalog`). +- `src/camp_map/ros/live_coverage/sonar_live_cache_layer.{h,cpp}` — `ros::Layer` subclass: GL render **duplicated from GggsTileLayer** (marked `// NOTE: shader duplicated; unify via RasterFieldSource camp#134`), reconcile/request/prune, write-through (QtConcurrent, atomic temp+rename), warm-load, opt-in **Enable/Disable live coverage** toggle, per-source QSettings (colormap/band/enabled). +- `src/camp_map/ros/live_coverage/sonar_live_cache_manager.{h,cpp}` — `TopicsManager`-filtered discovery (`SonarVisualizationTile`), spawns one inactive layer per source base namespace (deduped). +- `test/test_sonar_live_cache.cpp` — headless (no Qt loop / no ROS node / no boat): warm-load round-trip, patch-apply/dequantize, downtime-gap reconcile (seed A,B,C → catalog A,B,D → request={D}, prune={C} → converge), prune timestamp-gate. + +### Files changed +- `src/camp_map/map/item_types.h` — **(must-fix)** added `SonarLiveCacheLayerType` to `enum ItemType`. +- `package.xml` — **(must-fix)** added `marine_tiled_raster_store`. +- `CMakeLists.txt` — `find_package(marine_tiled_raster_store)`; new sources in `CAMP_MAP_ROS_SOURCES`; `marine_tiled_raster_store` + `${GDAL_LIBRARY}` on `camp_map_ros`; new `test_sonar_live_cache` target. +- `src/camp_map/ros/node.cpp` — wired `SonarLiveCacheManager` alongside `GridManager`. +- `.agent/work-plans/issue-121/plan.md` — synced to the opt-in model, must-fix table additions, and `ros/live_coverage/` placement; Open Questions resolved. + +### Operator decision applied (overrides plan auto-spawn) +**Discover automatically; subscribe to `coverage_tiles` + publish `TileRequest` only on opt-in.** Discovery spawns a *discovered-but-inactive* `Live Coverage []` layer (status `(available)`); the inactive layer subscribes only to the cheap transient-local `coverage_catalog`. "Enable live coverage" warm-loads the disk cache, subscribes to the best-effort tile stream, starts request/prune, and persists `live_enabled` per source (settingsKey on source ns) → an enabled source re-subscribes on warm restart; a never-enabled one stays passive. Serves #71. + +### Must-fix + suggestions addressed +- must-fix: `package.xml` + `map/item_types.h` reconciled with the plan's Consequences. ✔ +- Thread-safety invariant (ADR-0001/ADR-0006 D4): ROS callbacks only copy the message + `QMetaObject::invokeMethod(..., Qt::QueuedConnection)` to the GUI thread; the GUI-thread slot does all reconcile/markHave/drop/applyPatch/write-through; warm-load runs GUI-thread pre-subscribe. Documented in the layer header. ✔ +- Version semantics: one int64 = **nanoseconds since epoch** for markHave, the newest-wins stale check, and the prune timestamp-gate (single `toNanoseconds`). ✔ +- Dropped the unnecessary "node.h forward-declares the manager" step (managers are `new XManager(this)`). ✔ +- Manager (and, for ROS-free-core integrity, the tile + layer) placed under `ros/live_coverage/`. ✔ + +### Secondary OQ defaults applied +- Write-through **all received bands** (band name = GDAL band description). ✔ +- **No eviction** beyond prune-on-absence; `// TODO(camp): eviction-by-area follow-up` note left in the layer. ✔ + +### Build + test (verbatim) +- Lower layers built first (system-available deps from `/opt/ros/jazzy`): `colcon build --packages-up-to marine_tiled_raster_store marine_ais_msgs marine_nav_interfaces marine_nav_tasks` → all finished (only pre-existing `marine_autonomy` warnings). +- `./ui_ws/build.sh camp` → `Summary: 1 package finished [1min 59s]` (clean; only pre-existing camp warnings). +- `./ui_ws/test.sh camp` → `Summary: 129 tests, 0 errors, 0 failures, 4 skipped` (the 4 skips are the offscreen-GL render tests that skip in-container by design). +- `test_sonar_live_cache` → `[ PASSED ] 4 tests` (PatchApplyDequantize, WarmLoadRoundTrip, DowntimeGapReconcile, PruneTimestampGate). + +### Notes / deviations +- New TUs live in `ros/live_coverage/` (not `raster/` as the plan first listed): they depend on `marine_interfaces` + `marine_tiled_raster_store`, so they belong in `camp_map_ros`; `camp_map` stays ROS-free (ADR-0002 / CMake layering). Recorded in ADR-0006 + plan. +- Warm-load reads the multi-band GeoTIFF directly via GDAL (recovering band names from descriptions + GridIndex from the geotransform, the `marine_tiled_raster_store::loadTile` convention) rather than literally calling the single-band `GggsTile::loadPixels()`, since write-through-all-bands needs all bands + names in one pass. diff --git a/CMakeLists.txt b/CMakeLists.txt index ff9642b..b5b6941 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(nav2_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(marine_autonomy REQUIRED) find_package(marine_interfaces REQUIRED) +find_package(marine_tiled_raster_store REQUIRED) find_package(marine_nav_interfaces REQUIRED) find_package(marine_nav_tasks REQUIRED) @@ -313,6 +314,9 @@ set(CAMP_MAP_ROS_SOURCES src/camp_map/ros/grids/grid_map.cpp src/camp_map/ros/grids/occupancy_grid.cpp src/camp_map/ros/layer.cpp + src/camp_map/ros/live_coverage/sonar_live_tile.cpp + src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp + src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp src/camp_map/ros/markers/marker.cpp src/camp_map/ros/markers/marker_namespace.cpp src/camp_map/ros/markers/markers.cpp @@ -330,7 +334,10 @@ add_library(camp_map_ros SHARED ${CAMP_MAP_ROS_SOURCES}) # plain target_link_libraries form, and CMake forbids mixing the two on one # target. The plain form still links camp_map transitively, so camp_map's # PUBLIC Qt + include-dir usage requirements propagate downstream. -target_link_libraries(camp_map_ros camp_map) +# [camp#121] GDAL is linked directly here (not just transitively via camp_map, +# which links it PRIVATE): the live tile cache's write-through / warm-load +# (sonar_live_tile.cpp) calls GDAL itself. +target_link_libraries(camp_map_ros camp_map ${GDAL_LIBRARY}) ament_target_dependencies(camp_map_ros geometry_msgs @@ -338,6 +345,7 @@ ament_target_dependencies(camp_map_ros marine_ais_msgs marine_autonomy marine_interfaces + marine_tiled_raster_store rclcpp tf2 tf2_ros @@ -703,6 +711,29 @@ if(BUILD_TESTING) Qt5::Positioning ${GDAL_LIBRARY} ) + + # [camp#121] Live tile cache (Part B): warm-load + patch-apply/dequantize + # (SonarLiveTile, GDAL) and the downtime-gap reconcile + prune timestamp-gate + # (the node-boundary catalog conversion + TileCatalogReconciler). Headless: no + # Qt event loop, no ROS node, no boat — links camp_map_ros for SonarLiveTile + + # the conversions, and GDAL for the synthetic cached tiles. + ament_add_gtest(test_sonar_live_cache + test/test_sonar_live_cache.cpp + ) + target_include_directories(test_sonar_live_cache PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/camp_map + ) + ament_target_dependencies(test_sonar_live_cache + marine_autonomy + marine_interfaces + marine_tiled_raster_store + rclcpp + ) + target_link_libraries(test_sonar_live_cache + camp_map_ros + Qt5::Core + ${GDAL_LIBRARY} + ) endif() diff --git a/docs/decisions/0006-live-tile-cache-persistence.md b/docs/decisions/0006-live-tile-cache-persistence.md new file mode 100644 index 0000000..fe77f2e --- /dev/null +++ b/docs/decisions/0006-live-tile-cache-persistence.md @@ -0,0 +1,174 @@ +# ADR-0006: Live tile cache — persistence contract + opt-in tile-stream subscription + +## Status + +Accepted + +Implements CAMP issue #121 Part B (the live tile cache). Part A (the generic +`RasterFieldSource` render abstraction) is deferred to camp#134. Builds on +[ADR-0001](0001-topicbridge-and-executor-contract.md) (callback/executor +threading), [ADR-0002](0002-web-mercator-scene-and-layer-model.md) (the +Web-Mercator scene + layer model) and [ADR-0005](0005-stores-browser-and-flat-display-layers.md) +(browse/compose split). Consumes the boat-side anti-entropy transport +(uma ADR-0008: `SonarVisualizationTile` / `TileCatalog` / `TileRequest`) and the +payload-agnostic reconciler in `marine_tiled_raster_store`. + +## Context + +The boat publishes a live, GGGS-tiled sonar **display product**: per-tile, +per-band quantized rasters (`SonarVisualizationTile`, best-effort), a complete +periodic `TileCatalog` (transient-local + reliable), and accepts a `TileRequest` +"need" list (reliable). CAMP had no consumer: no live subscriber, no anti-entropy +reconciler, and no place to render incoming tiles. The gap is to land those tiles +in the existing GGGS GL render path **without** re-introducing the read/write race +a write-through-then-rescan-from-disk design would create. + +Three forces shape the contract: + +1. **Display-grade preview, not data of record.** These tiles are a lossy, + quantized projection of the durable boat-side stores (bathy/backscatter). They + carry no provenance and are always rebuildable from the stores. Losing the + cache is *not* losing survey data — which is what licenses an in-memory, + crash-safe-only persistence model rather than a durable store. + +2. **Bandwidth (#71, slow cell links).** The large payload is `coverage_tiles` + (best-effort, the full tile stream). The catalog is small and cheap. An + operator on a constrained link must not pay for a tile stream they did not ask + for, just because CAMP happened to see the topic appear. + +3. **Single-thread reconciler (`TileCatalogReconciler` is not thread-safe).** All + reconcile/markHave/drop/applyPatch mutation must happen on one thread. + +## Decision + +### D1 — In-memory render; disk is a crash-safe cache only + +Received tiles are dequantized to in-memory Float32 (`SonarLiveTile`, one +`SonarLiveBand` per `VisualizationBand`) and rendered straight from memory through +a GL pipeline duplicated from `GggsTileLayer` — **no GDAL read on the hot path**. +The in-memory map is authoritative; the layer never reads back from disk during +operation. Write-through to disk exists only so a warm restart can repopulate +without waiting for a full re-push. + +### D2 — Write-through: atomic temp+rename, all received bands + +On each applied patch the affected tile is serialized off the GUI thread to a +Float32 GeoTIFF under +`//__.tif`, written to a `.tif.tmp` +sibling and `rename()`d into place (atomic on POSIX) so a crash mid-write can +never leave a half-tile a later warm-load would choke on. The georeferencing is +the GGGS north-up geotransform for the tile's level (the same convention +`marine_tiled_raster_store::saveTile` and `GggsTile` use), so a cached tile is a +valid GGGS GeoTIFF. **All received bands are written** (one Float32 GeoTIFF band +per band, band name carried as the GDAL band description), so a warm-load can +reconstruct whichever band the operator later selects — not just depth. (Operator +secondary-question default: write-through all bands.) + +`cache_dir` defaults to `QStandardPaths::AppDataLocation + "/live_tile_cache"` and +is overridable via `QSettings LiveTileCache/cache_dir` (follows #117: a default, +never a hardcode the operator can't change). + +### D3 — Warm-load before subscribing + +When a source is (re)activated the layer reads any existing +`__.tif` tiles from its cache directory (GDAL, dequantized +Float32, all bands + band names recovered from band descriptions; GridIndex +recovered from the geotransform exactly as `marine_tiled_raster_store::loadTile` +does) into the in-memory map, and seeds the reconciler with `markHave(index, 0)` +for each. Version 0 means "I have *something* here" — strictly older than any real +catalog version, so the next catalog re-requests the tile if the boat has a newer +version, and the prune gate never deletes it spuriously (a held version of 0 is +older than any non-zero `generation_time`). This is what closes the +**downtime gap**: a CAMP that was off while coverage changed converges on +reconnect — requesting tiles it is missing/stale on, pruning tiles the boat no +longer holds. + +### D4 — Anti-entropy via the reconciler, single-threaded + +`TileCatalogReconciler` drives convergence. On each `TileCatalog`: +`reconcile()` → `{to_request, to_prune}`; publish a `TileRequest` for +`to_request`; for each `to_prune` delete the in-memory tile + its disk file + +`drop()`. On each `SonarVisualizationTile`: newest-wins (discard if the tile's +version is not newer than the held version), `applyPatch()`, `markHave()`, +schedule write-through, repaint. + +**Threading invariant (ADR-0001).** The reconciler and tile map are **not** +thread-safe and are touched only on the GUI thread. ROS subscription callbacks do +the minimum — copy the message and emit a **queued** Qt signal — and never touch +`reconciler_` or the tiles directly. The GUI-thread slot does all reconcile / +markHave / drop / applyPatch / write-through-scheduling. Warm-load runs on the GUI +thread (at activation) before the subscriptions are created, so it cannot race a +callback. + +**Version semantics — one int64.** Every version is **nanoseconds since epoch** +(`int64`, `marine_tiled_raster_store::TileVersion`). A tile's version is its +`header.stamp` flattened to ns; a catalog entry's version is its +`builtin_interfaces/Time` flattened to ns; the prune gate compares held versions +against the catalog `header.stamp` flattened to ns. The newest-wins check, the +`markHave` value, and the prune timestamp-gate are therefore all the same unit and +directly comparable. The single monotonic source clock is the boat (uma ADR-0008). + +### D5 — Discovery is automatic; tile-stream subscription is **opt-in** + +`SonarLiveCacheManager` auto-detects the `SonarVisualizationTile` topic (the +`GridManager` discovery pattern) and spawns a `SonarLiveCacheLayer` in a +**discovered-but-inactive** state. The inactive layer MAY subscribe to the small +`coverage_catalog` (transient-local, cheap) to populate availability, but it does +**NOT** subscribe to `coverage_tiles` and does **NOT** publish `TileRequest` until +the operator enables it. The layer shows in the Layers tree as +`Live Coverage []` with an `(available)` / `(live)` status and a +context-menu **"Enable live coverage"** toggle. Enabling subscribes to +`coverage_tiles`, warm-loads the disk cache, and starts the reconciler +(request/prune). Disabling unsubscribes from the tile stream (the catalog +subscription may remain). The per-source enabled/disabled state persists via +`QSettings` keyed on the source namespace, so an **enabled** source re-subscribes +on warm restart while a **never-enabled** one stays passive. + +This is a conscious tension with **ADR-0005 §2**, which anticipated a *generic* +`CatalogSource` seam for topic-discovery consumers (camp#44/#68). That seam is not +yet built, so this PR discovers directly (the `GridManager` pattern) but gates the +expensive subscription behind an explicit operator action — serving #71 (no +surprise bandwidth on a slow link) without waiting on #44/#68. When the generic +`CatalogSource` lands, this manager should migrate onto it; the opt-in gate is the +behaviour to preserve through that migration. + +### D6 — Operator presentation (ADR-0005 browse/compose) + +The live layer appears in the **Layers tree** (compose side), not the Stores +catalog browser — it is ROS-discovered, not file-browsed. Its display name carries +the source and its `(available)`/`(live)` state so the operator can tell a passive +discovered source from an active one. Selected band + colormap persist per source +(same controls as `GggsTileLayer`). + +## Consequences + +- `camp_map_ros` gains a dependency on `marine_tiled_raster_store` (the + reconciler) — added to `CMakeLists.txt` and `package.xml`. +- A new `map::SonarLiveCacheLayerType` enum value in `map/item_types.h` for + `qgraphicsitem_cast`. +- The GL render path is **duplicated** from `GggsTileLayer` (shader constants, + FBO, LUT) rather than shared. The duplication is explicit and temporary, marked + with a `// NOTE: shader duplicated; unify via RasterFieldSource camp#134` + comment. Unifying it is the whole point of Part A / camp#134. +- The new translation units live under `ros/live_coverage/` (not `raster/`): + they depend on `marine_interfaces` + `marine_tiled_raster_store`, so they belong + in the ROS-dependent `camp_map_ros` library — `camp_map` stays ROS-free, the + invariant ADR-0002 / the CMake layering relies on. (This refines the plan's + `raster/` placement for layering integrity.) +- No eviction beyond prune-on-absence. A long survey could accumulate many tiles; + eviction-by-area is a deferred follow-up (`// TODO(camp): eviction-by-area`), + not implemented here (operator secondary-question default). + +## Alternatives considered + +- **Write-through then rescan from disk** (reuse `GggsTileLayer::rescan()` + wholesale): rejected — it re-reads tiles the writer is still flushing, the exact + read/write race the in-memory-authoritative model avoids, and it pays a GDAL + read on every live update. +- **Auto-subscribe on discovery** (the pure `GridManager` pattern, as the plan + originally proposed): rejected for the tile stream — it spends bandwidth on a + best-effort full-tile stream the operator never asked for (#71). Discovery stays + automatic; only the tile-stream subscription is gated. See D5. +- **Durable store instead of crash-safe cache**: rejected — the tiles are a + rebuildable preview, not data of record (D1); a durable store would imply a + provenance/lifecycle contract these display tiles deliberately don't carry. diff --git a/package.xml b/package.xml index 928f976..0f02c2d 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ pluginlib marine_autonomy marine_interfaces + marine_tiled_raster_store marine_nav_interfaces marine_nav_tasks diff --git a/src/camp_map/map/item_types.h b/src/camp_map/map/item_types.h index 37a5a96..a7347e7 100644 --- a/src/camp_map/map/item_types.h +++ b/src/camp_map/map/item_types.h @@ -35,6 +35,7 @@ enum ItemType MapToolType, RasterLayerType, GggsTileLayerType, + SonarLiveCacheLayerType, RosGeometryManagerType, RosNamesManagerType, RosNodeType, diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp new file mode 100644 index 0000000..2b639a0 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp @@ -0,0 +1,819 @@ +#include "sonar_live_cache_layer.h" + +#include "../node.h" +#include "../../map_view/web_mercator.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +namespace +{ + +// NOTE: shader duplicated from GggsTileLayer; will unify via RasterFieldSource in +// camp#134. The pipeline is identical (CPU-side geo->Web-Mercator warp per vertex, +// single-band R32F value texture, colormap LUT, per-tile NoData discard); only the +// data source differs (in-memory dequantized Float32 vs. GggsTile's GDAL read). +constexpr char kVertexShader[] = R"( +#version 120 +attribute vec2 a_pos; +attribute vec2 a_texcoord; +uniform mat4 u_mvp; +varying vec2 v_texcoord; +void main() +{ + gl_Position = u_mvp * vec4(a_pos, 0.0, 1.0); + v_texcoord = a_texcoord; +} +)"; + +constexpr char kFragmentShader[] = R"( +#version 120 +uniform sampler2D u_tex; +uniform sampler2D u_lut; +uniform float u_min; +uniform float u_max; +uniform int u_has_nodata; +uniform float u_nodata; +varying vec2 v_texcoord; +void main() +{ + float v = texture2D(u_tex, v_texcoord).r; + if(u_has_nodata != 0 && v == u_nodata) + discard; + float t = clamp((v - u_min) / max(u_max - u_min, 1.0), 0.0, 1.0); + vec4 c = texture2D(u_lut, vec2(t, 0.5)); + gl_FragColor = vec4(c.rgb, 1.0); +} +)"; + +// Flat, filesystem-safe token for a source namespace (percent-encode every '/'). +QString sanitize(const std::string& ns) +{ + return QString::fromLatin1(QUrl::toPercentEncoding(QString::fromStdString(ns))); +} + +// Off-thread write-through worker. Takes everything by value so it is fully +// self-contained — safe even if the layer is destroyed before it finishes. +// Atomic temp+rename for crash safety only (ADR-0006 D2). +void writeTileToCache(SonarLiveTile tile, std::string dir) +{ + namespace fs = std::filesystem; + std::error_code ec; + fs::create_directories(dir, ec); + const std::string stem = std::to_string(static_cast(tile.index().level())) + "_" + + std::to_string(tile.index().row()) + "_" + + std::to_string(tile.index().column()); + const std::string final_path = (fs::path(dir) / (stem + ".tif")).string(); + const std::string tmp_path = final_path + ".tmp"; + if(!tile.writeToGeoTiff(tmp_path)) + { + fs::remove(tmp_path, ec); + return; + } + fs::rename(tmp_path, final_path, ec); + if(ec) + fs::remove(tmp_path, ec); +} + +} // namespace + +SonarLiveCacheLayer::SonarLiveCacheLayer(MapItem* parent, Node* node, + const QString& base_namespace): + Layer(parent, node, "Live Coverage [" + base_namespace + "]"), + base_namespace_(base_namespace.toStdString()) +{ + // Cache dir: /, base defaults to AppDataLocation but + // is operator-overridable (ADR-0006 D2 / #117 no-hardcoded-defaults). + QSettings settings; + const QString base_dir = settings.value( + "LiveTileCache/cache_dir", + QStandardPaths::writableLocation(QStandardPaths::AppDataLocation) + + "/live_tile_cache").toString(); + cache_dir_ = QDir(base_dir).filePath(sanitize(base_namespace_)).toStdString(); + + // Availability is cheap: subscribe to the catalog even while inactive so the + // operator can see how much coverage the source holds. The tile stream stays + // unsubscribed until enableLiveCoverage(). + subscribeCatalog(); + updateDisplay(); +} + +SonarLiveCacheLayer::~SonarLiveCacheLayer() +{ + // Join any in-flight write-through (each worker is self-contained, but joining + // matches the camp_map worker-lifetime convention). Then tear down GL. + write_watcher_.waitForFinished(); + releaseGL(); +} + +QString SonarLiveCacheLayer::settingsKey() const +{ + // Identity is the source namespace, not the display label (which embeds it but + // is a human string). Flat percent-encoded key, prefixed for readability. + return "live:" + sanitize(base_namespace_); +} + +// ------------------------------- subscriptions ------------------------------- + +void SonarLiveCacheLayer::subscribeCatalog() +{ + if(catalog_sub_ || !node_ || !node_->node()) + return; + // Complete snapshot, latest wins; transient-local so a late joiner gets the + // current catalog immediately (matches the boat-side producer QoS). + rclcpp::QoS qos(1); + qos.transient_local().reliable(); + const std::string topic = base_namespace_ + "/coverage_catalog"; + catalog_sub_ = node_->node()->create_subscription( + topic, qos, + [this](marine_interfaces::msg::TileCatalog::SharedPtr msg) + { + // ROS thread: copy + marshal to the GUI thread (ADR-0001). Never touch the + // reconciler / tiles here. + QMetaObject::invokeMethod( + this, [this, m = *msg]() { handleCatalog(m); }, Qt::QueuedConnection); + }); +} + +void SonarLiveCacheLayer::subscribeTiles() +{ + if(!node_ || !node_->node()) + return; + if(!tile_sub_) + { + rclcpp::QoS qos(10); + qos.best_effort(); + const std::string topic = base_namespace_ + "/coverage_tiles"; + tile_sub_ = + node_->node()->create_subscription( + topic, qos, + [this](marine_interfaces::msg::SonarVisualizationTile::SharedPtr msg) + { + QMetaObject::invokeMethod( + this, [this, m = *msg]() { handleTile(m); }, Qt::QueuedConnection); + }); + } + if(!request_pub_) + { + rclcpp::QoS qos(10); + qos.reliable(); + request_pub_ = node_->node()->create_publisher( + base_namespace_ + "/coverage_requests", qos); + } +} + +void SonarLiveCacheLayer::unsubscribeTiles() +{ + tile_sub_.reset(); + request_pub_.reset(); +} + +void SonarLiveCacheLayer::publishRequest(const std::vector& tiles) +{ + if(tiles.empty() || !request_pub_ || !node_ || !node_->node()) + return; + marine_interfaces::msg::TileRequest msg; + msg.header.stamp = node_->node()->now(); + msg.header.frame_id = "gggs"; + msg.tiles.reserve(tiles.size()); + for(const auto& index : tiles) + msg.tiles.push_back(tileIndexFromGridIndex(index)); + request_pub_->publish(msg); +} + +// -------------------------------- activation --------------------------------- + +void SonarLiveCacheLayer::enableLiveCoverage() +{ + if(enabled_) + return; + enabled_ = true; + // Warm-load the disk cache BEFORE subscribing so the in-memory state is seeded + // (and the reconciler primed) before any live tile/catalog arrives — and so the + // GUI-thread warm-load can't race a callback (ADR-0006 D3/D4). + warmLoad(); + subscribeTiles(); + writeSettings(); + updateDisplay(); + cached_image_ = QImage(); + update(boundingRect()); +} + +void SonarLiveCacheLayer::disableLiveCoverage() +{ + if(!enabled_) + return; + enabled_ = false; + unsubscribeTiles(); + writeSettings(); + updateDisplay(); + update(boundingRect()); +} + +void SonarLiveCacheLayer::warmLoad() +{ + if(!level_) + { + // Without a known level we can't recover a GridIndex from a cached GeoTIFF. + // Probe one cached file's level from its filename `__.tif`. + QDir dir(QString::fromStdString(cache_dir_)); + const QStringList files = dir.entryList(QStringList() << "*.tif", QDir::Files); + for(const QString& name : files) + { + const QString base = name.section('.', 0, 0); + bool ok = false; + const int lvl = base.section('_', 0, 0).toInt(&ok); + if(ok && lvl >= 0 && lvl < 256) + { + level_ = static_cast(lvl); + break; + } + } + } + if(!level_) + return; // nothing cached yet + + const gggs::Level level(*level_); + for(auto& tile : SonarLiveTile::loadCacheDir(cache_dir_, level)) + { + const gggs::GridIndex index = tile.index(); + if(!index.valid()) + continue; + // Seed the reconciler at version 0: "have something" — older than any real + // catalog version, so the next catalog re-requests it if the boat is newer, + // and the prune gate never deletes it spuriously (ADR-0006 D3). + reconciler_.markHave(index, 0); + tiles_.insert_or_assign(index, Entry{std::move(tile), nullptr, true}); + } + if(band_name_.empty()) + band_name_ = defaultBand(); + recomputeBounds(); + resetAutoRange(); + foldAutoRange(); + updateDisplay(); +} + +// ------------------------------ message handlers ----------------------------- + +void SonarLiveCacheLayer::handleTile(const marine_interfaces::msg::SonarVisualizationTile& msg) +{ + if(!enabled_) + return; + const gggs::GridIndex index = gridIndexFromTileIndex(msg.index); + if(!index.valid()) + return; + level_ = msg.index.level; + + const marine_tiled_raster_store::TileVersion version = toNanoseconds(msg.header); + // Newest-wins: a strictly-older patch is stale (the catalog/TileRequest path + // heals any genuinely lost sub-window by re-sending the tile in full). + const auto held = reconciler_.versionOf(index); + if(held && version < *held) + return; + + auto it = tiles_.find(index); + const bool is_new = (it == tiles_.end()); + if(is_new) + it = tiles_.emplace(index, + Entry{SonarLiveTile(index, msg.width, msg.height), nullptr, true}) + .first; + Entry& entry = it->second; + entry.tile.applyPatch(msg); + entry.texture_dirty = true; + reconciler_.markHave(index, entry.tile.version()); + // TODO(camp): eviction-by-area follow-up — only prune-on-absence bounds the + // cache today, so a long survey can accumulate many tiles (ADR-0006 consequences). + + scheduleWriteThrough(entry.tile); + + if(band_name_.empty()) + band_name_ = defaultBand(); + if(is_new) + recomputeBounds(); + foldAutoRange(); + updateDisplay(); + cached_image_ = QImage(); + update(boundingRect()); +} + +void SonarLiveCacheLayer::handleCatalog(const marine_interfaces::msg::TileCatalog& msg) +{ + // Learn the level even while inactive (used for availability + later warm-load). + if(!level_ && !msg.entries.empty()) + level_ = msg.entries.front().index.level; + + if(!enabled_) + { + updateDisplay(); // refresh availability count + return; + } + + // Anti-entropy: converge the cache to exactly the catalog set (ADR-0006 D4). + const marine_tiled_raster_store::TileCatalog catalog = toReconcilerCatalog(msg); + const marine_tiled_raster_store::ReconcileResult result = reconciler_.reconcile(catalog); + + publishRequest(result.to_request); + + bool pruned = false; + for(const auto& index : result.to_prune) + { + auto it = tiles_.find(index); + if(it != tiles_.end()) + { + tiles_.erase(it); + pruned = true; + } + // Delete the disk copy too (best-effort). + namespace fs = std::filesystem; + const std::string stem = std::to_string(static_cast(index.level())) + "_" + + std::to_string(index.row()) + "_" + + std::to_string(index.column()) + ".tif"; + std::error_code ec; + fs::remove(fs::path(cache_dir_) / stem, ec); + reconciler_.drop(index); + } + if(pruned) + { + recomputeBounds(); + foldAutoRange(); + cached_image_ = QImage(); + update(boundingRect()); + } + updateDisplay(); +} + +void SonarLiveCacheLayer::writeThroughFinished() +{ + // Hook kept for symmetry; the worker is self-contained, so nothing to fold. +} + +void SonarLiveCacheLayer::scheduleWriteThrough(const SonarLiveTile& tile) +{ + // Copy the tile + dir by value into a self-contained worker (no `this` deref). + write_watcher_.setFuture(QtConcurrent::run(writeTileToCache, tile, cache_dir_)); +} + +// ------------------------------ extent / range ------------------------------- + +void SonarLiveCacheLayer::recomputeBounds() +{ + prepareGeometryChange(); + QRectF bounds; + bool first = true; + for(const auto& entry : tiles_) + { + const SonarLiveTile& tile = entry.second.tile; + const QPointF lo = web_mercator::geoToMap(QGeoCoordinate(tile.minLat(), tile.minLon())); + const QPointF hi = web_mercator::geoToMap(QGeoCoordinate(tile.maxLat(), tile.maxLon())); + const QRectF rect = QRectF(lo, hi).normalized(); + bounds = first ? rect : bounds.united(rect); + first = false; + } + scene_bounds_ = bounds; + if(!scene_bounds_.isNull()) + { + // North-up anchored at the NW corner with a negative-Y transform (the camp_map + // raster convention; matches GggsTileLayer). + setTransform(QTransform::fromScale(1.0, -1.0)); + setPos(QPointF(scene_bounds_.left(), scene_bounds_.bottom())); + } +} + +void SonarLiveCacheLayer::resetAutoRange() +{ + data_min_ = 1.0; + data_max_ = 0.0; +} + +void SonarLiveCacheLayer::foldAutoRange() +{ + bool first = (data_min_ > data_max_); + for(const auto& entry : tiles_) + { + const SonarLiveBand* band = entry.second.tile.band(band_name_); + if(!band || band->data_min > band->data_max) + continue; + if(first || band->data_min < data_min_) data_min_ = band->data_min; + if(first || band->data_max > data_max_) data_max_ = band->data_max; + first = false; + } +} + +std::string SonarLiveCacheLayer::defaultBand() const +{ + // Prefer "depth" when present (the primary bathy band); else the first band. + for(const auto& entry : tiles_) + if(entry.second.tile.band("depth")) + return "depth"; + for(const auto& entry : tiles_) + { + const auto names = entry.second.tile.bandNames(); + if(!names.empty()) + return names.front(); + } + return std::string(); +} + +void SonarLiveCacheLayer::updateDisplay() +{ + if(enabled_) + setStatus(QString("(live: %1 tiles)").arg(qulonglong(tiles_.size()))); + else + setStatus("(available)"); +} + +// --------------------------------- rendering --------------------------------- + +QRectF SonarLiveCacheLayer::boundingRect() const +{ + return QRectF(QPointF(0.0, 0.0), scene_bounds_.size()); +} + +bool SonarLiveCacheLayer::ensureGL() +{ + if(gl_failed_) + return false; + if(gl_context_) + return true; + gl_surface_ = new QOffscreenSurface(); + gl_surface_->create(); + gl_context_ = new QOpenGLContext(); + if(!gl_surface_->isValid() || !gl_context_->create()) + { + qWarning("SonarLiveCacheLayer: offscreen GL unavailable; tiles not rendered"); + gl_failed_ = true; + delete gl_context_; gl_context_ = nullptr; + delete gl_surface_; gl_surface_ = nullptr; + return false; + } + return true; +} + +bool SonarLiveCacheLayer::ensureProgram() +{ + if(program_) + return program_->isLinked(); + program_ = std::make_unique(); + program_->addShaderFromSourceCode(QOpenGLShader::Vertex, kVertexShader); + program_->addShaderFromSourceCode(QOpenGLShader::Fragment, kFragmentShader); + if(!program_->link()) + { + qWarning("SonarLiveCacheLayer: shader link failed: %s", + program_->log().toUtf8().constData()); + setStatus("(shader error)"); + return false; + } + return true; +} + +QOpenGLTexture* SonarLiveCacheLayer::ensureLut() +{ + if(lut_texture_ && !lut_dirty_) + return lut_texture_.get(); + std::vector lut(256 * 4); + for(int i = 0; i < 256; ++i) + { + const QColor c = colormap_.colorNormalized(i / 255.0); + lut[i * 4 + 0] = uchar(c.red()); + lut[i * 4 + 1] = uchar(c.green()); + lut[i * 4 + 2] = uchar(c.blue()); + lut[i * 4 + 3] = uchar(c.alpha()); + } + if(!lut_texture_) + { + lut_texture_ = std::make_unique(QOpenGLTexture::Target2D); + lut_texture_->setFormat(QOpenGLTexture::RGBA8_UNorm); + lut_texture_->setSize(256, 1); + lut_texture_->setMipLevels(1); + lut_texture_->allocateStorage(QOpenGLTexture::RGBA, QOpenGLTexture::UInt8); + lut_texture_->setMinMagFilters(QOpenGLTexture::Linear, QOpenGLTexture::Linear); + lut_texture_->setWrapMode(QOpenGLTexture::ClampToEdge); + } + lut_texture_->setData(QOpenGLTexture::RGBA, QOpenGLTexture::UInt8, lut.data()); + lut_dirty_ = false; + return lut_texture_.get(); +} + +QOpenGLTexture* SonarLiveCacheLayer::textureFor(Entry& entry) +{ + const SonarLiveBand* band = entry.tile.band(band_name_); + if(!band || band->data.empty()) + return nullptr; + if(entry.texture && !entry.texture_dirty) + return entry.texture.get(); + // (Re)upload the selected band as an R32F value texture, Nearest-filtered so the + // shader's exact-equality NoData discard never blends across a sentinel boundary + // (same rationale as GggsTile::texture()). + entry.texture = std::make_unique(QOpenGLTexture::Target2D); + entry.texture->setFormat(QOpenGLTexture::R32F); + entry.texture->setSize(entry.tile.width(), entry.tile.height()); + entry.texture->setMipLevels(1); + entry.texture->allocateStorage(QOpenGLTexture::Red, QOpenGLTexture::Float32); + entry.texture->setData(QOpenGLTexture::Red, QOpenGLTexture::Float32, band->data.data()); + entry.texture->setMinMagFilters(QOpenGLTexture::Nearest, QOpenGLTexture::Nearest); + entry.texture->setWrapMode(QOpenGLTexture::ClampToEdge); + entry.texture_dirty = false; + return entry.texture.get(); +} + +QImage SonarLiveCacheLayer::renderImage(const QSize& size) +{ + if(tiles_.empty() || data_min_ > data_max_ || size.isEmpty()) + return QImage(); + if(!ensureGL()) + return QImage(); + if(!gl_context_->makeCurrent(gl_surface_)) + { + qWarning("SonarLiveCacheLayer: makeCurrent failed; tiles not rendered"); + gl_failed_ = true; + return QImage(); + } + + QOpenGLFunctions* f = gl_context_->functions(); + if(!fbo_ || fbo_->size() != size) + fbo_ = std::make_unique(size); + + fbo_->bind(); + f->glViewport(0, 0, size.width(), size.height()); + f->glClearColor(0.0f, 0.0f, 0.0f, 0.0f); + f->glClear(GL_COLOR_BUFFER_BIT); + f->glDisable(GL_DEPTH_TEST); + f->glEnable(GL_BLEND); + f->glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA); + + if(ensureProgram()) + { + const double origin_x = scene_bounds_.left(); + const double origin_y = scene_bounds_.top(); + const double width_m = scene_bounds_.width(); + const double height_m = scene_bounds_.height(); + QMatrix4x4 mvp; + mvp.ortho(0.0f, float(width_m), 0.0f, float(height_m), -1.0f, 1.0f); + + program_->bind(); + program_->setUniformValue("u_mvp", mvp); + program_->setUniformValue("u_min", float(data_min_)); + program_->setUniformValue("u_max", float(data_max_)); + program_->setUniformValue("u_tex", 0); + program_->setUniformValue("u_lut", 1); + QOpenGLTexture* lut = ensureLut(); + if(lut) + lut->bind(1); + + const int pos_loc = program_->attributeLocation("a_pos"); + const int texcoord_loc = program_->attributeLocation("a_texcoord"); + program_->enableAttributeArray(pos_loc); + program_->enableAttributeArray(texcoord_loc); + + const int rows = kLatSubdivisions + 1; + std::vector verts; + verts.reserve(rows * 2 * 4); + for(auto& item : tiles_) + { + Entry& entry = item.second; + const SonarLiveBand* band = entry.tile.band(band_name_); + if(!band) + continue; + QOpenGLTexture* texture = textureFor(entry); + if(!texture) + continue; + + verts.clear(); + const double min_lon = entry.tile.minLon(); + const double max_lon = entry.tile.maxLon(); + const double min_lat = entry.tile.minLat(); + const double max_lat = entry.tile.maxLat(); + for(int r = 0; r < rows; ++r) + { + const double frac = double(r) / kLatSubdivisions; + const double lat = max_lat + (min_lat - max_lat) * frac; // north -> south + const float v = float(frac); + const QPointF l = web_mercator::geoToMap(QGeoCoordinate(lat, min_lon)); + const QPointF rt = web_mercator::geoToMap(QGeoCoordinate(lat, max_lon)); + verts.insert(verts.end(), + {float(l.x() - origin_x), float(l.y() - origin_y), 0.0f, v}); + verts.insert(verts.end(), + {float(rt.x() - origin_x), float(rt.y() - origin_y), 1.0f, v}); + } + + texture->bind(0); + program_->setUniformValue("u_has_nodata", band->has_nodata ? 1 : 0); + program_->setUniformValue("u_nodata", band->has_nodata ? band->nodata : 0.0f); + program_->setAttributeArray(pos_loc, GL_FLOAT, verts.data(), 2, 4 * sizeof(float)); + program_->setAttributeArray(texcoord_loc, GL_FLOAT, verts.data() + 2, 2, + 4 * sizeof(float)); + f->glDrawArrays(GL_TRIANGLE_STRIP, 0, rows * 2); + texture->release(0); + } + + if(lut) + lut->release(1); + program_->disableAttributeArray(pos_loc); + program_->disableAttributeArray(texcoord_loc); + program_->release(); + } + + fbo_->release(); + QImage image = fbo_->toImage(); + gl_context_->doneCurrent(); + return image; +} + +void SonarLiveCacheLayer::paint(QPainter* painter, const QStyleOptionGraphicsItem*, QWidget*) +{ + if(tiles_.empty() || data_min_ > data_max_) + return; + + const QRectF dev = painter->worldTransform().mapRect(boundingRect()); + const int w = std::min(kMaxImageEdge, std::max(1, int(std::ceil(std::abs(dev.width()))))); + const int h = std::min(kMaxImageEdge, std::max(1, int(std::ceil(std::abs(dev.height()))))); + const QSize size(w, h); + + if(cached_image_.isNull() || cached_size_ != size) + { + cached_image_ = renderImage(size); + cached_size_ = size; + } + if(cached_image_.isNull()) + return; + + painter->save(); + painter->setRenderHint(QPainter::SmoothPixmapTransform); + painter->drawImage(boundingRect(), cached_image_); + painter->restore(); +} + +void SonarLiveCacheLayer::releaseGL() +{ + if(gl_context_ && gl_surface_ && gl_context_->makeCurrent(gl_surface_)) + { + fbo_.reset(); + program_.reset(); + lut_texture_.reset(); + for(auto& item : tiles_) + item.second.texture.reset(); + gl_context_->doneCurrent(); + } + delete gl_context_; gl_context_ = nullptr; + delete gl_surface_; gl_surface_ = nullptr; +} + +// ------------------------------- band / colormap ----------------------------- + +void SonarLiveCacheLayer::setColormap(map::ColorMap::Type type) +{ + if(type == colormap_.type()) + return; + colormap_.setType(type); + lut_dirty_ = true; + cached_image_ = QImage(); + writeSettings(); + update(boundingRect()); +} + +void SonarLiveCacheLayer::setBandName(const std::string& name) +{ + if(name == band_name_ || name.empty()) + return; + band_name_ = name; + // The value texture is per-band: mark every tile's texture for re-upload, reset + // and re-fold the auto-range over the new band. + for(auto& item : tiles_) + item.second.texture_dirty = true; + resetAutoRange(); + foldAutoRange(); + cached_image_ = QImage(); + writeSettings(); + update(boundingRect()); +} + +// --------------------------------- context menu ------------------------------ + +void SonarLiveCacheLayer::contextMenu(QMenu* menu) +{ + Layer::contextMenu(menu); + + // [camp#121 / ADR-0006 D5] The opt-in gate: enabling subscribes to the + // best-effort tile stream; disabling stops it. Default discovered state is + // inactive so a slow link (#71) pays nothing until the operator asks. + if(enabled_) + { + QAction* disable = menu->addAction("Disable live coverage"); + connect(disable, &QAction::triggered, this, + &SonarLiveCacheLayer::disableLiveCoverage); + } + else + { + QAction* enable = menu->addAction("Enable live coverage"); + connect(enable, &QAction::triggered, this, + &SonarLiveCacheLayer::enableLiveCoverage); + } + + QMenu* colormap_menu = menu->addMenu("Colormap"); + for(auto type : map::ColorMap::allTypes()) + { + QAction* action = colormap_menu->addAction(map::ColorMap::name(type)); + action->setCheckable(true); + action->setChecked(type == colormap_.type()); + connect(action, &QAction::triggered, this, [this, type]() { setColormap(type); }); + } + + // Band picker over the union of band names across held tiles. Only shown when + // there is more than one band to choose from. + std::vector names; + for(const auto& item : tiles_) + for(const auto& name : item.second.tile.bandNames()) + if(std::find(names.begin(), names.end(), name) == names.end()) + names.push_back(name); + if(names.size() > 1) + { + QMenu* band_menu = menu->addMenu("Band"); + for(const auto& name : names) + { + QAction* action = band_menu->addAction(QString::fromStdString(name)); + action->setCheckable(true); + action->setChecked(name == band_name_); + connect(action, &QAction::triggered, this, [this, name]() { setBandName(name); }); + } + } +} + +// --------------------------------- persistence ------------------------------- + +void SonarLiveCacheLayer::readSettings() +{ + Layer::readSettings(); + QSettings settings; + settings.beginGroup("MapItem"); + settings.beginGroup(settingsKey()); + // Default discovered layers OFF in the tree (like GGGS tile-sets) — the operator + // turns coverage on explicitly. + setVisible(settings.value("visible", false).toBool()); + const map::ColorMap::Type type = map::ColorMap::typeFromName( + settings.value("colormap", map::ColorMap::name(colormap_.type())).toString()); + const std::string band = settings.value("band", QString::fromStdString(band_name_)) + .toString().toStdString(); + const bool was_enabled = settings.value("live_enabled", false).toBool(); + settings.endGroup(); + settings.endGroup(); + + if(type != colormap_.type()) + { + colormap_.setType(type); + lut_dirty_ = true; + cached_image_ = QImage(); + } + if(!band.empty()) + band_name_ = band; + // [ADR-0006 D5] An enabled source re-subscribes on warm restart; a never-enabled + // one stays passive. enableLiveCoverage() persists, which is a harmless re-write. + if(was_enabled && !enabled_) + enableLiveCoverage(); +} + +void SonarLiveCacheLayer::writeSettings() +{ + Layer::writeSettings(); + QSettings settings; + settings.beginGroup("MapItem"); + settings.beginGroup(settingsKey()); + settings.setValue("colormap", map::ColorMap::name(colormap_.type())); + settings.setValue("band", QString::fromStdString(band_name_)); + settings.setValue("live_enabled", enabled_); + settings.endGroup(); + settings.endGroup(); +} + +} // namespace live_coverage +} // namespace ros +} // namespace camp diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h new file mode 100644 index 0000000..ed96fd8 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h @@ -0,0 +1,195 @@ +#ifndef CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_CACHE_LAYER_H +#define CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_CACHE_LAYER_H + +#include "../layer.h" +#include "../../map/color_map.h" +#include "sonar_live_tile.h" + +#include "marine_tiled_raster_store/tile_catalog.hpp" + +#include "marine_interfaces/msg/sonar_visualization_tile.hpp" +#include "marine_interfaces/msg/tile_catalog.hpp" +#include "marine_interfaces/msg/tile_request.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class QOpenGLContext; +class QOffscreenSurface; +class QOpenGLFramebufferObject; +class QOpenGLShaderProgram; +class QOpenGLTexture; + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +/// [camp#121] Live coverage cache layer for ONE boat-side source namespace. +/// +/// Renders dequantized in-memory `SonarLiveTile`s through a GL pipeline +/// duplicated from `GggsTileLayer` (no GDAL on the hot path) and drives +/// anti-entropy convergence with a `TileCatalogReconciler`. See ADR-0006. +/// +/// **Activation model (ADR-0006 D5).** The layer is spawned by discovery in a +/// *discovered-but-inactive* state: it may subscribe to the cheap, transient-local +/// `coverage_catalog` to advertise availability, but it does NOT subscribe to the +/// best-effort `coverage_tiles` stream and does NOT publish `TileRequest` until the +/// operator enables it (context-menu "Enable live coverage"). Enabling warm-loads +/// the disk cache, subscribes to the tile stream, and starts request/prune. The +/// enabled flag persists per source (QSettings), so an enabled source re-subscribes +/// on warm restart while a never-enabled one stays passive. This serves #71 (no +/// surprise bandwidth on a slow link). +/// +/// **Threading invariant (ADR-0001 / ADR-0006 D4).** `TileCatalogReconciler` and +/// the tile map are NOT thread-safe. They are touched ONLY on the GUI thread. The +/// ROS subscription callbacks do nothing but copy the message and marshal it to the +/// GUI thread (QMetaObject::invokeMethod, queued); the GUI-thread handlers do all +/// reconcile / markHave / drop / applyPatch / write-through scheduling. Warm-load +/// runs on the GUI thread before the subscriptions are created, so it cannot race a +/// callback. +class SonarLiveCacheLayer: public Layer +{ + Q_OBJECT + Q_INTERFACES(QGraphicsItem) +public: + /// @param base_namespace the remapped source namespace (e.g. "/cube_bathymetry"). + /// The topics are `/coverage_tiles`, `/coverage_catalog`, + /// `/coverage_requests`. + SonarLiveCacheLayer(MapItem* parent, Node* node, const QString& base_namespace); + ~SonarLiveCacheLayer() override; + + enum { Type = map::SonarLiveCacheLayerType }; + int type() const override { return Type; } + + QRectF boundingRect() const override; + void paint(QPainter* painter, const QStyleOptionGraphicsItem* option, QWidget* widget) override; + + /// Identity-stable QSettings key (the source namespace, not the display label). + QString settingsKey() const override; + + /// Whether the tile stream is currently subscribed (operator-enabled). + bool isEnabled() const { return enabled_; } + + /// [camp#121] Render the in-memory tiles into an offscreen image of @p size + /// spanning the layer extent. Null image if there is no data / GL is + /// unavailable. Exposed for a headless render check (skips with no GL). + QImage renderImage(const QSize& size); + + /// The layer's Web-Mercator extent (union of tile extents). Exposed for tests. + QRectF sceneBounds() const { return scene_bounds_; } + +public slots: + /// [camp#121] Subscribe to the tile stream, warm-load the disk cache, start + /// request/prune, and persist enabled=true. No-op if already enabled. + void enableLiveCoverage(); + /// [camp#121] Unsubscribe from the tile stream and persist enabled=false. The + /// in-memory + on-disk cache and the (cheap) catalog subscription are kept. + void disableLiveCoverage(); + +protected: + void contextMenu(QMenu* menu) override; + void readSettings() override; + void writeSettings() override; + +private slots: + /// GUI thread: a tile patch arrived (newest-wins, applyPatch, markHave, + /// schedule write-through, repaint). + void handleTile(const marine_interfaces::msg::SonarVisualizationTile& msg); + /// GUI thread: a catalog arrived (reconcile -> request + prune). + void handleCatalog(const marine_interfaces::msg::TileCatalog& msg); + /// GUI thread, after a write-through worker finishes (currently a no-op hook). + void writeThroughFinished(); + +private: + // [camp#121] One held tile plus its lazily-(re)uploaded GL texture for the + // currently selected band. texture_dirty marks the band data changed since the + // last upload so the render re-uploads it. + struct Entry + { + SonarLiveTile tile; + std::unique_ptr texture; + bool texture_dirty = true; + }; + + void subscribeCatalog(); + void subscribeTiles(); + void unsubscribeTiles(); + void publishRequest(const std::vector& tiles); + void warmLoad(); + void scheduleWriteThrough(const SonarLiveTile& tile); + + void recomputeBounds(); + void resetAutoRange(); + void foldAutoRange(); + void updateDisplay(); // refresh display name + status from enabled_/tile count + + // [camp#121] Band selection (by NAME, since live bands are named, unlike the + // GggsTile 1-indexed bands). Default picks "depth" if present else the first. + std::string defaultBand() const; + void setColormap(map::ColorMap::Type type); + void setBandName(const std::string& name); + + // GL helpers (duplicated from GggsTileLayer; see camp#134). + bool ensureGL(); + bool ensureProgram(); + QOpenGLTexture* ensureLut(); + QOpenGLTexture* textureFor(Entry& entry); + void releaseGL(); + + static constexpr int kLatSubdivisions = 16; + static constexpr int kMaxImageEdge = 4096; + + std::string base_namespace_; // e.g. "/cube_bathymetry" + std::string cache_dir_; // / + bool enabled_ = false; // tile stream subscribed (persisted) + + // GGGS level of this source's tiles, learned from the first message/cached tile. + // Needed to recover a GridIndex from a cached GeoTIFF on warm-load. + std::optional level_; + + marine_tiled_raster_store::TileCatalogReconciler reconciler_; + std::map tiles_; + + QRectF scene_bounds_; + double data_min_ = 1.0; // auto-range over the selected band (crossed => none) + double data_max_ = 0.0; + + std::string band_name_; // selected band (persisted) + map::ColorMap colormap_{map::ColorMap::Grayscale}; + bool lut_dirty_ = true; + + rclcpp::Subscription::SharedPtr tile_sub_; + rclcpp::Subscription::SharedPtr catalog_sub_; + rclcpp::Publisher::SharedPtr request_pub_; + + // Write-through runs off the GUI thread; keep them tracked so the dtor joins. + QFutureWatcher write_watcher_; + + // Offscreen GL (the layer owns it; never touches the GUI context). Duplicated + // from GggsTileLayer — unify via RasterFieldSource camp#134. + QOpenGLContext* gl_context_ = nullptr; + QOffscreenSurface* gl_surface_ = nullptr; + std::unique_ptr fbo_; + std::unique_ptr program_; + std::unique_ptr lut_texture_; + bool gl_failed_ = false; + + QImage cached_image_; + QSize cached_size_; +}; + +} // namespace live_coverage +} // namespace ros +} // namespace camp + +#endif diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp b/src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp new file mode 100644 index 0000000..5f6c479 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_manager.cpp @@ -0,0 +1,76 @@ +#include "sonar_live_cache_manager.h" + +#include "../node.h" +#include "../names_manager.h" +#include "../../map/layer_list.h" +#include "sonar_live_cache_layer.h" + +#include + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +namespace +{ +// The tile-stream topic suffix; the source base namespace is the topic with this +// stripped (e.g. "/cube_bathymetry/coverage_tiles" -> "/cube_bathymetry"). +constexpr char kTilesSuffix[] = "/coverage_tiles"; +} // namespace + +SonarLiveCacheManager::SonarLiveCacheManager(MapTool* parent): + tools::LayerManager(parent, "Live Coverage Manager") +{ + auto topics_manager = new TopicsManager(this, "Topics"); + topics_manager->setTypeFilter({"marine_interfaces/msg/SonarVisualizationTile"}); + connect(topics_manager, &TopicsManager::namesUpdated, this, + &SonarLiveCacheManager::updateTopics); +} + +void SonarLiveCacheManager::updateTopics() +{ + auto topic_manager = firstChildOfType(); + if(!topic_manager) + return; + auto node_item = parentOfType(); + if(!node_item) + return; + auto node = node_item->node(); + if(!node) + return; + + const std::string suffix = kTilesSuffix; + for(const auto& topic : topic_manager->namesAndTypes()) + { + if(node->count_publishers(topic.first) == 0) + continue; + bool is_tile_topic = false; + for(const auto& type : topic.second) + if(type == "marine_interfaces/msg/SonarVisualizationTile") + is_tile_topic = true; + if(!is_tile_topic) + continue; + + // Derive the source base namespace by stripping the tile-stream suffix. + const std::string& full = topic.first; + if(full.size() <= suffix.size() || + full.compare(full.size() - suffix.size(), suffix.size(), suffix) != 0) + continue; + const std::string base = full.substr(0, full.size() - suffix.size()); + + if(sources_[base]) + continue; // already spawned a layer for this source + auto layers = topLevelLayers(); + if(!layers) + continue; + new SonarLiveCacheLayer(layers, node_item, QString::fromStdString(base)); + sources_[base] = true; + } +} + +} // namespace live_coverage +} // namespace ros +} // namespace camp diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_manager.h b/src/camp_map/ros/live_coverage/sonar_live_cache_manager.h new file mode 100644 index 0000000..be0ffa8 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_manager.h @@ -0,0 +1,45 @@ +#ifndef CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_CACHE_MANAGER_H +#define CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_CACHE_MANAGER_H + +#include "../../tools/layer_manager.h" + +#include +#include + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +/// [camp#121] Discovers boat-side live-coverage sources and spawns one +/// `SonarLiveCacheLayer` per source. Mirrors `GridManager`'s topic-discovery +/// pattern: a child `TopicsManager` filtered to `SonarVisualizationTile`; on each +/// graph update, every `/coverage_tiles` topic with an active publisher gets +/// a layer (deduped by base namespace). +/// +/// Discovery is automatic, but the spawned layer is *inactive* — it does not +/// subscribe to the best-effort tile stream until the operator enables it +/// (ADR-0006 D5). The expensive subscription is the layer's decision, gated on its +/// persisted per-source enabled flag; the manager only ensures the layer exists. +class SonarLiveCacheManager: public tools::LayerManager +{ + Q_OBJECT +public: + SonarLiveCacheManager(MapTool* parent); + +public slots: + void updateTopics(); + +private: + // base namespace -> spawned, so a topic reappearing (DDS re-discovery) doesn't + // spawn a duplicate; the existing layer's subscriptions reconnect on their own. + std::map sources_; +}; + +} // namespace live_coverage +} // namespace ros +} // namespace camp + +#endif diff --git a/src/camp_map/ros/live_coverage/sonar_live_tile.cpp b/src/camp_map/ros/live_coverage/sonar_live_tile.cpp new file mode 100644 index 0000000..9433778 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_tile.cpp @@ -0,0 +1,416 @@ +#include "sonar_live_tile.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +namespace +{ + +// Byte width of a VisualizationBand dtype (mirrors the constants in the .msg). +int dtypeBytes(std::uint8_t dtype) +{ + using VB = marine_interfaces::msg::VisualizationBand; + switch(dtype) + { + case VB::UINT8: return 1; + case VB::INT16: return 2; + case VB::UINT16: return 2; + default: return 0; // unknown -> caller skips the band + } +} + +// Read one little-endian raw cell at element index @p i from a packed band buffer +// as a double (its RAW, pre-dequantization value), per the band's dtype. +double readRaw(const std::vector& data, std::size_t i, std::uint8_t dtype) +{ + using VB = marine_interfaces::msg::VisualizationBand; + switch(dtype) + { + case VB::UINT8: + return static_cast(data[i]); + case VB::INT16: + { + const std::uint16_t lo = data[2 * i]; + const std::uint16_t hi = data[2 * i + 1]; + return static_cast(static_cast(lo | (hi << 8))); + } + case VB::UINT16: + { + const std::uint16_t lo = data[2 * i]; + const std::uint16_t hi = data[2 * i + 1]; + return static_cast(static_cast(lo | (hi << 8))); + } + default: + return 0.0; + } +} + +} // namespace + +SonarLiveTile::SonarLiveTile(const gggs::GridIndex& index, int width, int height): + index_(index), width_(width), height_(height) +{ +} + +void SonarLiveTile::applyPatch(const marine_interfaces::msg::SonarVisualizationTile& msg) +{ + // [camp#121] Generic dequantize: value = raw * scale + offset, no per-band + // hardcoded knowledge (uma ADR-0008 D1). The dirty sub-window is patched into + // the band at the GGGS cell offset; GGGS cell order has row 0 = south, while we + // hold north-up (row 0 = north) to match the GggsTile-derived render path, so + // the destination row is flipped. + const int wc = msg.window_col; + const int wr = msg.window_row; + const int ww = msg.window_width; + const int wh = msg.window_height; + + // Reject a window that doesn't fit the tile rather than writing out of bounds. + if(wc < 0 || wr < 0 || ww < 0 || wh < 0 || + wc + ww > width_ || wr + wh > height_) + return; + + for(const auto& vb : msg.bands) + { + const int width_bytes = dtypeBytes(vb.dtype); + if(width_bytes == 0) + continue; // unknown dtype + const std::size_t cell_count = static_cast(ww) * wh; + if(vb.data.size() != cell_count * width_bytes) + continue; // declared window doesn't match the payload length + + // Dequantized NoData sentinel: the shader discards cells exactly equal to it + // (same contract as GggsTile's per-band NoData), and untouched cells outside + // any received window keep it so they stay transparent. + const float nodata_value = static_cast(vb.nodata * vb.scale + vb.offset); + + SonarLiveBand& band = bands_[vb.name]; + if(band.data.empty()) + { + band.name = vb.name; + band.data.assign(static_cast(width_) * height_, nodata_value); + } + band.has_nodata = true; + band.nodata = nodata_value; + + for(int r = 0; r < wh; ++r) + { + const int north_row = (height_ - 1) - (wr + r); // GGGS south-up -> north-up + for(int c = 0; c < ww; ++c) + { + const std::size_t src = static_cast(r) * ww + c; + const double raw = readRaw(vb.data, src, vb.dtype); + const std::size_t dst = + static_cast(north_row) * width_ + (wc + c); + band.data[dst] = (raw == vb.nodata) + ? nodata_value + : static_cast(raw * vb.scale + vb.offset); + } + } + refoldRange(band); + } + + version_ = std::max(version_, toNanoseconds(msg.header)); +} + +void SonarLiveTile::refoldRange(SonarLiveBand& band) +{ + // A patch can overwrite a former extreme cell, so re-fold the whole band rather + // than only widening — the tile is small (one GGGS grid) and patches arrive at + // survey rates, so the full scan is cheap and always correct. + float lo = std::numeric_limits::max(); + float hi = std::numeric_limits::lowest(); + bool any = false; + for(float v : band.data) + { + if(!std::isfinite(v) || (band.has_nodata && v == band.nodata)) + continue; + lo = std::min(lo, v); + hi = std::max(hi, v); + any = true; + } + if(any) + { + band.data_min = lo; + band.data_max = hi; + } + else + { + band.data_min = 1.0f; // crossed: no valid samples + band.data_max = 0.0f; + } +} + +const SonarLiveBand* SonarLiveTile::band(const std::string& name) const +{ + auto it = bands_.find(name); + return it == bands_.end() ? nullptr : &it->second; +} + +std::vector SonarLiveTile::bandNames() const +{ + std::vector names; + names.reserve(bands_.size()); + for(const auto& entry : bands_) + names.push_back(entry.first); + return names; +} + +std::optional SonarLiveTile::loadFromGeoTiff(const std::string& path, + const gggs::Level& level) +{ + if(GDALGetDriverCount() == 0) + GDALAllRegister(); + + auto* dataset = GDALDataset::FromHandle(GDALOpen(path.c_str(), GA_ReadOnly)); + if(!dataset) + return std::nullopt; + + const int width = dataset->GetRasterXSize(); + const int height = dataset->GetRasterYSize(); + const int band_count = dataset->GetRasterCount(); + double geo[6]; + if(width <= 0 || height <= 0 || band_count < 1 || + dataset->GetGeoTransform(geo) != CE_None) + { + GDALClose(dataset); + return std::nullopt; + } + + // Require a geographic WGS84 raster: the geotransform is read as degrees to + // recover the GridIndex (same guard as marine_tiled_raster_store::loadTile). + const OGRSpatialReference* srs = dataset->GetSpatialRef(); + OGRErr axis_error = OGRERR_NONE; + if(srs == nullptr || !srs->IsGeographic() || + std::abs(srs->GetSemiMajor(&axis_error) - 6378137.0) > 1.0 || + axis_error != OGRERR_NONE) + { + GDALClose(dataset); + return std::nullopt; + } + + const double west = geo[0]; + const double pixel_x = geo[1]; + const double north = geo[3]; + const double pixel_y = geo[5]; + const double east = west + pixel_x * width; + const double south = north + pixel_y * height; + + // Recover the GridIndex: the cell center maps back through the level. + gggs::GridIndex index; + try + { + index = level.gridIndex(0.5 * (north + south), 0.5 * (west + east)); + } + catch(const std::exception&) + { + GDALClose(dataset); + return std::nullopt; + } + const double tol_lon = 0.5 * std::abs(pixel_x); + const double tol_lat = 0.5 * std::abs(pixel_y); + if(!index.valid() || + std::abs(index.westLongitude() - west) > tol_lon || + std::abs(index.northLatitude() - north) > tol_lat) + { + GDALClose(dataset); + return std::nullopt; + } + + SonarLiveTile tile(index, width, height); + const std::size_t cells = static_cast(width) * height; + for(int b = 1; b <= band_count; ++b) + { + GDALRasterBand* raster = dataset->GetRasterBand(b); + SonarLiveBand band; + const char* desc = raster->GetDescription(); + band.name = (desc && desc[0] != '\0') ? desc : ("band" + std::to_string(b)); + int has_nodata = 0; + const double nodata = raster->GetNoDataValue(&has_nodata); + band.has_nodata = has_nodata != 0; + band.nodata = static_cast(nodata); + band.data.resize(cells); + if(raster->RasterIO(GF_Read, 0, 0, width, height, band.data.data(), + width, height, GDT_Float32, 0, 0) != CE_None) + { + GDALClose(dataset); + return std::nullopt; + } + refoldRange(band); + tile.bands_[band.name] = std::move(band); + } + GDALClose(dataset); + return tile; +} + +std::vector SonarLiveTile::loadCacheDir(const std::string& dir, + const gggs::Level& level) +{ + std::vector tiles; + std::error_code ec; + if(!std::filesystem::is_directory(dir, ec)) + return tiles; + for(const auto& entry : std::filesystem::directory_iterator(dir, ec)) + { + if(ec) + break; + if(!entry.is_regular_file()) + continue; + if(entry.path().extension() != ".tif") + continue; // skip `.tif.tmp` crash-partials and non-tile files + if(auto tile = loadFromGeoTiff(entry.path().string(), level)) + tiles.push_back(std::move(*tile)); + } + return tiles; +} + +bool SonarLiveTile::writeToGeoTiff(const std::string& path) const +{ + if(bands_.empty() || width_ <= 0 || height_ <= 0) + return false; + if(GDALGetDriverCount() == 0) + GDALAllRegister(); + + GDALDriver* driver = GetGDALDriverManager()->GetDriverByName("GTiff"); + if(!driver) + return false; + + char** options = nullptr; + options = CSLSetNameValue(options, "COMPRESS", "LZW"); + GDALDataset* out = driver->Create(path.c_str(), width_, height_, + static_cast(bands_.size()), GDT_Float32, + options); + CSLDestroy(options); + if(!out) + return false; + + // North-up GGGS geotransform for this tile's grid (the GggsTile / saveTile + // convention). Memory is already north-up (row 0 = north), so bands write + // straight through with no flip. + const double west = index_.westLongitude(); + const double north = index_.northLatitude(); + const double pixel_x = index_.longitudinalSpan() / width_; + const double pixel_y = -index_.latitudinalSpan() / height_; + double geo[6] = {west, pixel_x, 0.0, north, 0.0, pixel_y}; + bool ok = out->SetGeoTransform(geo) == CE_None; + + OGRSpatialReference wgs84; + wgs84.SetWellKnownGeogCS("WGS84"); + char* wkt = nullptr; + if(ok && wgs84.exportToWkt(&wkt) == OGRERR_NONE && wkt) + ok = out->SetProjection(wkt) == CE_None; + else + ok = false; + CPLFree(wkt); + + int band_index = 1; + for(auto it = bands_.begin(); ok && it != bands_.end(); ++it, ++band_index) + { + const SonarLiveBand& band = it->second; + GDALRasterBand* raster = out->GetRasterBand(band_index); + raster->SetDescription(band.name.c_str()); + if(band.has_nodata) + raster->SetNoDataValue(static_cast(band.nodata)); + // Const buffer: GDAL's RasterIO takes a non-const void*, but GF_Write only + // reads from it. + std::vector scratch(band.data); + if(raster->RasterIO(GF_Write, 0, 0, width_, height_, scratch.data(), + width_, height_, GDT_Float32, 0, 0) != CE_None) + ok = false; + } + + if(GDALClose(out) != CE_None) + ok = false; + return ok; +} + +// -------------------------- node-boundary conversions -------------------------- + +marine_tiled_raster_store::TileVersion toNanoseconds( + const builtin_interfaces::msg::Time& time) +{ + return static_cast(time.sec) * 1000000000LL + + static_cast(time.nanosec); +} + +marine_tiled_raster_store::TileVersion toNanoseconds(const std_msgs::msg::Header& header) +{ + return toNanoseconds(header.stamp); +} + +gggs::GridIndex gridIndexFromTileIndex(const marine_interfaces::msg::TileIndex& index) +{ + if(index.level >= gggs::levels.size()) + return gggs::GridIndex(); // invalid sentinel + + const gggs::LevelSpecs& spec = gggs::levels[index.level]; + // Cell-center lat/lon for (row, col), then round-trip through Level::gridIndex + // (the GridIndex ctor is private; this is the only public construction path). + const double grid_span = spec.grid_angular_span; + const double south = -96.0 + static_cast(index.row) * grid_span; + const double center_lat = south + 0.5 * grid_span; + const double lon_span = spec.gridLongitudinalSpan(index.row); + const double west = -180.0 + static_cast(index.col) * lon_span; + const double center_lon = west + 0.5 * lon_span; + + try + { + const gggs::Level level(index.level); + const gggs::GridIndex recovered = level.gridIndex(center_lat, center_lon); + // Reject anything that doesn't round-trip exactly (e.g. an out-of-range row/ + // col that the truncating lookup would snap to a different real grid). + if(recovered.valid() && recovered.level() == index.level && + recovered.row() == index.row && recovered.column() == index.col) + return recovered; + } + catch(const std::exception&) + { + } + return gggs::GridIndex(); +} + +marine_interfaces::msg::TileIndex tileIndexFromGridIndex(const gggs::GridIndex& index) +{ + marine_interfaces::msg::TileIndex out; + out.level = index.level(); + out.row = index.row(); + out.col = index.column(); + return out; +} + +marine_tiled_raster_store::TileCatalog toReconcilerCatalog( + const marine_interfaces::msg::TileCatalog& catalog) +{ + marine_tiled_raster_store::TileCatalog out; + out.generation_time = toNanoseconds(catalog.header); + out.entries.reserve(catalog.entries.size()); + for(const auto& entry : catalog.entries) + { + const gggs::GridIndex index = gridIndexFromTileIndex(entry.index); + if(!index.valid()) + continue; // invalid indices are ignored at ingestion (reconciler contract) + marine_tiled_raster_store::TileCatalogEntry out_entry; + out_entry.index = index; + out_entry.version = toNanoseconds(entry.version); + out.entries.push_back(out_entry); + } + return out; +} + +} // namespace live_coverage +} // namespace ros +} // namespace camp diff --git a/src/camp_map/ros/live_coverage/sonar_live_tile.h b/src/camp_map/ros/live_coverage/sonar_live_tile.h new file mode 100644 index 0000000..2e84133 --- /dev/null +++ b/src/camp_map/ros/live_coverage/sonar_live_tile.h @@ -0,0 +1,152 @@ +#ifndef CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_TILE_H +#define CAMP_ROS_LIVE_COVERAGE_SONAR_LIVE_TILE_H + +#include +#include +#include +#include + +#include "marine_autonomy/gggs.h" +#include "marine_tiled_raster_store/tile_catalog.hpp" + +#include "builtin_interfaces/msg/time.hpp" +#include "std_msgs/msg/header.hpp" +#include "marine_interfaces/msg/sonar_visualization_tile.hpp" +#include "marine_interfaces/msg/tile_catalog.hpp" +#include "marine_interfaces/msg/tile_index.hpp" +#include "marine_interfaces/msg/visualization_band.hpp" + +namespace camp +{ +namespace ros +{ +namespace live_coverage +{ + +/// [camp#121] One dequantized band of a live coverage tile, held in memory as +/// Float32. The render path duplicates GggsTileLayer's, so the cell layout +/// matches GggsTile: row-major, **north-up** (row 0 = north), `width*height` +/// cells. NoData is the dequantized sentinel (`raw_nodata * scale + offset`); +/// the duplicated shader discards cells exactly equal to it, as GggsTile's does. +struct SonarLiveBand +{ + std::string name; ///< "depth" | "uncertainty" | "backscatter" | ... + std::vector data; ///< row-major north-up, width*height cells + float nodata = 0.0f; ///< dequantized NoData sentinel + bool has_nodata = false; + float data_min = 1.0f; ///< auto-range over finite non-NoData cells + float data_max = 0.0f; ///< crossed (min > max) => no valid samples +}; + +/// [camp#121] An in-memory GGGS tile carrying dequantized Float32 bands received +/// over the live coverage transport (uma ADR-0008). Mirrors GggsTile where the +/// duplicated render path needs it, but never uses GDAL on the hot path — patches +/// arrive as `SonarVisualizationTile` messages and are dequantized generically +/// (`value = raw * scale + offset`). Persistence (write-through / warm-load) goes +/// through GeoTIFF for crash-safety only; see ADR-0006. +/// +/// **Not thread-safe** (ADR-0001 / ADR-0006 D4): construct, applyPatch, and +/// read only on the GUI thread. +class SonarLiveTile +{ +public: + /// Empty tile for @p index sized @p width x @p height. Bands are created lazily + /// by applyPatch / warm-load. + SonarLiveTile(const gggs::GridIndex& index, int width, int height); + + /// [camp#121] Dequantize the message's dirty sub-window and patch it into each + /// named band (creating the band, NoData-filled, on first sight). Bumps the + /// tile version to `max(version, ns(header.stamp))` and re-folds each touched + /// band's auto-range. Cells outside any received window stay at the band's + /// NoData sentinel (so they discard in the shader). A band whose declared + /// window exceeds the tile, or whose byte length doesn't match its dtype, is + /// skipped rather than throwing. + void applyPatch(const marine_interfaces::msg::SonarVisualizationTile& msg); + + /// [camp#121] Warm-load one cached Float32 GeoTIFF (all bands, band names from + /// band descriptions, per-band NoData, GridIndex recovered from the + /// geotransform at @p level — the marine_tiled_raster_store::loadTile + /// convention). Returns std::nullopt if the file can't be opened, isn't a + /// WGS84 geographic raster, or doesn't match a grid at @p level. version() is 0 + /// (warm-loaded: "have something", older than any real catalog version). + static std::optional loadFromGeoTiff(const std::string& path, + const gggs::Level& level); + + /// [camp#121] Warm-load every `*.tif` under @p dir via loadFromGeoTiff(), + /// skipping any file that fails to load (e.g. a half-written `.tif.tmp` left by + /// a crash — those are named `.tif.tmp` and excluded by the glob anyway). A + /// missing directory yields an empty vector. Used by the layer at activation + /// and exercised directly by the headless test. + static std::vector loadCacheDir(const std::string& dir, + const gggs::Level& level); + + /// [camp#121] Serialize all bands to a Float32 GeoTIFF at @p path (north-up + /// WGS84, GGGS geotransform for index_, band description = band name, per-band + /// NoData). The caller does the atomic temp+rename; this just writes @p path. + /// Returns false on any GDAL failure (the cache write is best-effort). + bool writeToGeoTiff(const std::string& path) const; + + const gggs::GridIndex& index() const { return index_; } + int width() const { return width_; } + int height() const { return height_; } + + /// Geographic extent in degrees, from the GGGS grid (north-up, pixel-edge). + double minLon() const { return index_.westLongitude(); } + double maxLon() const { return index_.eastLongitude(); } + double minLat() const { return index_.southLatitude(); } + double maxLat() const { return index_.northLatitude(); } + + /// Band by name, or nullptr if this tile has no such band yet. + const SonarLiveBand* band(const std::string& name) const; + std::vector bandNames() const; + int bandCount() const { return static_cast(bands_.size()); } + + /// Per-tile version (ns since epoch; the latest applied/seeded version). + marine_tiled_raster_store::TileVersion version() const { return version_; } + +private: + /// Re-fold @p band's data_min/data_max over its finite, non-NoData cells. + static void refoldRange(SonarLiveBand& band); + + gggs::GridIndex index_; + int width_ = 0; + int height_ = 0; + std::map bands_; + marine_tiled_raster_store::TileVersion version_ = 0; +}; + +// ---- Node-boundary conversions (marine_interfaces wire <-> gggs/reconciler) ---- +// [camp#121] CAMP is the node boundary that adapts the ROS wire messages to the +// ROS-free reconciler/gggs types (tile_catalog.hpp). Kept as free functions so +// the test can exercise the downtime-gap scenario without a layer/node. + +/// Flatten a builtin_interfaces/Time to ns-since-epoch (the single TileVersion +/// representation; ADR-0006 D4). +marine_tiled_raster_store::TileVersion toNanoseconds( + const builtin_interfaces::msg::Time& time); + +/// Flatten a Header's stamp to ns-since-epoch. +marine_tiled_raster_store::TileVersion toNanoseconds( + const std_msgs::msg::Header& header); + +/// Wire TileIndex {level,row,col} -> gggs::GridIndex. The GridIndex ctor is +/// private; we reconstruct it the way loadTile does — map the cell center back +/// through gggs::Level — and verify the round-trip. Returns an invalid (sentinel) +/// GridIndex if level/row/col don't name a real grid (the reconciler ignores +/// invalid indices, matching its "invalid indices ignored at ingestion" rule). +gggs::GridIndex gridIndexFromTileIndex(const marine_interfaces::msg::TileIndex& index); + +/// gggs::GridIndex -> wire TileIndex {level,row,col}. +marine_interfaces::msg::TileIndex tileIndexFromGridIndex(const gggs::GridIndex& index); + +/// Adapt a wire TileCatalog to the reconciler's TileCatalog (indices flattened to +/// gggs::GridIndex, versions + generation_time flattened to ns). Invalid indices +/// are dropped. +marine_tiled_raster_store::TileCatalog toReconcilerCatalog( + const marine_interfaces::msg::TileCatalog& catalog); + +} // namespace live_coverage +} // namespace ros +} // namespace camp + +#endif diff --git a/src/camp_map/ros/node.cpp b/src/camp_map/ros/node.cpp index d7357b1..bb0e669 100644 --- a/src/camp_map/ros/node.cpp +++ b/src/camp_map/ros/node.cpp @@ -5,6 +5,7 @@ #include "../tools/tools_manager.h" #include "geometry/geometry_manager.h" #include "grids/grid_manager.h" +#include "live_coverage/sonar_live_cache_manager.h" #include "markers/markers_manager.h" #include "names_manager.h" #include "graph_thread.h" @@ -75,6 +76,7 @@ void Node::nodeStarted(rclcpp::Node::SharedPtr node, tf2_ros::Buffer::SharedPtr new markers::MarkersManager(this); new grids::GridManager(this); + new live_coverage::SonarLiveCacheManager(this); if(create_geometry_manager_) new geometry::GeometryManager(this); diff --git a/test/test_sonar_live_cache.cpp b/test/test_sonar_live_cache.cpp new file mode 100644 index 0000000..6f8d8dd --- /dev/null +++ b/test/test_sonar_live_cache.cpp @@ -0,0 +1,263 @@ +// [camp#121] Headless tests for the CAMP live tile cache (Part B). No Qt event +// loop, no ROS node, no boat: +// 1. warm-load round-trip (SonarLiveTile write-through -> loadCacheDir) +// 2. patch-apply / dequantize (SonarLiveTile::applyPatch, INT16 depth band) +// 3. downtime-gap reconcile (request the new tile, prune the gone one) +// 4. prune timestamp-gate (a held version newer than the catalog stays) +// +// Tests 3-4 exercise the node-boundary conversion (wire TileCatalog -> +// reconciler) plus marine_tiled_raster_store::TileCatalogReconciler. The GL render +// is NOT exercised here (it self-skips with no offscreen GL; see test_gggs_render). + +#include + +#include +#include +#include + +#include + +#include "marine_autonomy/gggs.h" +#include "marine_tiled_raster_store/tile_catalog.hpp" + +#include "builtin_interfaces/msg/time.hpp" +#include "marine_interfaces/msg/sonar_visualization_tile.hpp" +#include "marine_interfaces/msg/tile_catalog.hpp" +#include "marine_interfaces/msg/visualization_band.hpp" + +#include "ros/live_coverage/sonar_live_tile.h" + +namespace mi = marine_interfaces::msg; +namespace mtrs = marine_tiled_raster_store; +using camp::ros::live_coverage::SonarLiveTile; +using camp::ros::live_coverage::gridIndexFromTileIndex; +using camp::ros::live_coverage::tileIndexFromGridIndex; +using camp::ros::live_coverage::toNanoseconds; +using camp::ros::live_coverage::toReconcilerCatalog; + +namespace +{ + +constexpr int kLevel = 10; +constexpr int kEdge = 8; // small synthetic tile (loadFromGeoTiff has no kEdge gate) + +builtin_interfaces::msg::Time timeFromSec(int32_t sec) +{ + builtin_interfaces::msg::Time t; + t.sec = sec; + t.nanosec = 0; + return t; +} + +void putInt16LE(std::vector& bytes, int16_t value) +{ + bytes.push_back(static_cast(value & 0xff)); + bytes.push_back(static_cast((value >> 8) & 0xff)); +} + +// A full-tile INT16 "depth" patch for @p grid: every cell raw=base, with optional +// per-cell overrides keyed by (gggs_row, gggs_col). scale=0.01, nodata=-32768. +mi::SonarVisualizationTile makeDepthPatch( + const gggs::GridIndex& grid, int16_t base, int32_t stamp_sec, + const std::vector>& overrides = {}) +{ + mi::SonarVisualizationTile msg; + msg.header.stamp = timeFromSec(stamp_sec); + msg.header.frame_id = "gggs"; + msg.index = tileIndexFromGridIndex(grid); + msg.width = kEdge; + msg.height = kEdge; + msg.window_col = 0; + msg.window_row = 0; + msg.window_width = kEdge; + msg.window_height = kEdge; + + mi::VisualizationBand band; + band.name = "depth"; + band.dtype = mi::VisualizationBand::INT16; + band.scale = 0.01; + band.offset = 0.0; + band.nodata = -32768.0; + band.data.reserve(static_cast(kEdge) * kEdge * 2); + for(int r = 0; r < kEdge; ++r) + for(int c = 0; c < kEdge; ++c) + { + int16_t raw = base; + for(const auto& o : overrides) + if(std::get<0>(o) == r && std::get<1>(o) == c) + raw = std::get<2>(o); + putInt16LE(band.data, raw); + } + msg.bands.push_back(band); + return msg; +} + +// North-up cell index for a GGGS (row, col) in a kEdge x kEdge tile. +size_t northUpIndex(int gggs_row, int col) +{ + return static_cast((kEdge - 1) - gggs_row) * kEdge + col; +} + +bool contains(const std::vector& v, const gggs::GridIndex& g) +{ + return std::find(v.begin(), v.end(), g) != v.end(); +} + +} // namespace + +// 2. Patch-apply / dequantize. A full-tile INT16 depth patch dequantizes +// value = raw * scale, the NoData sentinel is excluded from the auto-range and +// lands flipped to north-up at the right cell. +TEST(SonarLiveCache, PatchApplyDequantize) +{ + const gggs::Level level(kLevel); + const gggs::GridIndex grid = level.gridIndex(43.07, -70.76); + ASSERT_TRUE(grid.valid()); + + SonarLiveTile tile(grid, kEdge, kEdge); + // base raw 200 -> 2.0; one peak raw 500 -> 5.0 at (gggs 0,0); one NoData at (1,1). + tile.applyPatch(makeDepthPatch(grid, 200, 100, + {{0, 0, 500}, {1, 1, -32768}})); + + const auto* band = tile.band("depth"); + ASSERT_NE(band, nullptr); + ASSERT_EQ(band->data.size(), static_cast(kEdge) * kEdge); + + // Dequantized values at the flipped (north-up) positions. + EXPECT_FLOAT_EQ(band->data[northUpIndex(2, 2)], 2.0f); // a plain base cell + EXPECT_FLOAT_EQ(band->data[northUpIndex(0, 0)], 5.0f); // the peak + + // NoData cell: stored as the dequantized sentinel and excluded from the range. + EXPECT_FLOAT_EQ(band->data[northUpIndex(1, 1)], -327.68f); + EXPECT_TRUE(band->has_nodata); + EXPECT_FLOAT_EQ(band->data_min, 2.0f); + EXPECT_FLOAT_EQ(band->data_max, 5.0f); + + // The tile version is the patch stamp flattened to ns. + EXPECT_EQ(tile.version(), toNanoseconds(timeFromSec(100))); +} + +// 1. Warm-load round-trip: write tiles through (Float32 GeoTIFF), then warm-load +// the directory and recover the tile count + data range. +TEST(SonarLiveCache, WarmLoadRoundTrip) +{ + QTemporaryDir dir; + ASSERT_TRUE(dir.isValid()); + const gggs::Level level(kLevel); + + // Two distinct grids (well over a level-10 grid span apart). + const gggs::GridIndex a = level.gridIndex(43.07, -70.76); + const gggs::GridIndex b = level.gridIndex(43.50, -70.20); + ASSERT_TRUE(a.valid()); + ASSERT_TRUE(b.valid()); + ASSERT_FALSE(a == b); + + for(const auto& grid : {a, b}) + { + SonarLiveTile tile(grid, kEdge, kEdge); + tile.applyPatch(makeDepthPatch(grid, 300, 100, {{0, 0, 700}})); // 3.0 .. 7.0 + const std::string path = + dir.filePath(QString("%1_%2_%3.tif") + .arg(static_cast(grid.level())) + .arg(grid.row()) + .arg(grid.column())).toStdString(); + ASSERT_TRUE(tile.writeToGeoTiff(path)); + } + + std::vector loaded = + SonarLiveTile::loadCacheDir(dir.path().toStdString(), level); + ASSERT_EQ(loaded.size(), 2u); + + for(const auto& tile : loaded) + { + const auto* band = tile.band("depth"); + ASSERT_NE(band, nullptr); + EXPECT_FLOAT_EQ(band->data_min, 3.0f); + EXPECT_FLOAT_EQ(band->data_max, 7.0f); + // The GridIndex round-tripped through the geotransform. + EXPECT_TRUE(tile.index() == a || tile.index() == b); + } +} + +// 3. Downtime-gap reconcile (the acceptance scenario). Before downtime the cache +// holds A,B,C; the boat (which was up while CAMP was down) now advertises A,B,D. +// Reconcile must request D (newly present) and prune C (gone), then converge. +TEST(SonarLiveCache, DowntimeGapReconcile) +{ + const gggs::Level level(kLevel); + const gggs::GridIndex a = level.gridIndex(43.07, -70.76); + const gggs::GridIndex b = level.gridIndex(43.50, -70.20); + const gggs::GridIndex c = level.gridIndex(42.60, -71.30); + const gggs::GridIndex d = level.gridIndex(44.10, -69.50); + ASSERT_TRUE(a.valid() && b.valid() && c.valid() && d.valid()); + + mtrs::TileCatalogReconciler reconciler; + const mtrs::TileVersion v_old = toNanoseconds(timeFromSec(100)); + reconciler.markHave(a, v_old); + reconciler.markHave(b, v_old); + reconciler.markHave(c, v_old); + + // Catalog generation-time newer than the held versions, so prune-on-absence + // applies to C. + mi::TileCatalog catalog; + catalog.header.stamp = timeFromSec(200); + for(const auto& grid : {a, b, d}) + { + mi::TileCatalogEntry entry; + entry.index = tileIndexFromGridIndex(grid); + entry.version = timeFromSec(100); + catalog.entries.push_back(entry); + } + + const mtrs::ReconcileResult result = reconciler.reconcile(toReconcilerCatalog(catalog)); + + EXPECT_EQ(result.to_request.size(), 1u); + EXPECT_TRUE(contains(result.to_request, d)); + EXPECT_FALSE(contains(result.to_request, a)); + EXPECT_FALSE(contains(result.to_request, b)); + + EXPECT_EQ(result.to_prune.size(), 1u); + EXPECT_TRUE(contains(result.to_prune, c)); + + // Model the consumer acting on the result, then a second identical catalog must + // converge to no work. + reconciler.markHave(d, v_old); + reconciler.drop(c); + const mtrs::ReconcileResult after = reconciler.reconcile(toReconcilerCatalog(catalog)); + EXPECT_TRUE(after.to_request.empty()); + EXPECT_TRUE(after.to_prune.empty()); +} + +// 4. Prune timestamp-gate (ADR-0006 D4 / uma ADR-0008 D4b): a held tile whose +// version is NEWER than the catalog generation-time is NOT pruned even when absent +// — a late/reordered catalog cannot delete a just-pushed fresh tile. +TEST(SonarLiveCache, PruneTimestampGate) +{ + const gggs::Level level(kLevel); + const gggs::GridIndex a = level.gridIndex(43.07, -70.76); + const gggs::GridIndex b = level.gridIndex(43.50, -70.20); + const gggs::GridIndex c = level.gridIndex(42.60, -71.30); + ASSERT_TRUE(a.valid() && b.valid() && c.valid()); + + mtrs::TileCatalogReconciler reconciler; + reconciler.markHave(a, toNanoseconds(timeFromSec(100))); + reconciler.markHave(b, toNanoseconds(timeFromSec(100))); + // C is fresher than the catalog we're about to receive. + reconciler.markHave(c, toNanoseconds(timeFromSec(300))); + + // Catalog at sec=200 lists only A,B — C is absent but newer than generation_time. + mi::TileCatalog catalog; + catalog.header.stamp = timeFromSec(200); + for(const auto& grid : {a, b}) + { + mi::TileCatalogEntry entry; + entry.index = tileIndexFromGridIndex(grid); + entry.version = timeFromSec(100); + catalog.entries.push_back(entry); + } + + const mtrs::ReconcileResult result = reconciler.reconcile(toReconcilerCatalog(catalog)); + EXPECT_FALSE(contains(result.to_prune, c)); // protected by the timestamp gate + EXPECT_TRUE(result.to_prune.empty()); + EXPECT_TRUE(result.to_request.empty()); // A,B held at current version +} From 89136d44bde250c0fe4a11b74d03e3285be1fc11 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 19:15:39 +0000 Subject: [PATCH 6/9] progress: local review (Round 1) for #121 --- .agent/work-plans/issue-121/progress.md | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index 552c547..ac77592 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -104,3 +104,25 @@ and minor convention/version-semantics points. ### Notes / deviations - New TUs live in `ros/live_coverage/` (not `raster/` as the plan first listed): they depend on `marine_interfaces` + `marine_tiled_raster_store`, so they belong in `camp_map_ros`; `camp_map` stays ROS-free (ADR-0002 / CMake layering). Recorded in ADR-0006 + plan. - Warm-load reads the multi-band GeoTIFF directly via GDAL (recovering band names from descriptions + GridIndex from the geotransform, the `marine_tiled_raster_store::loadTile` convention) rather than literally calling the single-band `GggsTile::loadPixels()`, since write-through-all-bands needs all bands + names in one pass. + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-06-28 19:14 +00:00 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: changes-requested + +**Branch**: feature/issue-121 at `470d2c9` +**Mode**: pre-push +**Depth**: Deep (reason: ~1700 LOC new C++ + new ADR + concurrency/file-I/O across the ROS↔GUI boundary) +**Must-fix**: 1 | **Suggestions**: 2 +**Round**: 1 | **Ship**: continue — one genuine concurrency-correctness must-fix (same-tile write-through race) warrants the fix + a re-read + +### Findings +- [ ] (must-fix) Write-through launches uncoalesced concurrent `QtConcurrent` workers that race on a shared per-tile `.tif.tmp` (every patch calls `setFuture` unconditionally; deviates from the `GggsTileLayer` isRunning-guard / `GridMap` coalesce convention; defeats atomic temp+rename; dtor joins only the latest future) — `src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp:380-384` +- [ ] (suggestion) Stale auto-range after prune: prune path runs `recomputeBounds`+`foldAutoRange` but not `resetAutoRange`, so a pruned tile's extreme lingers in `data_min_/data_max_` — `src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp:365-371` +- [ ] (suggestion) Reset `tile_sub_`/`catalog_sub_` at the top of the dtor (before `waitForFinished`) for teardown symmetry / to close the executor-thread TOCTOU window — `src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp:131-137` + +### Notes +- Governance clean: ADR-0006 well-formed (Deep ADR-add trigger); ADR-0001/0002/0005 + #117/#71 all Pass; consequence updates (`package.xml`, `CMakeLists.txt`, `item_types.h`, `node.cpp`) all done. Plan-review must-fixes resolved. +- Unbounded in-memory/on-disk tile growth is a consciously deferred, ADR-0006-documented, TODO'd follow-up (eviction-by-area) — acceptable for a display-grade preview cache; not counted as a finding. +- Static analysis: camp configures no `ament_lint`/`cpplint`; build + 129 tests reported clean at implementation. New files match camp conventions. No new lint findings. From 2115dfd80429456e62a7dff0971967033acbe57b Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 19:25:55 +0000 Subject: [PATCH 7/9] fix(camp#121): coalesce+join per-tile write-through; reset auto-range on prune; dtor sub teardown --- .../live_coverage/sonar_live_cache_layer.cpp | 71 ++++++++++++++++--- .../live_coverage/sonar_live_cache_layer.h | 32 +++++++-- 2 files changed, 89 insertions(+), 14 deletions(-) diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp index 2b639a0..87a509d 100644 --- a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp @@ -130,9 +130,18 @@ SonarLiveCacheLayer::SonarLiveCacheLayer(MapItem* parent, Node* node, SonarLiveCacheLayer::~SonarLiveCacheLayer() { - // Join any in-flight write-through (each worker is self-contained, but joining - // matches the camp_map worker-lifetime convention). Then tear down GL. - write_watcher_.waitForFinished(); + // Reset the subscriptions FIRST so no executor-thread callback can marshal a new + // handler onto this object mid-teardown (closes the TOCTOU window; matches the + // teardown-symmetry of the other ROS layers). + tile_sub_.reset(); + catalog_sub_.reset(); + + // Stop coalesced relaunches, then JOIN every in-flight write worker (not just the + // latest) so none can outlive the object. Each worker is self-contained, so this + // is purely a lifetime join. Then tear down GL. + shutting_down_ = true; + for(QFutureWatcher* watcher : write_watchers_) + watcher->waitForFinished(); releaseGL(); } @@ -364,7 +373,11 @@ void SonarLiveCacheLayer::handleCatalog(const marine_interfaces::msg::TileCatalo } if(pruned) { + // Reset before re-folding so a pruned tile's extreme min/max can't linger in + // data_min_/data_max_ — the range reflects only the surviving tiles (mirrors + // the band-switch reset in setBandName()). recomputeBounds(); + resetAutoRange(); foldAutoRange(); cached_image_ = QImage(); update(boundingRect()); @@ -372,15 +385,53 @@ void SonarLiveCacheLayer::handleCatalog(const marine_interfaces::msg::TileCatalo updateDisplay(); } -void SonarLiveCacheLayer::writeThroughFinished() -{ - // Hook kept for symmetry; the worker is self-contained, so nothing to fold. -} - void SonarLiveCacheLayer::scheduleWriteThrough(const SonarLiveTile& tile) { - // Copy the tile + dir by value into a self-contained worker (no `this` deref). - write_watcher_.setFuture(QtConcurrent::run(writeTileToCache, tile, cache_dir_)); + // GUI thread. Coalesce per tile: snapshot the latest state and launch a worker + // only if this tile is idle. If a worker is already in flight for this tile, mark + // it dirty — onWriteThroughFinished() re-launches once with the latest snapshot, + // so two workers never race on the shared .tif.tmp path (the prior bug), + // and intermediate patches are coalesced into a single follow-up write. + WriteState& ws = write_states_[tile.index()]; + ws.pending = tile; // latest wins + ws.dirty = true; + if(!ws.in_flight) + startWriteThrough(tile.index()); +} + +void SonarLiveCacheLayer::startWriteThrough(const gggs::GridIndex& index) +{ + // GUI thread. Move the latest snapshot into a self-contained worker (no `this` + // deref) and track its watcher so the dtor can join it. `pending` is repopulated + // by the next scheduleWriteThrough() before it is read again, so moving is safe. + WriteState& ws = write_states_[index]; + ws.in_flight = true; + ws.dirty = false; + auto* watcher = new QFutureWatcher(this); + write_watchers_.push_back(watcher); + connect(watcher, &QFutureWatcher::finished, this, + [this, index, watcher]() { onWriteThroughFinished(index, watcher); }); + watcher->setFuture(QtConcurrent::run(writeTileToCache, std::move(*ws.pending), cache_dir_)); +} + +void SonarLiveCacheLayer::onWriteThroughFinished(const gggs::GridIndex& index, + QFutureWatcher* watcher) +{ + // GUI thread. Untrack + delete the finished worker's watcher. + write_watchers_.erase( + std::remove(write_watchers_.begin(), write_watchers_.end(), watcher), + write_watchers_.end()); + watcher->deleteLater(); + if(shutting_down_) + return; + auto it = write_states_.find(index); + if(it == write_states_.end()) + return; + it->second.in_flight = false; + if(it->second.dirty) + startWriteThrough(index); // coalesced: write the newest patch that arrived + else + write_states_.erase(it); // clean: drop the per-tile state } // ------------------------------ extent / range ------------------------------- diff --git a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h index ed96fd8..dc67775 100644 --- a/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h +++ b/src/camp_map/ros/live_coverage/sonar_live_cache_layer.h @@ -107,8 +107,6 @@ private slots: void handleTile(const marine_interfaces::msg::SonarVisualizationTile& msg); /// GUI thread: a catalog arrived (reconcile -> request + prune). void handleCatalog(const marine_interfaces::msg::TileCatalog& msg); - /// GUI thread, after a write-through worker finishes (currently a no-op hook). - void writeThroughFinished(); private: // [camp#121] One held tile plus its lazily-(re)uploaded GL texture for the @@ -126,7 +124,17 @@ private slots: void unsubscribeTiles(); void publishRequest(const std::vector& tiles); void warmLoad(); + // [camp#121] Coalesced, joinable per-tile write-through. scheduleWriteThrough() + // snapshots the latest tile state and either launches a worker (if the tile is + // idle) or marks it dirty (if a worker is already in flight for that tile); + // startWriteThrough() moves the snapshot into a self-contained worker; + // onWriteThroughFinished() (GUI thread) re-launches once more if a newer patch + // arrived during the write — so writes for a tile are serialized and never race + // on the shared .tif.tmp path. void scheduleWriteThrough(const SonarLiveTile& tile); + void startWriteThrough(const gggs::GridIndex& index); + void onWriteThroughFinished(const gggs::GridIndex& index, + QFutureWatcher* watcher); void recomputeBounds(); void resetAutoRange(); @@ -172,8 +180,24 @@ private slots: rclcpp::Subscription::SharedPtr catalog_sub_; rclcpp::Publisher::SharedPtr request_pub_; - // Write-through runs off the GUI thread; keep them tracked so the dtor joins. - QFutureWatcher write_watcher_; + // [camp#121] Per-tile write-through state (GUI thread only). `in_flight` marks a + // worker is serializing this tile; `dirty` marks a newer patch arrived during + // that write (coalesced into one follow-up); `pending` holds the latest snapshot + // to write (moved into the worker on launch). + struct WriteState + { + bool in_flight = false; + bool dirty = false; + std::optional pending; // SonarLiveTile has no default ctor + }; + std::map write_states_; + + // Every in-flight write worker is tracked here so the dtor joins ALL of them (not + // just the latest). Workers are self-contained (file serialization + rename only); + // they never touch this object. `shutting_down_` suppresses coalesced relaunches + // once teardown starts. + std::vector*> write_watchers_; + bool shutting_down_ = false; // Offscreen GL (the layer owns it; never touches the GUI context). Duplicated // from GggsTileLayer — unify via RasterFieldSource camp#134. From 58bc66ca6f8e568b77db8543b16df75fa56d82b6 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 19:26:29 +0000 Subject: [PATCH 8/9] progress: implementation (address pre-push findings) for #121 --- .agent/work-plans/issue-121/progress.md | 68 +++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index ac77592..add243e 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -126,3 +126,71 @@ and minor convention/version-semantics points. - Governance clean: ADR-0006 well-formed (Deep ADR-add trigger); ADR-0001/0002/0005 + #117/#71 all Pass; consequence updates (`package.xml`, `CMakeLists.txt`, `item_types.h`, `node.cpp`) all done. Plan-review must-fixes resolved. - Unbounded in-memory/on-disk tile growth is a consciously deferred, ADR-0006-documented, TODO'd follow-up (eviction-by-area) — acceptable for a display-grade preview cache; not counted as a finding. - Static analysis: camp configures no `ament_lint`/`cpplint`; build + 129 tests reported clean at implementation. New files match camp conventions. No new lint findings. + +## Implementation +**Status**: complete +**When**: 2026-06-28 20:00 +00:00 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: addressed all 3 Local Review (Pre-Push) Round 1 findings + +**Branch**: feature/issue-121 at `2115dfd` +**Scope**: address-findings — the one must-fix + two suggestions from the pre-push review. + +### 1. (must-fix) Coalesced + joinable per-tile write-through — `sonar_live_cache_layer.{h,cpp}` +Replaced the single `QFutureWatcher write_watcher_` (which `setFuture`'d +unconditionally on every patch — two patches for the SAME tile raced on the shared +`.tif.tmp`, and the dtor joined only the latest future) with per-tile coalescing +that matches the `GridMap` coalesce / `GggsTileLayer` guard convention: +- New per-tile `WriteState{ bool in_flight; bool dirty; std::optional pending; }` + kept in `std::map write_states_` (GUI thread only). + `pending` is `std::optional` because `SonarLiveTile` has no default ctor. +- `scheduleWriteThrough(tile)` snapshots the latest tile into `pending` and sets `dirty`; + it launches a worker via `startWriteThrough()` **only if the tile is idle**. If a worker + is already in flight for that tile, it just marks `dirty` — no second concurrent worker. +- `startWriteThrough(index)` sets `in_flight`, clears `dirty`, and **moves** the snapshot + into a self-contained `QtConcurrent::run(writeTileToCache, …)` worker (no `this` deref). + Each launch gets its own `QFutureWatcher` (child of the layer) tracked in + `write_watchers_`. +- `onWriteThroughFinished(index, watcher)` (GUI thread, via the watcher's queued + `finished()`) untracks + `deleteLater`s the watcher; if the tile is `dirty` it re-launches + ONCE with the newest snapshot (coalescing intermediate patches), else drops the per-tile + state. So writes for a tile are strictly serialized — the atomic temp+rename is now safe + with the existing shared tmp path (no unique suffix needed; distinct tiles already use + distinct `__` stems). +- Dtor now sets `shutting_down_` (suppresses coalesced relaunches) then **joins EVERY** + tracked watcher (`waitForFinished()` on each, not just the latest) before `releaseGL()`, + so no worker can outlive the object. All reconciler/tile/state mutation stays on the GUI + thread; only file serialization+rename runs in the worker (invariant preserved). +- Removed the now-dead `writeThroughFinished()` no-op slot. + +### 2. (suggestion) Reset auto-range on prune — `sonar_live_cache_layer.cpp` handleCatalog +The `if(pruned)` block now calls `resetAutoRange()` between `recomputeBounds()` and +`foldAutoRange()`, mirroring the band-switch reset in `setBandName()`, so a pruned tile's +extreme min/max can no longer linger in `data_min_`/`data_max_`; the range reflects only the +surviving tiles. + +### 3. (suggestion) Reset subscriptions first in the dtor — `sonar_live_cache_layer.cpp` +The dtor now `.reset()`s `tile_sub_` and `catalog_sub_` at the TOP (before the worker join), +so no executor-thread callback can marshal a new handler onto the object mid-teardown +(closes the TOCTOU window), matching the teardown-symmetry of the other ROS layers. + +### Test note +No new automated test added: the existing `test_sonar_live_cache` suite deliberately exercises +`SonarLiveTile`/reconciler at unit level and never constructs `SonarLiveCacheLayer` (which +needs a ROS `Node`, a Qt event loop, and GL). The coalescing lives in private layer methods +driven by `QFutureWatcher` signals that require an event loop, so a faithful write-through +test is not cheap — skipped per the finding's own guidance ("otherwise don't force it"). + +### Build + test (verbatim) +- Deps first: `cd core_ws && colcon build --packages-up-to marine_tiled_raster_store + marine_ais_msgs marine_nav_interfaces marine_nav_tasks` → `Summary: 7 packages finished + [1min 10s]` (only pre-existing `marine_autonomy` warnings). +- `./ui_ws/build.sh camp` → `Summary: 1 package finished [1min 2s]` (clean; only pre-existing + camp warnings). +- `./ui_ws/test.sh camp` → `Summary: 129 tests, 0 errors, 0 failures, 4 skipped` (the 4 skips + are the offscreen-GL render tests that skip in-container by design). +- `test_sonar_live_cache` → `[ PASSED ] 4 tests.` (PatchApplyDequantize, WarmLoadRoundTrip, + DowntimeGapReconcile, PruneTimestampGate). + +### Next step +Re-read by the reviewer (Round 2). Do NOT push — host performs pushes. From 7801de5c014449ae21ecaa97ccdd51154d48b3a0 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 19:36:56 +0000 Subject: [PATCH 9/9] progress: local review (Round 2, approved) for #121 --- .agent/work-plans/issue-121/progress.md | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/.agent/work-plans/issue-121/progress.md b/.agent/work-plans/issue-121/progress.md index add243e..94a35ae 100644 --- a/.agent/work-plans/issue-121/progress.md +++ b/.agent/work-plans/issue-121/progress.md @@ -194,3 +194,27 @@ test is not cheap — skipped per the finding's own guidance ("otherwise don't f ### Next step Re-read by the reviewer (Round 2). Do NOT push — host performs pushes. + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-06-28 19:35 +00:00 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: approved + +**Branch**: feature/issue-121 at `58bc66c` +**Mode**: pre-push +**Depth**: Deep (reason: ~1750 LOC new C++ + new ADR + off-thread file-I/O across the ROS↔GUI boundary) +**Must-fix**: 0 | **Suggestions**: 1 +**Round**: 2 | **Ship**: recommended — Round-1 must-fix + 2 suggestions all correctly resolved; both Deep adversarial passes produced only verified false-positives; only an optional display-grade suggestion remains + +### Findings +- [ ] (suggestion) Update-path auto-range only ever widens (foldAutoRange without resetAutoRange in handleTile), so a live tile's extreme revised to a less-extreme value leaves the colormap range too wide until band-switch/prune — display-grade, self-heals, optional — `src/camp_map/ros/live_coverage/sonar_live_cache_layer.cpp:332,469-481` + +### Notes +- Round-1 resolution verified: (must-fix) per-tile coalesced+joinable write-through (`WriteState`/`write_states_`/`write_watchers_`) serializes writes and joins EVERY watcher in the dtor — traced for move/coalesce correctness, no UB, no dropped final write; (suggestion) prune now resetAutoRange between recompute/fold; (suggestion) dtor resets subs first. +- Adversarial adjudication (all rejected): Lens-B "dangling-this from queued finished()/invokeMethod after dtor" is a false positive — QObject's dtor removes pending posted events + disconnects, no nested event loop in the dtor, and the merged sibling `GggsTileLayer::~GggsTileLayer()` uses the identical waitForFinished-without-disconnect pattern. Lens-B "loadFromGeoTiff half-loaded on RasterIO failure" misread the code (it returns nullopt, sonar_live_tile.cpp:247-252). ctor-throw leak + best-effort fs::remove ec are below threshold. Lens A clean. +- Governance clean: ADR-0001/0002/0005/0006 + #71/#117 all Pass; consequence updates (CMakeLists/package.xml/item_types.h/node.cpp/GDAL link) all done. Plan adherence matches the as-amended plan (opt-in model, ros/live_coverage/ placement). +- Static analysis: camp configures no ament_lint/cpplint; build + 129 tests + 4 cache tests reported clean at implementation. New files match camp conventions. + +### Next step +Lifecycle: **Local Review (approved)** → push / open PR → **triage-reviews**. Do NOT push — host performs pushes.