From fd5f5edca752cee7f5f3418dda1c0a7ea635541d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 9 Jul 2024 22:43:00 +0900 Subject: [PATCH] feat(map_loader, route_handler)!: add format_version validation (#7074) Signed-off-by: Mamoru Sobue Co-authored-by: Yamato Ando --- .../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 | 32 +++++++++++++++++++ .../test/lanelet2_map_loader_launch.test.py | 1 + .../src/route_handler.cpp | 11 +++++++ 6 files changed, 54 insertions(+) diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 48152605ec0a2..48d392a1b8e66 100755 --- 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 index 8bfdbc5d5560c..e38d65201ee56 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 @@ -15,6 +15,7 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include #include #include #include @@ -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 index aae295f847ab2..a55050e4ed570 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 e2b2a052619e4..437c9e44cb7c1 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 #include +#include #include 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("allow_unsupported_version"); declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); declare_parameter("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(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(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 index 9f9a59f565e3f..0a29a74e90b06 100644 --- 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 index 0540fb1758802..9811f982f8e1b 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 +#include #include #include #include @@ -186,6 +187,16 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg) lanelet_map_ptr_ = std::make_shared(); 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(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(lanelet::autoware::version)); + } const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle);