From f0bddc5c0c6ba5ec187ffb4b4fc39ffad4ea2e02 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Mon, 20 May 2024 18:25:29 +0900
Subject: [PATCH] feat(map_loader): add format_version validation

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../config/lanelet2_map_loader.param.yaml     |  1 +
 .../map_loader/lanelet2_map_loader_node.hpp   |  4 +++
 .../schema/lanelet2_map_loader.schema.json    |  5 ++++
 .../lanelet2_map_loader_node.cpp              | 28 +++++++++++++++++++
 4 files changed, 38 insertions(+)

diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
index b830e038f1de2..b3d1925e6e057 100755
--- a/map/map_loader/config/lanelet2_map_loader.param.yaml
+++ b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
+    allow_unsupported_version: true # flag to load unsupported version anyway
     center_line_resolution: 5.0         # [m]
     lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path
diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
index 0adc7612e9f61..79cc870526f55 100644
--- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
+++ b/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_auto_mapping_msgs/msg/had_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);
 
diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json
index fa2b4d363ff92..2cd112b0a0670 100644
--- a/map/map_loader/schema/lanelet2_map_loader.schema.json
+++ b/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]",
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 617f3dd503ce0..c63eb256a3821 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
@@ -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,6 +62,7 @@ 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");
 }
@@ -68,6 +70,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
 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: %d", static_cast<int>(version));
+
   // overwrite centerline
   lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);