feat(map_loader): add format_version validation

Signed-off-by: Mamoru Sobue <[email protected]>

map/map_loader/config/lanelet2_map_loader.param.yaml

@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
+    allow_unsupported_version: false # flag to load unsupported format_version anyway. If true, just prints warning.
     center_line_resolution: 5.0         # [m]
     lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -17,6 +17,7 @@
 
 #include <component_interface_specs/map.hpp>
 #include <component_interface_utils/rclcpp.hpp>
+#include <lanelet2_extension/version.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
@@ -32,6 +33,9 @@ using tier4_map_msgs::msg::MapProjectorInfo;
 
 class Lanelet2MapLoaderNode : public rclcpp::Node
 {
+public:
+  static constexpr lanelet::autoware::Version version = lanelet::autoware::version;
+
 public:
   explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
 

map/map_loader/schema/lanelet2_map_loader.schema.json

@@ -6,6 +6,11 @@
     "lanelet2_map_loader": {
       "type": "object",
       "properties": {
+        "allow_unsupported_version": {
+          "type": "boolean",
+          "description": "Flag to load unsupported format_version anyway. If true, just prints warning.",
+          "default": "true"
+        },
         "center_line_resolution": {
           "type": "number",
           "description": "Resolution of the Lanelet center line [m]",

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -49,6 +49,7 @@
 #include <lanelet2_io/Io.h>
 #include <lanelet2_projection/UTM.h>
 
+#include <stdexcept>
 #include <string>
 
 Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options)
@@ -61,13 +62,15 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
     sub_map_projector_info_,
     [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); });
 
+  declare_parameter<bool>("allow_unsupported_version");
   declare_parameter<std::string>("lanelet2_map_path");
   declare_parameter<double>("center_line_resolution");
 }
 
 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_filename = get_parameter("lanelet2_map_path").as_string();
   const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
 
@@ -78,6 +81,31 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
     return;
   }
 
+  std::string format_version{"null"}, map_version{""};
+  lanelet::io_handlers::AutowareOsmParser::parseVersions(
+    lanelet2_filename, &format_version, &map_version);
+  if (format_version == "null" || format_version.empty() || !isdigit(format_version[0])) {
+    RCLCPP_WARN(
+      get_logger(),
+      "%s has no format_version(null) or non semver-style format_version(%s) information",
+      lanelet2_filename.c_str(), format_version.c_str());
+    if (!allow_unsupported_version) {
+      throw std::invalid_argument(
+        "allow_unsupported_version is false, so stop loading lanelet map");
+    }
+  } else if (const int map_major_ver = static_cast<int>(format_version[0] - '0');
+             map_major_ver > static_cast<int>(version)) {
+    RCLCPP_WARN(
+      get_logger(),
+      "format_version(%d) of the provided map(%s) is larger than the supported version(%d)",
+      map_major_ver, lanelet2_filename.c_str(), static_cast<int>(version));
+    if (!allow_unsupported_version) {
+      throw std::invalid_argument(
+        "allow_unsupported_version is false, so stop loading lanelet map");
+    }
+  }
+  RCLCPP_INFO(get_logger(), "Loaded map format_version: %s", format_version.c_str());
+
   // overwrite centerline
   lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);