From 11206bc57f4888bf985ffb1bc0be29269fa96552 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Tue, 11 Jun 2024 18:59:35 +0900
Subject: [PATCH] check major version in map_loader and route_handler

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../lanelet2_map_loader_node.cpp              | 22 +++++++++++--------
 .../src/route_handler.cpp                     | 11 ++++++++++
 2 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
index 8f1cb69230660..2f90c9a7a27f2 100644
--- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
+++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
@@ -93,15 +93,19 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
       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");
+  } 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());
diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp
index 213cca3bd5d80..ba785f4fab78a 100644
--- a/planning/autoware_route_handler/src/route_handler.cpp
+++ b/planning/autoware_route_handler/src/route_handler.cpp
@@ -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 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg)
   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);