feat(s57): cold-cache mitigation — faster polling, timing logs, precompute, buffer param#20
Conversation
…mpute, buffer param
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>
There was a problem hiding this comment.
Pull request overview
This PR adds several parameterized mechanisms in s57_grids and s57_layer to reduce S57 cold-cache latency by polling completed grid futures more frequently, logging per-chart latency at INFO, optionally precomputing nearby charts once TF is available, and making the S57Layer bounds buffer configurable.
Changes:
- Add
buffer_fractionparameter toS57Layerto control buffered chart-request bounds (default preserves prior 5% behavior). - Add
check_new_grids_periodparameter and INFO timing logs tos57_gridsgrid publishing path. - Add TF-triggered, one-shot precompute (
robot_base_frame,precompute_radius) to queue nearby charts early.
Reviewed changes
Copilot reviewed 4 out of 4 changed files in this pull request and generated 6 comments.
| File | Description |
|---|---|
| s57_layer/src/s57_layer.h | Documents and introduces buffer_fraction_ member (default 0.05). |
| s57_layer/src/s57_layer.cpp | Declares/reads buffer_fraction parameter and uses it to compute buffered bounds. |
| s57_grids/src/grid_publisher.h | Adds members for timing logs, polling period parameter, and precompute configuration/state. |
| s57_grids/src/grid_publisher.cpp | Implements configurable polling period, per-chart INFO timing logs, and TF-based precompute queuing. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| 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); | ||
| } |
There was a problem hiding this comment.
requested_grids_to_publish_ is append-only (labels are pushed here on every request, including precompute) but is never pruned, and later membership is checked with a linear std::find(...). With the new precompute path potentially queuing many charts at once, this vector can grow without bound and make the publish check increasingly expensive. Consider using a std::unordered_set for membership and removing labels once published (or otherwise bounding its size).
| 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; | ||
|
|
There was a problem hiding this comment.
The catalog’s intersects() check assumes minLon <= maxLon and does not handle dateline wrapping (see marine_charts::S57Dataset::intersects). Here lon ± delta can exceed the valid [-180, 180] range or cross the dateline, which will make intersectingCharts(bounds) miss charts (or behave incorrectly). Consider normalizing/clamping longitudes and handling the wrap case by splitting into two bounding boxes when needed (and clamping latitudes to [-90, 90]).
| 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; |
There was a problem hiding this comment.
buffer_fraction_ is only checked for negative values. If it is NaN/Inf (or otherwise non-finite), buffer becomes non-finite in updateBounds, which can prevent bounds updates and block dataset requests. Consider validating with std::isfinite(buffer_fraction_) && buffer_fraction_ >= 0.0, and fall back to the previous default value (captured before get_parameter) instead of hardcoding 0.05 in the warning path.
| 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; | |
| const double default_buffer_fraction = buffer_fraction_; | |
| declareParameter("buffer_fraction", rclcpp::ParameterValue(buffer_fraction_)); | |
| node->get_parameter(name_+".buffer_fraction", buffer_fraction_); | |
| if(!std::isfinite(buffer_fraction_) || buffer_fraction_ < 0.0) | |
| { | |
| RCLCPP_WARN_STREAM(logger_, | |
| "Invalid buffer_fraction value " << buffer_fraction_ | |
| << "; using " << default_buffer_fraction << " instead."); | |
| buffer_fraction_ = default_buffer_fraction; |
| @@ -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) | |||
| ); | |||
There was a problem hiding this comment.
check_new_grids_period_ is taken from a ROS parameter but not validated. Values <= 0, NaN/Inf, or very small values can result in a 0ms/negative timer period after int(1000.0 * check_new_grids_period_), which can throw or create a tight loop. Please validate the parameter (finite and > 0), and clamp the resulting timer duration to a sane minimum (e.g., >= 1ms) before creating the wall timer.
| { | ||
| declare_parameter("precompute_radius", rclcpp::ParameterValue(precompute_radius_)); | ||
| } | ||
| precompute_radius_ = get_parameter("precompute_radius").as_double(); |
There was a problem hiding this comment.
precompute_radius_ is read from a parameter but never validated. NaN/Inf or negative values will produce invalid bounding boxes (NaNs/Infs) and can cause catalog intersection queries to behave unpredictably. Consider validating it (finite and >= 0) at configure time and disabling precompute (or falling back to the default) when invalid.
| precompute_radius_ = get_parameter("precompute_radius").as_double(); | |
| const double precompute_radius = get_parameter("precompute_radius").as_double(); | |
| if(std::isfinite(precompute_radius) && precompute_radius >= 0.0) | |
| { | |
| precompute_radius_ = precompute_radius; | |
| } | |
| else | |
| { | |
| RCLCPP_WARN_STREAM( | |
| get_logger(), | |
| "Invalid precompute_radius parameter (" << precompute_radius | |
| << "). Expected a finite value >= 0. Disabling precompute." | |
| ); | |
| precompute_radius_ = 0.0; | |
| } |
| bounds.min_pt.longitude = lon - precompute_radius_ / meters_per_deg_lon; | ||
| bounds.max_pt.longitude = lon + precompute_radius_ / meters_per_deg_lon; | ||
|
|
There was a problem hiding this comment.
meters_per_deg_lon can become 0 (or extremely small) near the poles because it is scaled by cos(lat). This makes the longitude bounds division blow up (Inf/NaN) and can queue essentially the entire catalog. Add a guard for small |meters_per_deg_lon| (or avoid the equirectangular approximation by using a more robust geodesic/bounds computation) before computing bounds.min_pt.longitude/max_pt.longitude.
| bounds.min_pt.longitude = lon - precompute_radius_ / meters_per_deg_lon; | |
| bounds.max_pt.longitude = lon + precompute_radius_ / meters_per_deg_lon; | |
| // Near the poles, meters_per_deg_lon approaches zero and the simple | |
| // equirectangular longitude span can become non-finite or effectively | |
| // global. In that case, search across the full longitude range. | |
| constexpr double kMinMetersPerDegLon = 1e-9; | |
| if(std::abs(meters_per_deg_lon) <= kMinMetersPerDegLon) | |
| { | |
| bounds.min_pt.longitude = -180.0; | |
| bounds.max_pt.longitude = 180.0; | |
| } | |
| else | |
| { | |
| const double lon_delta = precompute_radius_ / meters_per_deg_lon; | |
| if(!std::isfinite(lon_delta) || std::abs(lon_delta) >= 180.0) | |
| { | |
| bounds.min_pt.longitude = -180.0; | |
| bounds.max_pt.longitude = 180.0; | |
| } | |
| else | |
| { | |
| bounds.min_pt.longitude = lon - lon_delta; | |
| bounds.max_pt.longitude = lon + lon_delta; | |
| } | |
| } |
|
Precompute path verified end-to-end in a follow-up sim run on deadpool 2026-04-22 with Precompute fired 0.6 s after node activation (TF availability). Queued 10 charts vs 8 in the costmap-layer-driven baseline — the 5 km radius caught two additional charts ( Companion ben_project11 yaml PR (to enable precompute by default) will follow. Authored-By: |
…teline/pole handling, bounded request set 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.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 4 out of 4 changed files in this pull request and generated 5 comments.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| #include "nav_msgs/msg/occupancy_grid.hpp" | ||
| #include <cmath> |
There was a problem hiding this comment.
This file now directly uses std::max/std::min (and already uses std::getenv) but doesn’t include <algorithm> / <cstdlib> explicitly. It may compile today due to transitive includes, but that’s not guaranteed. Consider adding the standard headers for the standard-library symbols used here to keep the translation unit self-contained.
| #include "nav_msgs/msg/occupancy_grid.hpp" | |
| #include <cmath> | |
| #include "nav_msgs/msg/occupancy_grid.hpp" | |
| #include <algorithm> | |
| #include <cmath> | |
| #include <cstdlib> |
| 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); | ||
| 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); | ||
| } |
There was a problem hiding this comment.
getDatasets() unconditionally adds every returned label to requested_grids_to_publish_ and grid_request_start_times_. If a grid was already published earlier (so grid_publishers_ already contains the label and no new future will be started), these entries will never be erased because cleanup only happens when a pending future completes. This will leak memory over time and can also leave stale start times around indefinitely. Consider only inserting into requested_grids_to_publish_ / grid_request_start_times_ when the label is not already available (e.g., no existing publisher/grid and no pending future), or add a cleanup path that removes entries for labels that are already published/cached.
| // 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); | ||
| } |
There was a problem hiding this comment.
The timing log is described as “wall-clock” but uses get_clock()->now() / rclcpp::Time (ROS time). With use_sim_time or time jumps, elapsed durations can be inaccurate or even negative. For latency measurements, record timestamps using a steady/monotonic source (e.g., std::chrono::steady_clock or an rclcpp::Clock(RCL_STEADY_TIME)) and compute elapsed from that.
| // 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); | |
| } | |
| // A request-to-publish latency should be measured with a steady clock. | |
| // Do not compute it from ROS time here, because ROS time may jump, | |
| // pause, or run under use_sim_time. | |
| auto start_it = grid_request_start_times_.find(pg.first); | |
| if(start_it != grid_request_start_times_.end()) | |
| { | |
| grid_request_start_times_.erase(start_it); | |
| } | |
| RCLCPP_INFO_STREAM(get_logger(), "Grid ready: " << pg.first); |
| const double meters_per_deg_lat = 111000.0; | ||
| const double meters_per_deg_lon = 111000.0 * std::cos(lat * M_PI / 180.0); |
There was a problem hiding this comment.
M_PI is a non-standard macro and may be undefined depending on platform / standard library feature macros. To avoid a portability build break, prefer a standard C++17 approach (e.g., std::acos(-1.0) or a local constexpr pi constant) when converting degrees to radians.
| const double meters_per_deg_lat = 111000.0; | |
| const double meters_per_deg_lon = 111000.0 * std::cos(lat * M_PI / 180.0); | |
| 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); |
| 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; | ||
| } |
There was a problem hiding this comment.
tryPrecompute() queues labels into requested_grids_to_publish_ / grid_request_start_times_ based only on grid_request_start_times_. If precompute runs after some charts were already published, those labels can be re-inserted even though no new async future will be created (because grid_publishers_ already exists), leaving entries that never get erased. The same gating/cleanup suggested for getDatasets() should be applied here (skip labels that are already published/cached, or actively remove them).
…eak gating, steady-clock latency, portability - 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.
Summary
Four independent knobs/mechanisms in
s57_gridsands57_layerto mitigate the S57 chart cold-cache latency identified in the deployment-log analysis. None couple the costmap layer to mission_manager / BT publishing patterns.s57_grids
check_new_grids_periodparameter, default0.1s (was hardcoded1.0s ingrid_publisher.cpp:79). Faster reaping of completedstd::asyncfutures.Per-chart timing logs at INFO level:
"Grid ready: <label> (X.X s from request to publish)". Records wall-clock from first request to publish. Lets us measureS57Dataset::getGridcost in the field without recompile.Precompute on first available pose. New parameters:
robot_base_frame(default""= disabled)precompute_radius(meters, default5000.0)On first
tf_buffer_->lookupTransform(map_frame, robot_base_frame)success, queues all charts withinprecompute_radiusof the boat forstd::asyncprocessing via the existingrequested_grids_path. Single-shot — setsdid_precompute_on success and stops checking. TF-only trigger; no goal-source coupling.s57_layer
buffer_fractionparameter, default0.05(preserves the prior hardcoded value ats57_layer.cpp:190). Configs that want a larger pre-load ring around the rolling costmap window can override without code changes. Negative values rejected with warning, fall back to default.Closes #19. Related: rolker/unh_marine_navigation#19.
Test plan
colcon build --packages-select s57_grids s57_layer— clean.colcon test --packages-select s57_layer— 23 tests, 0 errors, 0 failures, 5 skipped.TideOffsetTest5/5 passing (defaultbuffer_fractionunchanged).S57Dataset::getGridis the dominant component, not the polling loop.rolker/ben_project11to setrobot_base_frame: ben/base_link(and optionallyprecompute_radius) on/**/s57_gridsinben.yaml. Will follow as a separate small PR. The fall-through path (defaults) is exercised by the sim run above.Out of scope (separate work)
buffer_fractionisn't enough.S57Dataset::getGrid. Now that timing data identifies it as the bottleneck for wide-area charts, it's worth its own issue.Authored-By:
Claude Code AgentModel:
Claude Opus 4.7 (1M context)