From 92c7e64dcf07136308bc120c288675ff3983bb88 Mon Sep 17 00:00:00 2001
From: Roland Arsenault <roland@ccom.unh.edu>
Date: Wed, 22 Apr 2026 01:00:29 -0400
Subject: [PATCH 1/3] =?UTF-8?q?feat(s57):=20cold-cache=20mitigation=20?=
 =?UTF-8?q?=E2=80=94=20faster=20polling,=20timing=20logs,=20precompute,=20?=
 =?UTF-8?q?buffer=20param?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Four bundled changes to reduce S57 chart cold-cache latency for the
costmap stack. None of them couple s57_layer to mission_manager / BT
publishing patterns; all use signals already available to the costmap
layer (TF, costmap window, lifecycle).

s57_grids changes:

1. check_new_grids_period parameter, default 0.1 s (was hardcoded
   1.0 s). Reaps completed std::async futures sooner, removing up to
   1 s per chart from publish latency. Verified via per-chart timing
   log: US5PSMBC published 0.78 s after request (would have been
   0.78–1.78 s with old 1 s timer).

2. Per-chart timing logs at INFO level. Records wall-clock time from
   first request to publish. Format:
     "Grid ready: <label> (X.X s from request to publish)"
   Sim run on deadpool 2026-04-22 showed the slowest two charts
   (US3EC10M, US2EC04M) take 11–13 s — pinpoints S57Dataset::getGrid
   as the dominant cost component for wide-area East Coast charts,
   not the polling loop.

3. Precompute on first available pose. New parameters:
     robot_base_frame  — when set, polls TF (map_frame, this) until
                         lookup succeeds, then queues all charts
                         within precompute_radius of that position.
                         Default empty = disabled.
     precompute_radius — meters, default 5000.0 (matches typical
                         global-costmap window with buffer).
   Trigger uses TF only; no goal subscription. Single-shot — sets
   did_precompute_ on success and stops trying. Equirectangular
   approximation for the bounding box (good to <1% for ~50 km radii
   outside polar regions). Queues via the existing requested_grids_
   path so cached grids are published latched and any S57Layer that
   subscribes later (including post-precompute) gets them.

s57_layer change:

4. buffer_fraction parameter, default 0.05 (preserves the prior
   hardcoded value). Configs that want a wider pre-load ring around
   the rolling costmap window can override without code changes.
   Negative values are rejected with a warning and fall back to 0.05.

Existing TideOffsetTest suite (5/5) passes unchanged. Build clean.

Closes #19
Related: rolker/unh_marine_navigation#19

---
Authored-By: Claude Code Agent
Model: Claude Opus 4.7 (1M context)

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
---
 s57_grids/src/grid_publisher.cpp | 137 ++++++++++++++++++++++++++++++-
 s57_grids/src/grid_publisher.h   |  21 +++++
 s57_layer/src/s57_layer.cpp      |  14 +++-
 s57_layer/src/s57_layer.h        |   9 ++
 4 files changed, 177 insertions(+), 4 deletions(-)

diff --git a/s57_grids/src/grid_publisher.cpp b/s57_grids/src/grid_publisher.cpp
index f08ebaf..4598b4b 100644
--- a/s57_grids/src/grid_publisher.cpp
+++ b/s57_grids/src/grid_publisher.cpp
@@ -4,6 +4,7 @@
 #include "geometry_msgs/msg/point_stamped.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include <cmath>
 #include <thread>
 
 namespace s57_grids
@@ -62,6 +63,24 @@ GridPublisher::on_configure(const rclcpp_lifecycle::State &state)
   }
   publish_costmaps_ = get_parameter("publish_costmaps").as_bool();
 
