From e8e4ec0a5cba45391bf4f0394dc254167ef5cd57 Mon Sep 17 00:00:00 2001 From: Roland Arsenault Date: Sat, 25 Apr 2026 07:35:42 -0400 Subject: [PATCH 1/2] Short-circuit elevation>0 to LETHAL in get_cost_from_grid MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Land / lethal S57 features (LNDARE, COALNE, CAUSWY, HULKES, PONTON, SLCONS, FSHFAC, GATCON, OBSTRN, PILPNT, PYLONS, DAMCON, DYKCON) are rasterized into the elevation channel with a constant 1.0 in marine_charts/src/s57_dataset.cpp, with the explicit comment "no min depth values, so lethal". That encoding worked while tide_offset_ was always 0 (depth = -1 + 0 = -1 < minimum_depth_ = 0 → LETHAL), but breaks the moment the chart_datum_frame / sea_surface_frame parameters are set: as soon as tide_offset_ exceeds ~minimum_depth_ + 1.0, the depth math (-elevation + tide_offset_) makes every coastline cell report a positive depth and become FREE / CAUTION. In Portsmouth field bag 2026-04-24T17.39.30 we observed tide_offset_ ramping smoothly from 1.67 m to 2.0 m during normal in-water operation — i.e., for the entire run, every land polygon in the S57 chart was navigable in the costmap. This is what was meant by "costmap no longer looks correct now that we are accounting for tides" (rolker/s57_tools#21). Fix: short-circuit elevation > 0 to LETHAL_OBSTACLE before applying the tide-offset depth math. Anything above chart datum is shoreline / intertidal / built feature and should be lethal regardless of tide. The depth math then runs only for genuinely submerged cells, where it makes sense. Tests: - LandFeatureLethalAcrossAllTides — sweeps tide_offset across [-1, 0, 0.5, 1, 2, 3, 5, 7] m for an elevation = 1.0 cell and asserts LETHAL at every level. Would fail prior to this fix at any tide >= 1.0 m. - SubmergedCellUsesTideAdjustedDepth — guards the existing depth = -elevation + tide_offset_ behavior for negative-elevation cells: 5 m-deep at -1 m tide is not lethal, at +2 m tide it is FREE_SPACE; a 1 m-deep cell drained to -0.5 m at low tide is lethal. Visibility change: get_cost_from_grid moved from private to protected so the test fixture can subclass S57Layer. No external API change. Closes #21 --- **Authored-By**: \`Claude Code Agent\` **Model**: \`Claude Opus 4.7 (1M context)\` --- s57_layer/src/s57_layer.cpp | 9 +++ s57_layer/src/s57_layer.h | 2 +- s57_layer/test/test_tide_offset.cpp | 91 +++++++++++++++++++++++++++++ 3 files changed, 101 insertions(+), 1 deletion(-) diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp index 63798c3..0f0da7a 100644 --- a/s57_layer/src/s57_layer.cpp +++ b/s57_layer/src/s57_layer.cpp @@ -505,6 +505,15 @@ unsigned char S57Layer::get_cost_from_grid(grid_map::GridMap &grid, const grid_m auto elevation = grid.at("elevation", index); if(!std::isnan(elevation)) { + // Anything above chart datum is shoreline / intertidal / built feature + // (LNDARE, COALNE, CAUSWY, HULKES, PONTON, SLCONS, … rasterized as + // elevation = 1.0 in marine_charts/s57_dataset.cpp; see "no min depth + // values, so lethal" comment there). Without this short-circuit the + // tide-offset math below would convert positive elevations into + // "navigable depth" once tide_offset_ exceeds elevation, silently + // turning every coastline navigable at any tide above ~1 m above MLLW. + if (elevation > 0.0) + return nav2_costmap_2d::LETHAL_OBSTACLE; auto depth = -elevation + tide_offset_; if (depth < minimum_depth_) return nav2_costmap_2d::LETHAL_OBSTACLE; diff --git a/s57_layer/src/s57_layer.h b/s57_layer/src/s57_layer.h index 7277b97..08ad505 100644 --- a/s57_layer/src/s57_layer.h +++ b/s57_layer/src/s57_layer.h @@ -39,7 +39,7 @@ class S57Layer: public nav2_costmap_2d::Layer void matchSize() override; -private: +protected: using GetDatasetsClient = rclcpp::Client; diff --git a/s57_layer/test/test_tide_offset.cpp b/s57_layer/test/test_tide_offset.cpp index 089db0d..888d5a4 100644 --- a/s57_layer/test/test_tide_offset.cpp +++ b/s57_layer/test/test_tide_offset.cpp @@ -6,6 +6,7 @@ #include #include +#include "grid_map_core/GridMap.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/cost_values.hpp" @@ -15,6 +16,21 @@ #include "s57_layer.h" +// Test subclass exposing protected get_cost_from_grid and tide_offset_ +// for direct unit testing of the elevation-cost-mapping logic without +// going through the full updateBounds/updateCosts pipeline. +class S57LayerForTest : public s57_layer::S57Layer +{ +public: + using s57_layer::S57Layer::S57Layer; + unsigned char testGetCost(grid_map::GridMap & grid, + const grid_map::Index & index) + { + return get_cost_from_grid(grid, index); + } + void setTideOffset(double v) { tide_offset_ = v; } +}; + class TideOffsetTest : public ::testing::Test { protected: @@ -249,3 +265,78 @@ TEST_F(TideOffsetTest, InvalidThresholdFallsBackToDefault) layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); EXPECT_TRUE(layer->isCurrent()); } + +// Helper: build a 1x1 grid_map with all S57 channels initialized to NaN. +// Caller fills in only the channel(s) they want to test. +static grid_map::GridMap makeEmptyS57Grid() +{ + grid_map::GridMap grid({"elevation", "overhead", "restricted", + "unsurveyed", "caution"}); + grid.setGeometry(grid_map::Length(1.0, 1.0), 1.0, + grid_map::Position(0.0, 0.0)); + for (const auto & name : grid.getLayers()) { + grid[name].setConstant(std::numeric_limits::quiet_NaN()); + } + return grid; +} + +// Land features (LNDARE, COALNE, CAUSWY, HULKES, PONTON, SLCONS, …) are +// rasterized as elevation = 1.0 with the intent "no min depth values, so +// lethal" (marine_charts/s57_dataset.cpp). Before the elevation>0 +// short-circuit, any tide_offset_ above ~minimum_depth_ + 1.0 would +// convert these cells into "navigable depth" via -elevation + tide_offset_, +// silently turning every coastline navigable at any tide above ~1 m above +// MLLW. Verify land cells stay LETHAL across realistic tide values. +TEST_F(TideOffsetTest, LandFeatureLethalAcrossAllTides) +{ + S57LayerForTest layer; + + auto grid = makeEmptyS57Grid(); + grid_map::Index idx(0, 0); + grid.at("elevation", idx) = 1.0; // canonical land marker + + for (double tide : {-1.0, 0.0, 0.5, 1.0, 2.0, 3.0, 5.0, 7.0}) { + layer.setTideOffset(tide); + EXPECT_EQ( + layer.testGetCost(grid, idx), + nav2_costmap_2d::LETHAL_OBSTACLE) + << "Land cell (elevation=1.0) must be LETHAL at tide_offset=" + << tide << " m, but was not."; + } +} + +// Genuinely-submerged cells (negative elevation) must continue to use +// the depth = -elevation + tide_offset math. Verify the existing +// behavior is preserved by the short-circuit fix: a 5 m-deep cell at +// MLLW (elevation = -5) stays navigable through the full tidal cycle, +// and goes lethal only when minimum_depth is not satisfied. +TEST_F(TideOffsetTest, SubmergedCellUsesTideAdjustedDepth) +{ + S57LayerForTest layer; + + auto grid = makeEmptyS57Grid(); + grid_map::Index idx(0, 0); + + // 5 m deep at chart datum (DRVAL1=5 → elevation=-5) + grid.at("elevation", idx) = -5.0; + + // Default minimum_depth_ = 0.0, maximum_caution_depth_ = 5.0. + // depth = 5 + tide_offset. At tide_offset = -1 m (below MLLW), + // depth = 4 m (still inside caution band, not lethal). At + // tide_offset = 2 m, depth = 7 m (above caution → FREE_SPACE). + layer.setTideOffset(-1.0); + EXPECT_NE(layer.testGetCost(grid, idx), + nav2_costmap_2d::LETHAL_OBSTACLE) + << "5m-deep cell at -1m tide must not be LETHAL."; + layer.setTideOffset(2.0); + EXPECT_EQ(layer.testGetCost(grid, idx), + nav2_costmap_2d::FREE_SPACE) + << "5m-deep cell at +2m tide should be FREE_SPACE."; + + // A 1 m-deep cell drained dry at low tide must go lethal. + grid.at("elevation", idx) = -1.0; + layer.setTideOffset(-1.5); // 1.5 m below MLLW + EXPECT_EQ(layer.testGetCost(grid, idx), + nav2_costmap_2d::LETHAL_OBSTACLE) + << "Cell with depth = -0.5m (drained) must be LETHAL."; +} From 1946d8fd7747d4a33f771e796218ad2971aeeddb Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sat, 25 Apr 2026 08:08:00 -0400 Subject: [PATCH 2/2] Address Copilot review on PR #22 Two findings from Copilot review of e8e4ec0: - test_tide_offset.cpp uses std::numeric_limits::quiet_NaN() but did not include ; it worked via transitive includes. Add the explicit include. - s57_layer.h previously moved the entire private: block to protected:, exposing all internal helpers (worldToLatLon, llToWorld, callbacks) and member state (subscription / tile cache / thresholds, etc.) to any subclass. The tests only need get_cost_from_grid and tide_offset_. Tighten the protected surface to just those two; the rest goes back to private. No external API change. --- **Authored-By**: \`Claude Code Agent\` **Model**: \`Claude Opus 4.7 (1M context)\` --- s57_layer/src/s57_layer.h | 17 ++++++++++++++--- s57_layer/test/test_tide_offset.cpp | 1 + 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/s57_layer/src/s57_layer.h b/s57_layer/src/s57_layer.h index 08ad505..7c8ecb2 100644 --- a/s57_layer/src/s57_layer.h +++ b/s57_layer/src/s57_layer.h @@ -40,11 +40,21 @@ class S57Layer: public nav2_costmap_2d::Layer void matchSize() override; protected: + // Exposed for unit testing (see test/test_tide_offset.cpp). + // Subclassed-test access only; not part of the layer's public API. + unsigned char get_cost_from_grid(grid_map::GridMap &grid, const grid_map::Index &index); + + // Same as above — declared protected so the test fixture can set + // tide_offset_ directly instead of driving it through the full TF + // lookup path in updateBounds(). Moved out of the private block + // below; the rest of the layer's internal helpers and state stay + // private. + double tide_offset_ = 0.0; + +private: using GetDatasetsClient = rclcpp::Client; - unsigned char get_cost_from_grid(grid_map::GridMap &grid, const grid_map::Index &index); - geographic_msgs::msg::GeoPoint worldToLatLon(double x, double y); geometry_msgs::msg::Point llToWorld(const geographic_msgs::msg::GeoPoint& geo_point); @@ -113,9 +123,10 @@ class S57Layer: public nav2_costmap_2d::Layer // lookupTransform(chart_datum_frame, sea_surface_frame) // to get the sea surface position expressed in the chart datum frame. // The Z component is the water height above MLLW (chart datum). + // (tide_offset_ itself is declared in the protected section above so + // the test fixture can set it directly.) std::string chart_datum_frame_; std::string sea_surface_frame_ = "map_tide"; - double tide_offset_ = 0.0; // Tide changes smaller than this (in meters) do not invalidate cached // tile state. The default 1 cm matches typical real-world tide rate diff --git a/s57_layer/test/test_tide_offset.cpp b/s57_layer/test/test_tide_offset.cpp index 888d5a4..c396bd1 100644 --- a/s57_layer/test/test_tide_offset.cpp +++ b/s57_layer/test/test_tide_offset.cpp @@ -2,6 +2,7 @@ // Licensed under BSD license #include +#include #include #include