feat(map_loader, route_handler)!: add format_version validation

map/map_loader/config/lanelet2_map_loader.param.yaml

@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
+    allow_unsupported_version: true # 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,23 +62,54 @@
     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();
 
   // load map from file
   const auto map = load_map(lanelet2_filename, *msg);
   if (!map) {
     RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published.");
     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 auto map_major_ver_opt = lanelet::io_handlers::parseMajorVersion(format_version);
+             map_major_ver_opt.has_value()) {
+    const auto map_major_ver = map_major_ver_opt.value();
+    if (map_major_ver > static_cast<uint64_t>(lanelet::autoware::version)) {
+      RCLCPP_WARN(
+        get_logger(),
+        "format_version(%ld) of the provided map(%s) is larger than the supported version(%ld)",
+        map_major_ver, lanelet2_filename.c_str(),
+        static_cast<uint64_t>(lanelet::autoware::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);
 

planning/autoware_route_handler/src/route_handler.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,6 +15,7 @@
 #include "autoware_route_handler/route_handler.hpp"
 
 #include <autoware_utils/math/normalization.hpp>
+#include <lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/route_checker.hpp>
@@ -186,6 +187,16 @@
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
     map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
+  const auto map_major_version_opt =
+    lanelet::io_handlers::parseMajorVersion(map_msg.version_map_format);
+  if (!map_major_version_opt) {
+    RCLCPP_WARN(
+      logger_, "setMap() for invalid version map: %s", map_msg.version_map_format.c_str());
+  } else if (map_major_version_opt.value() > static_cast<uint64_t>(lanelet::autoware::version)) {
+    RCLCPP_WARN(
+      logger_, "setMap() for a map(version %s) newer than lanelet2_extension support version(%d)",
+      map_msg.version_map_format.c_str(), static_cast<int>(lanelet::autoware::version));
+  }
 
   const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
     lanelet::Locations::Germany, lanelet::Participants::Vehicle);