diff --git a/s57_grids/src/grid_publisher.cpp b/s57_grids/src/grid_publisher.cpp index f08ebaf..ec75770 100644 --- a/s57_grids/src/grid_publisher.cpp +++ b/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 +#include +#include +#include #include +#include 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( "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(get_clock()); tf_listener_ = std::make_shared(*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 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 lock(requested_grids_mutex); for(auto r: requested_grids_) @@ -175,7 +243,7 @@ void GridPublisher::checkForNewGrids() auto grid = pg.second.get(); { std::lock_guard 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 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 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> charts; + std::unordered_set 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 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"); diff --git a/s57_grids/src/grid_publisher.h b/s57_grids/src/grid_publisher.h index 197af81..3aec28c 100644 --- a/s57_grids/src/grid_publisher.h +++ b/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 #include +#include #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 catalog_; rclcpp::Service::SharedPtr list_service_; rclcpp::Service::SharedPtr get_service_; @@ -77,20 +85,40 @@ class GridPublisher: public rclcpp_lifecycle::LifecycleNode // Futures waiting for datasets being generated in separate threads std::map > > 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 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 requested_grids_; - std::vector 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 requested_grids_to_publish_; std::mutex requested_grids_mutex; std::unique_ptr tf_buffer_; std::shared_ptr 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 abort_flag_ = false; }; diff --git a/s57_layer/src/s57_layer.cpp b/s57_layer/src/s57_layer.cpp index 5efd666..63798c3 100644 --- a/s57_layer/src/s57_layer.cpp +++ b/s57_layer/src/s57_layer.cpp @@ -54,6 +54,17 @@ 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(!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; + } + declareParameter("allow_uncharted", rclcpp::ParameterValue(allow_uncharted_)); node->get_parameter(name_+".allow_uncharted", allow_uncharted_); @@ -186,8 +197,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;