feat(map_loader, route_handler)!: add format_version validation (auto…

…warefoundation#7074) Signed-off-by: Mamoru Sobue <[email protected]> Co-authored-by: Yamato Ando <[email protected]>
Ariiees · Jul 22, 2024 · fd5f5ed · fd5f5ed
1 parent 23cf515
commit fd5f5ed
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 0 deletions.
diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    allow_unsupported_version: true             # flag to load unsupported format_version anyway. If true, just prints warning.
     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) # 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
@@ -15,6 +15,7 @@
 #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 
+#include <autoware_lanelet2_extension/version.hpp>
 #include <component_interface_specs/map.hpp>
 #include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -29,6 +30,9 @@
 
 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
@@ -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
@@ -49,6 +49,7 @@
 #include <lanelet2_io/Io.h>
 #include <lanelet2_projection/UTM.h>
 
+#include <stdexcept>
 #include <string>
 
 using autoware_map_msgs::msg::LaneletMapBin;
@@ -64,6 +65,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");
   declare_parameter<bool>("use_waypoints");
@@ -72,6 +74,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();
   const auto use_waypoints = get_parameter("use_waypoints").as_bool();
@@ -83,6 +86,35 @@ 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 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
   if (use_waypoints) {
     lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution, false);

diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py
@@ -39,6 +39,7 @@ def generate_test_description():
                 "lanelet2_map_path": lanelet2_map_path,
                 "center_line_resolution": 5.0,
                 "use_waypoints": True,
+                "allow_unsupported_version": True,
             }
         ],
     )

diff --git 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/universe_utils/geometry/geometry.hpp>
+#include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_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);