feat(autoware_map_loader): add support of GetSelectedLanelet2Map service

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

Author: mitsudome-r

map/autoware_map_loader/CMakeLists.txt

@@ -30,6 +30,8 @@ rclcpp_components_register_node(pointcloud_map_loader_node
 ament_auto_add_library(lanelet2_map_loader_node SHARED
   src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
   src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp
+  src/lanelet2_map_loader/lanelet2_map_cell_metadata.cpp
+  src/lanelet2_map_loader/lanelet2_selected_map_loader_module.cpp
 )
 
 rclcpp_components_register_node(lanelet2_map_loader_node
@@ -66,6 +68,8 @@ if(BUILD_TESTING)
   add_testcase(test/test_partial_map_loader_module.cpp)
   add_testcase(test/test_differential_map_loader_module.cpp)
   add_testcase(test/test_lanelet2_map_loader_utils.cpp)
+  add_testcase(test/test_lanelet2_map_cell_metadata.cpp)
+  add_testcase(test/test_lanelet2_selected_map_loader_module.cpp)
 endif()
 
 install(PROGRAMS

map/autoware_map_loader/README.md

@@ -137,6 +137,39 @@ Please see [autoware_map_msgs/msg/MapProjectorInfo.msg](https://github.com/autow
 `lanelet2_map_path` can point to either a single `.osm` file or a directory.
 When a directory is given, all `.osm` files inside are loaded and merged into a single map before publishing.
 
+#### Selected map loading (`enable_selected_map_loading`)
+
+When `enable_selected_map_loading` is `true`, the node additionally:
+
+- Publishes per-cell bounding-box metadata on `output/lanelet2_map_metadata`.
+- Exposes the `service/get_selected_lanelet2_map` service
+  (`autoware_map_msgs/srv/GetSelectedLanelet2Map`), which returns the binary
+  map for the requested set of cell IDs.
+
+Cell IDs equal the absolute file path of the corresponding `.osm` file.
+
+##### Cell metadata source
+
+The node determines bounding boxes for each cell in one of two ways:
+
+1. **From a metadata YAML file** — when `metadata_file_path` points to an
+   existing file.  The expected format is:
+
+   ```yaml
+   x_resolution: 100.0
+   y_resolution: 100.0
+   1.osm: [58700.0, 42500.0]   # [min_x, min_y] of this cell
+   2.osm: [58800.0, 42500.0]
+   ```
+
+   Relative filenames are resolved against the directory that contains the
+   YAML file.  The bounding box of each cell is
+   `[min_x, min_y, min_x + x_resolution, min_y + y_resolution]`.
+
+2. **Computed from the loaded map** — when `metadata_file_path` is empty or
+   the file does not exist, the axis-aligned bounding box is derived from all
+   points in each loaded map.
+
 ### How to run
 
 `ros2 run autoware_map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm`
@@ -147,7 +180,12 @@ When a directory is given, all `.osm` files inside are loaded and merged into a
 
 ### Published Topics
 
-- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map
+- `/map/vector_map` (autoware_map_msgs/LaneletMapBin) : Merged binary Lanelet2 map
+- `output/lanelet2_map_metadata` (autoware_map_msgs/LaneletMapMetaData) : Per-cell bounding-box metadata *(only when `enable_selected_map_loading` is `true`)*
+
+### Services
+
+- `service/get_selected_lanelet2_map` (autoware_map_msgs/srv/GetSelectedLanelet2Map) : Returns the binary map for a requested set of cell IDs *(only when `enable_selected_map_loading` is `true`)*
 
 ### Parameters
 

map/autoware_map_loader/config/lanelet2_map_loader.param.yaml

@@ -4,3 +4,5 @@
     center_line_resolution: 5.0                 # [m]
     use_waypoints: true                         # "centerline" in the Lanelet2 map will be used as a "waypoints" tag.
     lanelet2_map_path: $(var lanelet2_map_path) # Path to a lanelet2 .osm map file, or a directory containing .osm files
+    enable_selected_map_loading: false          # expose service/get_selected_lanelet2_map and publish cell metadata
+    metadata_file_path: $(var metadata_file_path) # path to lanelet2_map_metadata.yaml; if empty or missing, bboxes are computed from map points

map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp

@@ -29,13 +29,18 @@
 
 namespace autoware::map_loader
 {
+
+// Forward declaration — full definition lives in the private source directory.
+class Lanelet2SelectedMapLoaderModule;
+
 class Lanelet2MapLoaderNode : public rclcpp::Node
 {
 public:
   static constexpr lanelet::autoware::Version version = lanelet::autoware::version;
 
 public:
   explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
+  ~Lanelet2MapLoaderNode();  // defined in .cpp so unique_ptr can see the full module type
 
   static lanelet::LaneletMapPtr load_map(
     const std::string & lanelet2_filename,
@@ -51,6 +56,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
 
   rclcpp::Subscription<MapProjectorInfo::Message>::SharedPtr sub_map_projector_info_;
   rclcpp::Publisher<VectorMap::Message>::SharedPtr pub_map_bin_;
+
+  std::unique_ptr<Lanelet2SelectedMapLoaderModule> selected_map_loader_module_;
 };
 }  // namespace autoware::map_loader
 

map/autoware_map_loader/launch/lanelet2_map_loader.launch.xml

@@ -1,6 +1,7 @@
 <launch>
   <arg name="lanelet2_map_loader_param_path" default="$(find-pkg-share autoware_map_loader)/config/lanelet2_map_loader.param.yaml"/>
   <arg name="lanelet2_map_path"/>
+  <arg name="metadata_file_path" default=""/>
   <arg name="lanelet2_map_topic" default="vector_map"/>
   <arg name="lanelet2_map_marker_topic" default="vector_map_marker"/>
 

map/autoware_map_loader/schema/lanelet2_map_loader.schema.json

@@ -25,6 +25,16 @@
           "type": "string",
           "description": "Path to a lanelet2 .osm map file, or a directory containing .osm files",
           "default": ""
+        },
+        "enable_selected_map_loading": {
+          "type": "boolean",
+          "description": "Expose service/get_selected_lanelet2_map and publish per-cell metadata.",
+          "default": false
+        },
+        "metadata_file_path": {
+          "type": "string",
+          "description": "Path to lanelet2_map_metadata.yaml. If empty or missing, bounding boxes are computed from the loaded map points.",
+          "default": ""
         }
       },
       "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"],

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_cell_metadata.cpp

@@ -0,0 +1,78 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lanelet2_map_cell_metadata.hpp"
+
+#include <yaml-cpp/yaml.h>
+
+#include <filesystem>
+#include <limits>
+#include <map>
+#include <optional>
+#include <string>
+
+namespace autoware::map_loader::utils
+{
+
+Lanelet2FileMetaData compute_cell_metadata(
+  const std::string & cell_id, const lanelet::LaneletMap & map)
+{
+  double min_x = std::numeric_limits<double>::max();
+  double min_y = std::numeric_limits<double>::max();
+  double max_x = std::numeric_limits<double>::lowest();
+  double max_y = std::numeric_limits<double>::lowest();
+
+  for (const lanelet::ConstPoint3d & pt : map.pointLayer) {
+    if (pt.x() < min_x) min_x = pt.x();
+    if (pt.y() < min_y) min_y = pt.y();
+    if (pt.x() > max_x) max_x = pt.x();
+    if (pt.y() > max_y) max_y = pt.y();
+  }
+
+  if (map.pointLayer.empty()) {
+    min_x = max_x = min_y = max_y = 0.0;
+  }
+
+  return {cell_id, min_x, min_y, max_x, max_y};
+}
+
+std::optional<std::map<std::string, Lanelet2FileMetaData>> load_cell_metadata_from_yaml(
+  const std::string & yaml_path)
+{
+  if (!std::filesystem::exists(yaml_path)) {
+    return std::nullopt;
+  }
+
+  const std::filesystem::path map_dir = std::filesystem::path(yaml_path).parent_path();
+
+  const YAML::Node node = YAML::LoadFile(yaml_path);
+  const double x_res = node["x_resolution"].as<double>();
+  const double y_res = node["y_resolution"].as<double>();
+
+  std::map<std::string, Lanelet2FileMetaData> result;
+  for (const auto & kv : node) {
+    const std::string key = kv.first.as<std::string>();
+    if (key == "x_resolution" || key == "y_resolution") {
+      continue;
+    }
+    const double min_x = kv.second[0].as<double>();
+    const double min_y = kv.second[1].as<double>();
+    const std::string abs_path = (map_dir / key).string();
+    result[abs_path] = {abs_path, min_x, min_y, min_x + x_res, min_y + y_res};
+  }
+
+  return result;
+}
+
+}  // namespace autoware::map_loader::utils

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_cell_metadata.hpp

@@ -0,0 +1,73 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_MAP_LOADER__LANELET2_MAP_CELL_METADATA_HPP_
+#define LANELET2_MAP_LOADER__LANELET2_MAP_CELL_METADATA_HPP_
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <map>
+#include <optional>
+#include <string>
+
+namespace autoware::map_loader
+{
+
+/// @brief Bounding-box metadata for a single lanelet2 map cell (one OSM file).
+///
+/// The bounding box covers all points in the loaded map, making it accurate without
+/// an external metadata file or a fixed grid resolution.
+/// @p id equals the absolute file path and serves as the cell identifier in
+/// `GetSelectedLanelet2Map` requests.
+struct Lanelet2FileMetaData
+{
+  std::string id;  ///< Unique cell identifier (== absolute file path).
+  double min_x;
+  double min_y;
+  double max_x;
+  double max_y;
+};
+
+}  // namespace autoware::map_loader
+
+namespace autoware::map_loader::utils
+{
+
+/// @brief Compute the axis-aligned bounding box of all points in @p map.
+/// If the map has no points all coordinates are set to 0.
+Lanelet2FileMetaData compute_cell_metadata(
+  const std::string & cell_id, const lanelet::LaneletMap & map);
+
+/// @brief Load per-cell bounding-box metadata from @p yaml_path.
+///
+/// The expected YAML format is:
+/// @code
+///   x_resolution: 100.0
+///   y_resolution: 100.0
+///   1.osm: [58700.0, 42500.0]   # [min_x, min_y] of that cell
+///   2.osm: [58800.0, 42500.0]
+///   ...
+/// @endcode
+///
+/// Cell bounding boxes are `[min_x, min_y, min_x + x_resolution, min_y + y_resolution]`.
+/// Relative filename keys are resolved against the parent directory of @p yaml_path.
+///
+/// @return Populated metadata dict on success, or `std::nullopt` if the file does not exist or
+///         cannot be parsed.
+std::optional<std::map<std::string, Lanelet2FileMetaData>> load_cell_metadata_from_yaml(
+  const std::string & yaml_path);
+
+}  // namespace autoware::map_loader::utils
+
+#endif  // LANELET2_MAP_LOADER__LANELET2_MAP_CELL_METADATA_HPP_

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -34,7 +34,9 @@
 #include "autoware/map_loader/lanelet2_map_loader_node.hpp"
 
 #include "lanelet2_local_projector.hpp"
+#include "lanelet2_map_cell_metadata.hpp"
 #include "lanelet2_map_loader_utils.hpp"
+#include "lanelet2_selected_map_loader_module.hpp"
 
 #include <autoware/geography_utils/lanelet2_projector.hpp>
 #include <autoware/lanelet2_utils/conversion.hpp>
@@ -49,13 +51,17 @@
 #include <lanelet2_io/Io.h>
 #include <lanelet2_projection/UTM.h>
 
+#include <filesystem>
+#include <map>
 #include <memory>
+#include <optional>
 #include <stdexcept>
 #include <string>
 #include <vector>
 
 namespace autoware::map_loader
 {
+
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_map_msgs::msg::MapProjectorInfo;
 
@@ -71,15 +77,24 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
   declare_parameter<std::string>("lanelet2_map_path");
   declare_parameter<double>("center_line_resolution");
   declare_parameter<bool>("use_waypoints");
+  declare_parameter<bool>("enable_selected_map_loading");
+  declare_parameter<std::string>("metadata_file_path");
 }
 
+// Defined here so the compiler sees the full type of Lanelet2SelectedMapLoaderModule
+// when generating the unique_ptr destructor.
+Lanelet2MapLoaderNode::~Lanelet2MapLoaderNode() = default;
+
 void Lanelet2MapLoaderNode::on_map_projector_info(
   const MapProjectorInfo::Message::ConstSharedPtr msg)
 {
   const auto allow_unsupported_version = get_parameter("allow_unsupported_version").as_bool();
   const auto lanelet2_map_path = get_parameter("lanelet2_map_path").as_string();
   const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
   const auto use_waypoints = get_parameter("use_waypoints").as_bool();
+  const auto enable_selected_map_loading =
+    get_parameter("enable_selected_map_loading").as_bool();
+  const auto metadata_file_path = get_parameter("metadata_file_path").as_string();
 
   // get lanelet2 file paths (handles both a single .osm file and a directory)
   const std::vector<std::string> lanelet2_paths = utils::get_lanelet2_paths(lanelet2_map_path);
@@ -101,6 +116,26 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
     maps.push_back(map_tmp);
   }
 
+  if (enable_selected_map_loading) {
+    std::map<std::string, Lanelet2FileMetaData> cell_metadata_dict;
+
+    const auto yaml_metadata = utils::load_cell_metadata_from_yaml(metadata_file_path);
+    if (yaml_metadata) {
+      cell_metadata_dict = *yaml_metadata;
+      RCLCPP_INFO(get_logger(), "Loaded cell metadata from %s.", metadata_file_path.c_str());
+    } else {
+      for (size_t i = 0; i < lanelet2_paths.size(); ++i) {
+        cell_metadata_dict[lanelet2_paths[i]] =
+          utils::compute_cell_metadata(lanelet2_paths[i], *maps[i]);
+      }
+    }
+
+    selected_map_loader_module_ = std::make_unique<Lanelet2SelectedMapLoaderModule>(
+      this, std::move(cell_metadata_dict), *msg, center_line_resolution, use_waypoints);
+
+    RCLCPP_INFO(get_logger(), "Selected lanelet2 map loading is enabled.");
+  }
+
   // merge all loaded maps into a new empty map
   auto map = std::make_shared<lanelet::LaneletMap>();
   for (auto & loaded_map : maps) {