From cb87fcafe461cb81a9629f561ff7c843e939684e Mon Sep 17 00:00:00 2001 From: Roland Arsenault Date: Tue, 21 Apr 2026 23:01:13 -0400 Subject: [PATCH 1/2] feat(s57_layer): parameterize tide invalidation threshold MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Make the previously-hardcoded 0.01 m threshold a parameter (tide_invalidate_threshold, default 0.01) so sim configs can override without affecting field defaults. Field operation (real tide ~0.5 m/hr near peak): 1 cm step fires invalidation roughly every 72 s — keep default. Sim runs with tide_speed_factor=10 fire ~10× as often (every ~8 s) which causes costmap stalls and planner_server timeouts (deployment-log analysis of deadpool sim run 2026-04-21T22:03:55 showed 15 tide updates in 125 s correlating with 7 "Costmap timed out" planner aborts and a "Control loop missed its desired rate" warning). Companion change in rolker/ben_project11 will set 0.05 m in ben_sim.yaml (sim-only, gated by is_simulator) which restores roughly real-time-equivalent invalidation cadence. Existing TideOffsetTest suite (3/3) passes unchanged: defaults preserved, threshold semantics identical for the existing test inputs (0.005 m below threshold; 1 m and 2.5 m above threshold). Closes #17 --- Authored-By: Claude Code Agent Model: Claude Opus 4.7 (1M context) Co-Authored-By: Claude Opus 4.7 (1M context) --- s57_layer/src/s57_layer.cpp | 8 ++++++-- s57_layer/src/s57_layer.h | 6 ++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp index fe08693..38e0fb0 100644 --- a/s57_layer/src/s57_layer.cpp +++ b/s57_layer/src/s57_layer.cpp @@ -63,9 +63,13 @@ void S57Layer::onInitialize() declareParameter("sea_surface_frame", rclcpp::ParameterValue(sea_surface_frame_)); node->get_parameter(name_+".sea_surface_frame", sea_surface_frame_); + declareParameter("tide_invalidate_threshold", rclcpp::ParameterValue(tide_invalidate_threshold_)); + node->get_parameter(name_+".tide_invalidate_threshold", tide_invalidate_threshold_); + if(!chart_datum_frame_.empty() && !sea_surface_frame_.empty()) RCLCPP_INFO_STREAM(logger_, "Tide correction enabled: sea surface height in chart datum frame (" - << sea_surface_frame_ << " expressed in " << chart_datum_frame_ << ")"); + << sea_surface_frame_ << " expressed in " << chart_datum_frame_ + << "), invalidate threshold " << tide_invalidate_threshold_ << " m"); declareParameter("s57_grids_namespace", rclcpp::ParameterValue(s57_grids_namespace_)); node->get_parameter(name_+".s57_grids_namespace", s57_grids_namespace_); @@ -135,7 +139,7 @@ void S57Layer::updateBounds(double, double, double, double* min_x, double* min_y { auto transform = tf_->lookupTransform(chart_datum_frame_, sea_surface_frame_, tf2::TimePointZero); double new_offset = transform.transform.translation.z; - if(std::abs(new_offset - tide_offset_) > 0.01) + if(std::abs(new_offset - tide_offset_) > tide_invalidate_threshold_) { tide_offset_ = new_offset; RCLCPP_INFO_STREAM(logger_, "Tide offset updated: " << tide_offset_ << " m (water above chart datum)"); diff --git a/s57_layer/src/s57_layer.h b/s57_layer/src/s57_layer.h index cd86a4a..49f7856 100644 --- a/s57_layer/src/s57_layer.h +++ b/s57_layer/src/s57_layer.h @@ -108,6 +108,12 @@ class S57Layer: public nav2_costmap_2d::Layer 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 + // (~0.5 m/hr near peak ⇒ 1 cm step every ~72 s). Sim runs with + // accelerated tide should override to a larger value. + double tide_invalidate_threshold_ = 0.01; + typedef std::pair TileID; struct TileInfo From 4dff6eb61f242a300ab97e2be10629f32e2fcbfb Mon Sep 17 00:00:00 2001 From: Roland Arsenault Date: Tue, 21 Apr 2026 23:28:52 -0400 Subject: [PATCH 2/2] fix(s57_layer): validate tide_invalidate_threshold + add test coverage MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two follow-ups from Copilot review on PR #18: 1. Validate tide_invalidate_threshold after get_parameter. ROS 2 parameters from YAML are external input and warrant validation at that boundary. A negative configured value would make every tide change exceed the threshold (continuous invalidation = costmap stalls — exactly what this PR is meant to prevent). A non-finite value would silently disable invalidation. Reject both cases with RCLCPP_WARN and fall back to the default. 2. Add test coverage for the new parameter: - CustomThresholdAcceptsLargerChanges: declares chart_layer.tide_invalidate_threshold=0.05, exercises a 3 cm change (between default 0.01 and configured 0.05 — must NOT invalidate), then a 6 cm change (above 0.05 — must invalidate), then verifies recovery after updateCosts. - InvalidThresholdFallsBackToDefault: declares -1.0 (negative), exercises a 0.005 m change which would invalidate if validation was skipped but stays current with the restored 0.01 m default. s57_layer test suite now 23 tests (was 21), 0 failures, 0 errors. TideOffsetTest suite now 5/5 passing. Refs: rolker/s57_tools#17 --- Authored-By: Claude Code Agent Model: Claude Opus 4.7 (1M context) Co-Authored-By: Claude Opus 4.7 (1M context) --- s57_layer/src/s57_layer.cpp | 11 ++++ s57_layer/test/test_tide_offset.cpp | 86 +++++++++++++++++++++++++++++ 2 files changed, 97 insertions(+) diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp index 38e0fb0..5efd666 100644 --- a/s57_layer/src/s57_layer.cpp +++ b/s57_layer/src/s57_layer.cpp @@ -63,8 +63,19 @@ void S57Layer::onInitialize() declareParameter("sea_surface_frame", rclcpp::ParameterValue(sea_surface_frame_)); node->get_parameter(name_+".sea_surface_frame", sea_surface_frame_); + const double default_tide_invalidate_threshold = tide_invalidate_threshold_; declareParameter("tide_invalidate_threshold", rclcpp::ParameterValue(tide_invalidate_threshold_)); node->get_parameter(name_+".tide_invalidate_threshold", tide_invalidate_threshold_); + // ROS 2 parameters are external input — validate. Negative would make + // every tide change exceed the threshold (continuous invalidation = + // costmap stalls); non-finite would silently disable invalidation. + if(!std::isfinite(tide_invalidate_threshold_) || tide_invalidate_threshold_ < 0.0) + { + RCLCPP_WARN_STREAM(logger_, + "Invalid tide_invalidate_threshold value " << tide_invalidate_threshold_ + << " m; using default " << default_tide_invalidate_threshold << " m instead."); + tide_invalidate_threshold_ = default_tide_invalidate_threshold; + } if(!chart_datum_frame_.empty() && !sea_surface_frame_.empty()) RCLCPP_INFO_STREAM(logger_, "Tide correction enabled: sea surface height in chart datum frame (" diff --git a/s57_layer/test/test_tide_offset.cpp b/s57_layer/test/test_tide_offset.cpp index 394161b..089db0d 100644 --- a/s57_layer/test/test_tide_offset.cpp +++ b/s57_layer/test/test_tide_offset.cpp @@ -163,3 +163,89 @@ TEST_F(TideOffsetTest, SmallTideChangeIgnored) // Should still be current — change too small EXPECT_TRUE(layer->isCurrent()); } + +// A configured tide_invalidate_threshold should override the default, +// allowing larger tide changes (between default and override) to pass +// without invalidating cached tiles. Used by sim configs where the +// accelerated tide_speed_factor would otherwise fire invalidations +// every few seconds. +TEST_F(TideOffsetTest, CustomThresholdAcceptsLargerChanges) +{ + auto node = std::make_shared("test_custom_threshold"); + + node->declare_parameter("chart_layer.chart_datum_frame", std::string("chart_datum")); + node->declare_parameter("chart_layer.sea_surface_frame", std::string("map_tide")); + node->declare_parameter("chart_layer.tide_invalidate_threshold", 0.05); + + auto tf_buffer = std::make_shared(node->get_clock()); + + publishTideTransforms(tf_buffer, -30.0, -29.0); + + nav2_costmap_2d::LayeredCostmap layered_costmap("map", false, false); + layered_costmap.resizeMap(10, 10, 1.0, 0.0, 0.0); + + auto * master = layered_costmap.getCostmap(); + + auto layer = std::make_shared(); + layer->initialize(&layered_costmap, "chart_layer", tf_buffer.get(), node, nullptr); + + double minx = 1e30, miny = 1e30, maxx = -1e30, maxy = -1e30; + layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); + layer->updateCosts(*master, 0, 0, 10, 10); + EXPECT_TRUE(layer->isCurrent()); + + // 3 cm change — above the default 0.01 m but below the configured + // 0.05 m — must NOT invalidate. + publishTideTransforms(tf_buffer, -30.0, -28.97); + minx = 1e30; miny = 1e30; maxx = -1e30; maxy = -1e30; + layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); + EXPECT_TRUE(layer->isCurrent()); + + // 6 cm change — above the configured 0.05 m — must invalidate. + publishTideTransforms(tf_buffer, -30.0, -28.94); + minx = 1e30; miny = 1e30; maxx = -1e30; maxy = -1e30; + layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); + EXPECT_FALSE(layer->isCurrent()); + + // After updateCosts, regen completes and tiles are current again. + layer->updateCosts(*master, 0, 0, 10, 10); + EXPECT_TRUE(layer->isCurrent()); +} + +// Invalid threshold values (negative, NaN, inf) should be rejected at +// onInitialize and the default restored. +TEST_F(TideOffsetTest, InvalidThresholdFallsBackToDefault) +{ + auto node = std::make_shared("test_invalid_threshold"); + + node->declare_parameter("chart_layer.chart_datum_frame", std::string("chart_datum")); + node->declare_parameter("chart_layer.sea_surface_frame", std::string("map_tide")); + // Negative threshold: would otherwise make every tide change pass the + // (std::abs(...) > threshold) test and invalidate continuously. + node->declare_parameter("chart_layer.tide_invalidate_threshold", -1.0); + + auto tf_buffer = std::make_shared(node->get_clock()); + + publishTideTransforms(tf_buffer, -30.0, -29.0); + + nav2_costmap_2d::LayeredCostmap layered_costmap("map", false, false); + layered_costmap.resizeMap(10, 10, 1.0, 0.0, 0.0); + + auto * master = layered_costmap.getCostmap(); + + auto layer = std::make_shared(); + layer->initialize(&layered_costmap, "chart_layer", tf_buffer.get(), node, nullptr); + + double minx = 1e30, miny = 1e30, maxx = -1e30, maxy = -1e30; + layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); + layer->updateCosts(*master, 0, 0, 10, 10); + EXPECT_TRUE(layer->isCurrent()); + + // 0.005 m change — below the restored default 0.01 m — must NOT + // invalidate. If validation was skipped, the negative threshold would + // cause invalidation here. + publishTideTransforms(tf_buffer, -30.0, -28.995); + minx = 1e30; miny = 1e30; maxx = -1e30; maxy = -1e30; + layer->updateBounds(5.0, 5.0, 0.0, &minx, &miny, &maxx, &maxy); + EXPECT_TRUE(layer->isCurrent()); +}