diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp index fe08693..5efd666 100644 --- a/s57_layer/src/s57_layer.cpp +++ b/s57_layer/src/s57_layer.cpp @@ -63,9 +63,24 @@ 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 (" - << 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 +150,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 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()); +}