feat(s57): cold-cache mitigation — faster polling, timing logs, precompute, buffer param

s57_grids/src/grid_publisher.cpp

@@ -4,7 +4,12 @@
 #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>
 
 namespace s57_grids
 {
@@ -62,6 +67,40 @@ 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"))
+  {
+    declare_parameter("robot_base_frame", rclcpp::ParameterValue(robot_base_frame_));
+  }
+  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",
     std::bind(&GridPublisher::listDatasets, this, std::placeholders::_1, std::placeholders::_2)
@@ -75,11 +114,20 @@ 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(1000),
+    std::chrono::milliseconds(new_grids_timer_ms),
     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 +187,35 @@ void GridPublisher::getDatasets(
 {
   listDatasets(request, response);
   std::lock_guard<std::mutex> lock(requested_grids_mutex);
+  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_.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);
   }
 }
 
 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_)
@@ -175,7 +243,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);
 
@@ -189,7 +257,21 @@ 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())
+            {
+              const std::chrono::duration<double> elapsed =
+                std::chrono::steady_clock::now() - start_it->second;
+              RCLCPP_INFO_STREAM(get_logger(),
+                "Grid ready: " << pg.first << " (" << elapsed.count() << " 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_)
@@ -202,6 +284,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.
@@ -220,6 +304,171 @@ 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.
+  constexpr double kPi = 3.14159265358979323846;
+  const double meters_per_deg_lat = 111000.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.
+  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);
+      }
+    }
+  }
+
+  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(),
+      "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). 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);
+  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_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()
+    << " charts within " << precompute_radius_ << " m of ("
+    << lat << ", " << lon << ").");
+  did_precompute_ = true;
+}
+
 void GridPublisher::republishGrids()
 {
   RCLCPP_INFO_STREAM(get_logger(), "Republishing grids");

s57_grids/src/grid_publisher.h

@@ -1,7 +1,9 @@
 #ifndef S57_GRIDS_S57_GRID_PUBLISHER_H
 #define S57_GRIDS_S57_GRID_PUBLISHER_H
 
+#include <chrono>
 #include <future>
+#include <unordered_set>
 
 
 #include "grid_map_ros/grid_map_ros.hpp"
@@ -58,6 +60,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,20 +85,40 @@ 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_;
 
+  // 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
+  // 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
 
   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_;
   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;
 };