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..7c8ecb2 100644 --- a/s57_layer/src/s57_layer.h +++ b/s57_layer/src/s57_layer.h @@ -39,12 +39,22 @@ 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 089db0d..c396bd1 100644 --- a/s57_layer/test/test_tide_offset.cpp +++ b/s57_layer/test/test_tide_offset.cpp @@ -2,10 +2,12 @@ // Licensed under BSD license #include +#include #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 +17,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 +266,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."; +}