+  if(!has_parameter("check_new_grids_period"))
+  {
+    declare_parameter("check_new_grids_period", rclcpp::ParameterValue(check_new_grids_period_));
+  }
+  check_new_grids_period_ = get_parameter("check_new_grids_period").as_double();
+
+  if(!has_parameter("robot_base_frame"))
+  {
+    declare_parameter("robot_base_frame", rclcpp::ParameterValue(robot_base_frame_));
+  }
+  robot_base_frame_ = get_parameter("robot_base_frame").as_string();
+
+  if(!has_parameter("precompute_radius"))
+  {
+    declare_parameter("precompute_radius", rclcpp::ParameterValue(precompute_radius_));
+  }
+  precompute_radius_ = get_parameter("precompute_radius").as_double();
+
   list_service_ = create_service<s57_msgs::srv::GetDatasets>(
     "list_datasets",
     std::bind(&GridPublisher::listDatasets, this, std::placeholders::_1, std::placeholders::_2)
@@ -76,10 +95,16 @@ GridPublisher::on_configure(const rclcpp_lifecycle::State &state)
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
   new_grids_timer_ = create_wall_timer(
-    std::chrono::milliseconds(1000),
+    std::chrono::milliseconds(int(1000.0 * check_new_grids_period_)),
     std::bind(&GridPublisher::checkForNewGrids, this)
   );
 
+  if(!robot_base_frame_.empty() && precompute_radius_ > 0.0)
+  {
+    RCLCPP_INFO_STREAM(get_logger(),
+      "Precompute enabled: will queue charts within " << precompute_radius_
+      << " m of " << robot_base_frame_ << " when TF first becomes available.");
+  }
 
   return LifecycleNode::on_configure(state);
 
@@ -139,15 +164,27 @@ void GridPublisher::getDatasets(
 {
   listDatasets(request, response);
   std::lock_guard<std::mutex> lock(requested_grids_mutex);
+  auto now = get_clock()->now();
   for(const auto& d: response->datasets)
   {
     requested_grids_.push_back(d.label);
     requested_grids_to_publish_.push_back(d.label);
+    // Only record start time on the first request for this label, so
+    // re-requests don't reset the measurement.
+    grid_request_start_times_.emplace(d.label, now);
   }
 }
 
 void GridPublisher::checkForNewGrids()
 {
+  // Try a one-shot eager-precompute on the first tick where the boat's
+  // pose is known. Cheap when disabled (empty robot_base_frame_) or
+  // already done.
+  if(!did_precompute_ && !robot_base_frame_.empty() && precompute_radius_ > 0.0)
+  {
+    tryPrecompute();
+  }
+
   {
     std::lock_guard<std::mutex> lock(requested_grids_mutex);
     for(auto r: requested_grids_)
@@ -189,7 +226,20 @@ void GridPublisher::checkForNewGrids()
             grid_publishers_[pg.first]->on_activate();
 
             auto message = grid_map::GridMapRosConverter::toMessage(*grid);
-            RCLCPP_DEBUG_STREAM(get_logger(), "Publishing grid to " << "datasets/" << pg.first);
+            // Report time from first request to publish for this chart.
+            // Useful for diagnosing cold-cache behaviour without recompiling.
+            auto start_it = grid_request_start_times_.find(pg.first);
+            if(start_it != grid_request_start_times_.end())
+            {
+              auto elapsed = (get_clock()->now() - start_it->second).seconds();
+              RCLCPP_INFO_STREAM(get_logger(),
+                "Grid ready: " << pg.first << " (" << elapsed << " s from request to publish)");
+              grid_request_start_times_.erase(start_it);
+            }
+            else
+            {
+              RCLCPP_INFO_STREAM(get_logger(), "Grid ready: " << pg.first);
+            }
             grid_publishers_[pg.first]->publish(*message);
 
             if(publish_costmaps_)
@@ -220,6 +270,89 @@ void GridPublisher::checkForNewGrids()
 
 }
 
+void GridPublisher::tryPrecompute()
+{
+  // Look up the boat's current pose in the map frame. If not yet
+  // available (TF tree still bootstrapping), silently retry next tick.
+  geometry_msgs::msg::TransformStamped pose_in_map;
+  try
+  {
+    pose_in_map = tf_buffer_->lookupTransform(
+      map_frame_, robot_base_frame_, tf2::TimePointZero);
+  }
+  catch(const tf2::TransformException&)
+  {
+    return;
+  }
+
+  // Convert (x, y, z) from map frame to ECEF, then to lat/lon. The
+  // catalog needs lat/lon to find intersecting charts.
+  geometry_msgs::msg::PointStamped map_pt;
+  map_pt.header = pose_in_map.header;
+  map_pt.point.x = pose_in_map.transform.translation.x;
+  map_pt.point.y = pose_in_map.transform.translation.y;
+  map_pt.point.z = pose_in_map.transform.translation.z;
+  geometry_msgs::msg::PointStamped ecef_pt;
+  try
+  {
+    tf_buffer_->transform(map_pt, ecef_pt, "earth");
+  }
+  catch(const tf2::TransformException&)
+  {
+    return;
+  }
+  double lat = 0.0, lon = 0.0;
+  if(!catalog_->ecefToLatLong(ecef_pt.point.x, ecef_pt.point.y, ecef_pt.point.z, lat, lon))
+    return;
+
+  // Equirectangular approximation: 1° lat ≈ 111 000 m everywhere;
+  // 1° lon ≈ 111 000 m × cos(lat). Good to <1% for radii up to ~50 km
+  // outside polar regions.
+  const double meters_per_deg_lat = 111000.0;
+  const double meters_per_deg_lon = 111000.0 * std::cos(lat * M_PI / 180.0);
+  geographic_msgs::msg::BoundingBox bounds;
+  bounds.min_pt.latitude  = lat - precompute_radius_ / meters_per_deg_lat;
+  bounds.max_pt.latitude  = lat + precompute_radius_ / meters_per_deg_lat;
+  bounds.min_pt.longitude = lon - precompute_radius_ / meters_per_deg_lon;
+  bounds.max_pt.longitude = lon + precompute_radius_ / meters_per_deg_lon;
+
+  auto charts = catalog_->intersectingCharts(bounds);
+  if(charts.empty())
+  {
+    RCLCPP_WARN_STREAM(get_logger(),
+      "Precompute: no charts intersect " << precompute_radius_ << " m radius around ("
+      << lat << ", " << lon << ").");
+    did_precompute_ = true;
+    return;
+  }
+
+  // Queue the chart labels via the same path getDatasets uses (both
+  // requested_grids_ to trigger generation and requested_grids_to_publish_
+  // so the latched topic message gets published when ready — late-arriving
+  // S57Layer subscribers see the cached message thanks to transient_local
+  // QoS). Labels already in flight (tracked via grid_request_start_times_)
+  // are skipped to avoid double-queuing.
+  std::lock_guard<std::mutex> lock(requested_grids_mutex);
+  auto now = get_clock()->now();
+  size_t queued = 0;
+  for(const auto& chart: charts)
+  {
+    const std::string& label = chart->label();
+    if(grid_request_start_times_.count(label) == 0)
+    {
+      requested_grids_.push_back(label);
+      requested_grids_to_publish_.push_back(label);
+      grid_request_start_times_.emplace(label, now);
+      ++queued;
+    }
+  }
+  RCLCPP_INFO_STREAM(get_logger(),
+    "Precompute: queued " << queued << " of " << charts.size()
+    << " charts within " << precompute_radius_ << " m of ("
+    << lat << ", " << lon << ").");
+  did_precompute_ = true;
+}
+
 void GridPublisher::republishGrids()
 {
   RCLCPP_INFO_STREAM(get_logger(), "Republishing grids");
diff --git a/s57_grids/src/grid_publisher.h b/s57_grids/src/grid_publisher.h
index 197af81..e540c21 100644
--- a/s57_grids/src/grid_publisher.h
+++ b/s57_grids/src/grid_publisher.h
@@ -58,6 +58,12 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode
   // Called periodically to republish existing grids with fresh timestamps.
   void republishGrids();
 
+  // Called once after the boat's pose first becomes known via TF, queues
+  // all charts within precompute_radius_ around that position so the
+  // costmap layer's first get_datasets request finds them already cached
+  // (or close to ready).
+  void tryPrecompute();
+
   std::shared_ptr<marine_charts::S57Catalog> catalog_;
   rclcpp::Service<s57_msgs::srv::GetDatasets>::SharedPtr list_service_;
   rclcpp::Service<s57_msgs::srv::GetDatasets>::SharedPtr get_service_;
@@ -77,8 +83,15 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode
   // Futures waiting for datasets being generated in separate threads
   std::map<std::string, std::future<std::shared_ptr<grid_map::GridMap> > > pending_dataset_grids_;
 
+  // Wall-clock time each chart was first added to requested_grids_ — used
+  // to log per-chart processing latency when the grid is published.
+  std::map<std::string, rclcpp::Time> grid_request_start_times_;
 
   rclcpp::TimerBase::SharedPtr new_grids_timer_;
+  // Period for new_grids_timer_ in seconds. Smaller values reap completed
+  // std::async futures sooner; the previous hardcoded 1.0 s added up to
+  // 1 s of latency per chart.
+  double check_new_grids_period_ = 0.1;
 
   rclcpp::TimerBase::SharedPtr republish_grids_timer_;
   double grid_republish_period_ = 0.0; // seconds, if <= 0.0, no republishing
@@ -91,6 +104,14 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   std::string map_frame_ = "map";
 
+  // Precompute-on-first-TF: when robot_base_frame_ is set and
+  // precompute_radius_ > 0, tryPrecompute polls TF until the boat's
+  // pose is known, then queues all charts within precompute_radius_
+  // (meters) of that position.
+  std::string robot_base_frame_;
+  double precompute_radius_ = 5000.0;
+  bool did_precompute_ = false;
+
   std::atomic<bool> abort_flag_ = false;
 };
 
diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp
index 5efd666..fd2f5eb 100644
--- a/s57_layer/src/s57_layer.cpp
+++ b/s57_layer/src/s57_layer.cpp
@@ -54,6 +54,15 @@ void S57Layer::onInitialize()
   declareParameter("tile_size", rclcpp::ParameterValue(tile_size_));
   node->get_parameter(name_+".tile_size", tile_size_);
 
+  declareParameter("buffer_fraction", rclcpp::ParameterValue(buffer_fraction_));
+  node->get_parameter(name_+".buffer_fraction", buffer_fraction_);
+  if(buffer_fraction_ < 0.0)
+  {
+    RCLCPP_WARN_STREAM(logger_,
+      "Invalid buffer_fraction value " << buffer_fraction_ << "; using 0.05 instead.");
+    buffer_fraction_ = 0.05;
+  }
+
   declareParameter("allow_uncharted", rclcpp::ParameterValue(allow_uncharted_));
   node->get_parameter(name_+".allow_uncharted", allow_uncharted_);
 
@@ -186,8 +195,9 @@ void S57Layer::updateBounds(double, double, double, double* min_x, double* min_y
   world_min_y = parent->getOriginY();
   world_max_y = world_min_y + parent->getSizeInMetersY();
 
-  // 5% buffer around the world bounds
-  double buffer = std::max(world_max_x-world_min_x, world_max_y-world_min_y)*0.05;
+  // Buffer around the world bounds, controlled by buffer_fraction_
+  // (default 0.05 = 5%). Larger values pre-load more chart area.
+  double buffer = std::max(world_max_x-world_min_x, world_max_y-world_min_y)*buffer_fraction_;
 
   // use two layers of buffer. inner buffer to make sure data is available
   // and an outer buffer to limit the number of service calls to the get data service
diff --git a/s57_layer/src/s57_layer.h b/s57_layer/src/s57_layer.h
index 49f7856..7277b97 100644
--- a/s57_layer/src/s57_layer.h
+++ b/s57_layer/src/s57_layer.h
@@ -95,6 +95,15 @@ class S57Layer: public nav2_costmap_2d::Layer
 
   double update_timeout_ = 0.5;
 
+  // Fraction of the costmap window size to extend the buffered chart
+  // request bounds. The inner ring (1×) acts as a pre-load buffer so
+  // chart data is ready before the rolling window scrolls into it; the
+  // outer ring (2×) limits how often a new get_datasets service call
+  // is sent. A larger value pre-loads more area at the cost of more
+  // memory and processing per request. The 0.05 default preserves the
+  // historical hardcoded value.
+  double buffer_fraction_ = 0.05;
+
   // When true, allow navigation in areas without ENC chart coverage.
   // When false, uncharted areas are marked NO_INFORMATION to block planning.
   bool allow_uncharted_ = true;

From 8517b12cb965965b0eabe48a55771ad7027f0dfb Mon Sep 17 00:00:00 2001
From: Claude Code Agent <roland+claude-code@ccom.unh.edu>
Date: Wed, 22 Apr 2026 01:53:58 -0400
Subject: [PATCH 2/3] =?UTF-8?q?fix(s57):=20address=20Copilot=20review=20on?=
 =?UTF-8?q?=20PR=20#20=20=E2=80=94=20parameter=20validation,=20dateline/po?=
 =?UTF-8?q?le=20handling,=20bounded=20request=20set?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Copilot flagged six issues; all are valid for global deployment.

s57_layer:
- buffer_fraction: validate with isfinite (was negative-only) and fall
  back to the captured default instead of hardcoding 0.05. Matches the
  established tide_invalidate_threshold pattern in this file.

s57_grids:
- check_new_grids_period: validate (isfinite + > 0); clamp the wall-timer
  duration to >= 1 ms so a misconfigured value can never produce a
  zero-duration timer.
- precompute_radius: validate (isfinite + >= 0).
- tryPrecompute: clamp latitude to [-90, 90]; near the poles
  (cos(lat) ≈ 0) the equirectangular lon span becomes ill-defined, so
  fall back to a full longitude sweep. When the radius wraps the
  dateline, split the query into two bounding boxes and dedupe results
  by chart label — S57Dataset::intersects assumes minLon <= maxLon and
  does not handle wrap.
- requested_grids_to_publish_: convert from std::vector to
  std::unordered_set and erase entries after publish. The vector was
  append-only and queried with linear std::find, which would grow
  unbounded across long missions.

Tested in worktree (./core_ws/build.sh + ./core_ws/test.sh): 41 tests, 0
failures. Field verification of the precompute path on PR #20 still
applies; these changes don't alter the NH-coast code path, only the
behaviour at high latitudes / on the dateline.
---
 s57_grids/src/grid_publisher.cpp | 120 ++++++++++++++++++++++++++++---
 s57_grids/src/grid_publisher.h   |   6 +-
 s57_layer/src/s57_layer.cpp      |   8 ++-
 3 files changed, 120 insertions(+), 14 deletions(-)

diff --git a/s57_grids/src/grid_publisher.cpp b/s57_grids/src/grid_publisher.cpp
index 4598b4b..c783ba6 100644
--- a/s57_grids/src/grid_publisher.cpp
+++ b/s57_grids/src/grid_publisher.cpp
@@ -6,6 +6,7 @@
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include <cmath>
 #include <thread>
+#include <unordered_set>
 
 namespace s57_grids
 {
@@ -63,11 +64,19 @@ GridPublisher::on_configure(const rclcpp_lifecycle::State &state)
   }
   publish_costmaps_ = get_parameter("publish_costmaps").as_bool();
 
+  const double default_check_new_grids_period = check_new_grids_period_;
   if(!has_parameter("check_new_grids_period"))
   {
     declare_parameter("check_new_grids_period", rclcpp::ParameterValue(check_new_grids_period_));
   }
   check_new_grids_period_ = get_parameter("check_new_grids_period").as_double();
+  if(!std::isfinite(check_new_grids_period_) || check_new_grids_period_ <= 0.0)
+  {
+    RCLCPP_WARN_STREAM(get_logger(),
+      "Invalid check_new_grids_period value " << check_new_grids_period_
+      << " s; using " << default_check_new_grids_period << " s instead.");
+    check_new_grids_period_ = default_check_new_grids_period;
+  }
 
   if(!has_parameter("robot_base_frame"))
   {
@@ -75,11 +84,19 @@ GridPublisher::on_configure(const rclcpp_lifecycle::State &state)
   }
   robot_base_frame_ = get_parameter("robot_base_frame").as_string();
 
+  const double default_precompute_radius = precompute_radius_;
   if(!has_parameter("precompute_radius"))
   {
     declare_parameter("precompute_radius", rclcpp::ParameterValue(precompute_radius_));
   }
   precompute_radius_ = get_parameter("precompute_radius").as_double();
+  if(!std::isfinite(precompute_radius_) || precompute_radius_ < 0.0)
+  {
+    RCLCPP_WARN_STREAM(get_logger(),
+      "Invalid precompute_radius value " << precompute_radius_
+      << " m; using " << default_precompute_radius << " m instead.");
+    precompute_radius_ = default_precompute_radius;
+  }
 
   list_service_ = create_service<s57_msgs::srv::GetDatasets>(
     "list_datasets",
@@ -94,8 +111,11 @@ GridPublisher::on_configure(const rclcpp_lifecycle::State &state)
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
+  // Clamp to >= 1 ms — a zero-duration wall timer is undefined behaviour
+  // and would also pin a CPU.
+  const int new_grids_timer_ms = std::max(1, int(1000.0 * check_new_grids_period_));
   new_grids_timer_ = create_wall_timer(
-    std::chrono::milliseconds(int(1000.0 * check_new_grids_period_)),
+    std::chrono::milliseconds(new_grids_timer_ms),
     std::bind(&GridPublisher::checkForNewGrids, this)
   );
 
@@ -168,7 +188,7 @@ void GridPublisher::getDatasets(
   for(const auto& d: response->datasets)
   {
     requested_grids_.push_back(d.label);
-    requested_grids_to_publish_.push_back(d.label);
+    requested_grids_to_publish_.insert(d.label);
     // Only record start time on the first request for this label, so
     // re-requests don't reset the measurement.
     grid_request_start_times_.emplace(d.label, now);
@@ -212,7 +232,7 @@ void GridPublisher::checkForNewGrids()
         auto grid = pg.second.get();
         {
           std::lock_guard<std::mutex> lock(requested_grids_mutex);
-          if(std::find(requested_grids_to_publish_.begin(), requested_grids_to_publish_.end(), pg.first) != requested_grids_to_publish_.end())
+          if(requested_grids_to_publish_.count(pg.first) > 0)
           {
             auto ds = catalog_->dataset(pg.first);
 
@@ -252,6 +272,8 @@ void GridPublisher::checkForNewGrids()
               grid_map::GridMapRosConverter::toOccupancyGrid(*grid, "elevation", -10.0, 0.0, occupancy_grid);
               costmap_publishers_[pg.first]->publish(occupancy_grid);
             }
+
+            requested_grids_to_publish_.erase(pg.first);
           }
           else
             grid_publishers_[pg.first]; // create the entry in the map so above check to see if we need to generate a grid works.
@@ -310,13 +332,91 @@ void GridPublisher::tryPrecompute()
   // outside polar regions.
   const double meters_per_deg_lat = 111000.0;
   const double meters_per_deg_lon = 111000.0 * std::cos(lat * M_PI / 180.0);
-  geographic_msgs::msg::BoundingBox bounds;
-  bounds.min_pt.latitude  = lat - precompute_radius_ / meters_per_deg_lat;
-  bounds.max_pt.latitude  = lat + precompute_radius_ / meters_per_deg_lat;
-  bounds.min_pt.longitude = lon - precompute_radius_ / meters_per_deg_lon;
-  bounds.max_pt.longitude = lon + precompute_radius_ / meters_per_deg_lon;
+  const double lat_delta = precompute_radius_ / meters_per_deg_lat;
+
+  // Latitude is bounded; clamp to the physically meaningful range.
+  const double lat_min = std::max(-90.0, lat - lat_delta);
+  const double lat_max = std::min( 90.0, lat + lat_delta);
+
+  // Longitude wraps. Build one or two query boxes:
+  //   - Polar region (cos(lat) ≈ 0): meters_per_deg_lon → 0 makes the
+  //     equirectangular lon span effectively global; use [-180, 180].
+  //   - Radius spans the globe (lon_delta >= 180): same.
+  //   - Crosses the dateline: split into two boxes, since
+  //     S57Dataset::intersects assumes minLon <= maxLon and does not wrap.
+  std::vector<geographic_msgs::msg::BoundingBox> query_bounds;
+  constexpr double kMinMetersPerDegLon = 1.0;  // ~89.99999° lat
+  if(std::abs(meters_per_deg_lon) <= kMinMetersPerDegLon)
+  {
+    geographic_msgs::msg::BoundingBox b;
+    b.min_pt.latitude = lat_min;
+    b.max_pt.latitude = lat_max;
+    b.min_pt.longitude = -180.0;
+    b.max_pt.longitude =  180.0;
+    query_bounds.push_back(b);
+  }
+  else
+  {
+    const double lon_delta = precompute_radius_ / meters_per_deg_lon;
+    if(lon_delta >= 180.0)
+    {
+      geographic_msgs::msg::BoundingBox b;
+      b.min_pt.latitude = lat_min;
+      b.max_pt.latitude = lat_max;
+      b.min_pt.longitude = -180.0;
+      b.max_pt.longitude =  180.0;
+      query_bounds.push_back(b);
+    }
+    else
+    {
+      const double lon_min = lon - lon_delta;
+      const double lon_max = lon + lon_delta;
+      if(lon_min < -180.0)
+      {
+        geographic_msgs::msg::BoundingBox west, east;
+        west.min_pt.latitude = lat_min; west.max_pt.latitude = lat_max;
+        east.min_pt.latitude = lat_min; east.max_pt.latitude = lat_max;
+        west.min_pt.longitude = lon_min + 360.0;
+        west.max_pt.longitude =  180.0;
+        east.min_pt.longitude = -180.0;
+        east.max_pt.longitude = lon_max;
+        query_bounds.push_back(west);
+        query_bounds.push_back(east);
+      }
+      else if(lon_max > 180.0)
+      {
+        geographic_msgs::msg::BoundingBox west, east;
+        west.min_pt.latitude = lat_min; west.max_pt.latitude = lat_max;
+        east.min_pt.latitude = lat_min; east.max_pt.latitude = lat_max;
+        west.min_pt.longitude = lon_min;
+        west.max_pt.longitude =  180.0;
+        east.min_pt.longitude = -180.0;
+        east.max_pt.longitude = lon_max - 360.0;
+        query_bounds.push_back(west);
+        query_bounds.push_back(east);
+      }
+      else
+      {
+        geographic_msgs::msg::BoundingBox b;
+        b.min_pt.latitude = lat_min;
+        b.max_pt.latitude = lat_max;
+        b.min_pt.longitude = lon_min;
+        b.max_pt.longitude = lon_max;
+        query_bounds.push_back(b);
+      }
+    }
+  }
 
-  auto charts = catalog_->intersectingCharts(bounds);
+  std::vector<std::shared_ptr<marine_charts::S57Dataset>> charts;
+  std::unordered_set<std::string> seen_labels;
+  for(const auto& qb: query_bounds)
+  {
+    for(const auto& chart: catalog_->intersectingCharts(qb))
+    {
+      if(seen_labels.insert(chart->label()).second)
+        charts.push_back(chart);
+    }
+  }
   if(charts.empty())
   {
     RCLCPP_WARN_STREAM(get_logger(),
@@ -341,7 +441,7 @@ void GridPublisher::tryPrecompute()
     if(grid_request_start_times_.count(label) == 0)
     {
       requested_grids_.push_back(label);
-      requested_grids_to_publish_.push_back(label);
+      requested_grids_to_publish_.insert(label);
       grid_request_start_times_.emplace(label, now);
       ++queued;
     }
diff --git a/s57_grids/src/grid_publisher.h b/s57_grids/src/grid_publisher.h
index e540c21..7edacff 100644
--- a/s57_grids/src/grid_publisher.h
+++ b/s57_grids/src/grid_publisher.h
@@ -2,6 +2,7 @@
 #define S57_GRIDS_S57_GRID_PUBLISHER_H
 
 #include <future>
+#include <unordered_set>
 
 
 #include "grid_map_ros/grid_map_ros.hpp"
@@ -97,7 +98,10 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode
   double grid_republish_period_ = 0.0; // seconds, if <= 0.0, no republishing
 
   std::vector<std::string> requested_grids_;
-  std::vector<std::string> requested_grids_to_publish_;
+  // Labels that have been requested but not yet published. Used to gate
+  // publish-on-ready in checkForNewGrids; entries are erased after publish
+  // so the set stays bounded across long missions.
+  std::unordered_set<std::string> requested_grids_to_publish_;
   std::mutex requested_grids_mutex;
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp
index fd2f5eb..63798c3 100644
--- a/s57_layer/src/s57_layer.cpp
+++ b/s57_layer/src/s57_layer.cpp
@@ -54,13 +54,15 @@ void S57Layer::onInitialize()
   declareParameter("tile_size", rclcpp::ParameterValue(tile_size_));
   node->get_parameter(name_+".tile_size", tile_size_);
 
+  const double default_buffer_fraction = buffer_fraction_;
   declareParameter("buffer_fraction", rclcpp::ParameterValue(buffer_fraction_));
   node->get_parameter(name_+".buffer_fraction", buffer_fraction_);
-  if(buffer_fraction_ < 0.0)
+  if(!std::isfinite(buffer_fraction_) || buffer_fraction_ < 0.0)
   {
     RCLCPP_WARN_STREAM(logger_,
-      "Invalid buffer_fraction value " << buffer_fraction_ << "; using 0.05 instead.");
-    buffer_fraction_ = 0.05;
+      "Invalid buffer_fraction value " << buffer_fraction_
+      << "; using " << default_buffer_fraction << " instead.");
+    buffer_fraction_ = default_buffer_fraction;
   }
 
   declareParameter("allow_uncharted", rclcpp::ParameterValue(allow_uncharted_));

From 218707bb7ae4674f8fa3d11332aca5f6ac05e3fc Mon Sep 17 00:00:00 2001
From: Claude Code Agent <roland+claude-code@ccom.unh.edu>
Date: Wed, 22 Apr 2026 02:07:24 -0400
Subject: [PATCH 3/3] =?UTF-8?q?fix(s57=5Fgrids):=20address=20second-round?=
 =?UTF-8?q?=20Copilot=20review=20on=20PR=20#20=20=E2=80=94=20map-leak=20ga?=
 =?UTF-8?q?ting,=20steady-clock=20latency,=20portability?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Gate insertions in getDatasets and tryPrecompute on
  grid_publishers_ / pending_dataset_grids_ membership. Previously a
  re-request for an already-published chart would add entries to
  requested_grids_to_publish_ and grid_request_start_times_ that never
  got cleaned up (cleanup only runs when a pending future completes,
  and no future is created for charts already in grid_publishers_).
- Switch request-to-publish latency tracking from rclcpp::Time to
  std::chrono::steady_clock. ROS time can pause, jump, or run under
  use_sim_time — our deadpool sim in fact uses sim time — so the
  reported "request to publish" seconds could be inaccurate or
  negative. Steady clock measures actual wall-clock elapsed regardless.
- Replace M_PI (POSIX, non-standard C++) with a constexpr kPi.
- Add explicit <algorithm>, <cstdlib>, <chrono> includes for symbols
  already used (std::max, std::getenv, steady_clock) — they compile
  today via transitive includes but the translation unit should be
  self-contained.

Build + tests green in worktree: 41 tests, 0 failures.
---
 s57_grids/src/grid_publisher.cpp | 44 ++++++++++++++++++++++----------
 s57_grids/src/grid_publisher.h   |  9 ++++---
 2 files changed, 36 insertions(+), 17 deletions(-)

diff --git a/s57_grids/src/grid_publisher.cpp b/s57_grids/src/grid_publisher.cpp
index c783ba6..ec75770 100644
--- a/s57_grids/src/grid_publisher.cpp
+++ b/s57_grids/src/grid_publisher.cpp
@@ -4,7 +4,10 @@
 #include "geometry_msgs/msg/point_stamped.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include <algorithm>
+#include <chrono>
 #include <cmath>
+#include <cstdlib>
 #include <thread>
 #include <unordered_set>
 
@@ -184,9 +187,17 @@ void GridPublisher::getDatasets(
 {
   listDatasets(request, response);
   std::lock_guard<std::mutex> lock(requested_grids_mutex);
-  auto now = get_clock()->now();
+  const auto now = std::chrono::steady_clock::now();
   for(const auto& d: response->datasets)
   {
+    // Skip labels already published or in flight — otherwise their
+    // entries in requested_grids_to_publish_ / grid_request_start_times_
+    // would never be cleaned up. Cleanup runs only when a pending future
+    // completes, and checkForNewGrids will not start a new future for
+    // labels already in grid_publishers_ or pending_dataset_grids_.
+    if(grid_publishers_.count(d.label) > 0
+       || pending_dataset_grids_.count(d.label) > 0)
+      continue;
     requested_grids_.push_back(d.label);
     requested_grids_to_publish_.insert(d.label);
     // Only record start time on the first request for this label, so
@@ -251,9 +262,10 @@ void GridPublisher::checkForNewGrids()
             auto start_it = grid_request_start_times_.find(pg.first);
             if(start_it != grid_request_start_times_.end())
             {
-              auto elapsed = (get_clock()->now() - start_it->second).seconds();
+              const std::chrono::duration<double> elapsed =
+                std::chrono::steady_clock::now() - start_it->second;
               RCLCPP_INFO_STREAM(get_logger(),
-                "Grid ready: " << pg.first << " (" << elapsed << " s from request to publish)");
+                "Grid ready: " << pg.first << " (" << elapsed.count() << " s from request to publish)");
               grid_request_start_times_.erase(start_it);
             }
             else
@@ -330,8 +342,9 @@ void GridPublisher::tryPrecompute()
   // Equirectangular approximation: 1° lat ≈ 111 000 m everywhere;
   // 1° lon ≈ 111 000 m × cos(lat). Good to <1% for radii up to ~50 km
   // outside polar regions.
+  constexpr double kPi = 3.14159265358979323846;
   const double meters_per_deg_lat = 111000.0;
-  const double meters_per_deg_lon = 111000.0 * std::cos(lat * M_PI / 180.0);
+  const double meters_per_deg_lon = 111000.0 * std::cos(lat * kPi / 180.0);
   const double lat_delta = precompute_radius_ / meters_per_deg_lat;
 
   // Latitude is bounded; clamp to the physically meaningful range.
@@ -430,21 +443,24 @@ void GridPublisher::tryPrecompute()
   // requested_grids_ to trigger generation and requested_grids_to_publish_
   // so the latched topic message gets published when ready — late-arriving
   // S57Layer subscribers see the cached message thanks to transient_local
-  // QoS). Labels already in flight (tracked via grid_request_start_times_)
-  // are skipped to avoid double-queuing.
+  // QoS). Skip labels already published, in flight, or queued — otherwise
+  // their entries in requested_grids_to_publish_ / grid_request_start_times_
+  // would never be cleaned up (cleanup runs only when a pending future
+  // completes).
   std::lock_guard<std::mutex> lock(requested_grids_mutex);
-  auto now = get_clock()->now();
+  const auto now = std::chrono::steady_clock::now();
   size_t queued = 0;
   for(const auto& chart: charts)
   {
     const std::string& label = chart->label();
-    if(grid_request_start_times_.count(label) == 0)
-    {
-      requested_grids_.push_back(label);
-      requested_grids_to_publish_.insert(label);
-      grid_request_start_times_.emplace(label, now);
-      ++queued;
-    }
+    if(grid_publishers_.count(label) > 0
+       || pending_dataset_grids_.count(label) > 0
+       || grid_request_start_times_.count(label) > 0)
+      continue;
+    requested_grids_.push_back(label);
+    requested_grids_to_publish_.insert(label);
+    grid_request_start_times_.emplace(label, now);
+    ++queued;
   }
   RCLCPP_INFO_STREAM(get_logger(),
     "Precompute: queued " << queued << " of " << charts.size()
diff --git a/s57_grids/src/grid_publisher.h b/s57_grids/src/grid_publisher.h
index 7edacff..3aec28c 100644
--- a/s57_grids/src/grid_publisher.h
+++ b/s57_grids/src/grid_publisher.h
@@ -1,6 +1,7 @@
 #ifndef S57_GRIDS_S57_GRID_PUBLISHER_H
 #define S57_GRIDS_S57_GRID_PUBLISHER_H
 
+#include <chrono>
 #include <future>
 #include <unordered_set>
 
@@ -84,9 +85,11 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode
   // Futures waiting for datasets being generated in separate threads
   std::map<std::string, std::future<std::shared_ptr<grid_map::GridMap> > > pending_dataset_grids_;
 
-  // Wall-clock time each chart was first added to requested_grids_ — used
-  // to log per-chart processing latency when the grid is published.
-  std::map<std::string, rclcpp::Time> grid_request_start_times_;
+  // Steady-clock time each chart was first added to requested_grids_ —
+  // used to log per-chart processing latency when the grid is published.
+  // Steady (not ROS) time so that use_sim_time, sim pauses, or NTP step
+  // corrections don't produce negative or inflated latency numbers.
+  std::map<std::string, std::chrono::steady_clock::time_point> grid_request_start_times_;
 
   rclcpp::TimerBase::SharedPtr new_grids_timer_;
   // Period for new_grids_timer_ in seconds. Smaller values reap completed