feat(lanelet2_map_validator): enable to catch map loading issues such as invalid point elevations

map/autoware_lanelet2_map_validator/CMakeLists.txt

@@ -44,10 +44,6 @@ if(BUILD_TESTING)
     test/src/include
     src/include)
 
-  target_link_libraries(autoware_lanelet2_map_validator_test_lib
-    autoware_lanelet2_map_validator_lib
-  )
-
   # test for general lanelet2 map validators
   function(add_validation_test TEST_FILE)
     get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)
@@ -59,6 +55,7 @@ if(BUILD_TESTING)
     target_link_libraries(
       ${VALIDATION_NAME}_test
       autoware_lanelet2_map_validator_test_lib
+      autoware_lanelet2_map_validator_lib
     )
   endfunction()
 

map/autoware_lanelet2_map_validator/LICENSE

@@ -0,0 +1,27 @@
+This package is licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at:
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Portions of this package are based on code licensed under the BSD-3-Clause License:
+
+====================================================================
+Lanelet2 by FZI Forschungszentrum Informatik
+Copyright (c) [2018] [FZI Forschungszentrum Informatik]
+Licensed under the BSD-3-Clause License. See below for the full license text.
+
+BSD-3-Clause License:
+--------------------------------------------------------------------
+Copyright 2018 FZI Forschungszentrum Informatik
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+====================================================================

map/autoware_lanelet2_map_validator/README.md

@@ -359,3 +359,8 @@
 | vm-07-02 | Range of detecting pedestrians who enter the road       | (Not possible to validate because real-world correspondence cannot be determined programmatically.)                                                                                                                                                                                                                                                             |
 | vm-07-03 | Guardrails, guard pipes, fences                         | (Not possible to validate because real-world correspondence cannot be determined programmatically.)                                                                                                                                                                                                                                                             |
 | vm-07-04 | Ellipsoidal height                                      | (Not possible to validate because real-world correspondence cannot be determined programmatically?)                                                                                                                                                                                                                                                             |
+
+## Third-Party Components
+
+Portions of this software are derived from Lanelet2 by FZI Forschungszentrum Informatik, Karlsruhe, Germany, and are licensed under the BSD-3-Clause license.
+See the LICENSE file for the original license details.

map/autoware_lanelet2_map_validator/src/common/autoware_validator_osm_parser.cpp

@@ -0,0 +1,313 @@
+/*
+ * Copyright (c) 2018
+ * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
+ * All rights reserved.
+ *
+ * Modifications by [Taiki Yamada/Autoware Foundation], 2025
+ *
+ * This software is licensed under the BSD-3-Clause license.
+ * See the LICENSE file for details.
+ */
+
+#include "lanelet2_map_validator/autoware_validator_osm_parser.hpp"
+
+#include <lanelet2_core/utility/Utilities.h>
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/io_handlers/Factory.h>
+#include <lanelet2_io/io_handlers/OsmFile.h>
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace lanelet::io_handlers
+{
+using Errors = std::vector<std::string>;
+
+namespace
+{
+template <typename MapT>
+void registerIds(const MapT & map)
+{
+  if (!map.empty()) {
+    utils::registerId(map.rbegin()->first);
+  }
+}
+
+void testAndPrintLocaleWarning(ErrorMessages & errors)
+{
+  auto * decimalPoint = std::localeconv()->decimal_point;
+  if (decimalPoint == nullptr || *decimalPoint != '.') {
+    std::stringstream ss;
+    ss << "Warning: Current decimal point of the C locale is set to \""
+       << (decimalPoint == nullptr ? ' ' : *decimalPoint)
+       << "\". The loaded map will have wrong coordinates!\n";
+    errors.emplace_back(ss.str());
+    std::cerr << errors.back();
+  }
+}
+
+RegisterParser<AutowareValidatorOsmParser> regParser;
+}  // namespace
+
+std::unique_ptr<LaneletMap> AutowareValidatorOsmParser::parse(
+  const std::string & filename, ErrorMessages & errors) const
+{
+  // read xml
+  pugi::xml_document doc;
+  auto result = doc.load_file(filename.c_str());
+  if (!result) {
+    throw lanelet::ParseError(
+      std::string("Errors occured while parsing osm file: ") + result.description());
+  }
+  osm::Errors osmReadErrors;
+  testAndPrintLocaleWarning(osmReadErrors);
+  auto file = lanelet::osm::autoware::read(doc, &osmReadErrors);
+  auto map = fromOsmFile(file, errors);
+  // make sure ids in the file are known to Lanelet2 id management.
+  registerIds(file.nodes);
+  registerIds(file.ways);
+  registerIds(file.relations);
+  errors = utils::concatenate({osmReadErrors, errors});
+  return map;
+}
+}  // namespace lanelet::io_handlers
+
+namespace lanelet::osm
+{
+namespace keyword
+{
+constexpr const char * Osm = "osm";
+constexpr const char * Tag = "tag";
+constexpr const char * Key = "k";
+constexpr const char * Value = "v";
+constexpr const char * Node = "node";
+constexpr const char * Way = "way";
+constexpr const char * Relation = "relation";
+constexpr const char * Member = "member";
+constexpr const char * Role = "role";
+constexpr const char * Type = "type";
+constexpr const char * Nd = "nd";
+constexpr const char * Ref = "ref";
+constexpr const char * Id = "id";
+constexpr const char * Lat = "lat";
+constexpr const char * Lon = "lon";
+constexpr const char * Version = "version";
+constexpr const char * Visible = "visible";
+constexpr const char * Elevation = "ele";
+constexpr const char * Action = "action";
+constexpr const char * Delete = "delete";
+}  // namespace keyword
+
+struct UnresolvedRole
+{
+  Id relation{};
+  Id referencedRelation{};
+  Primitive ** location{};
+};
+
+Attributes tags(const pugi::xml_node & node)
+{
+  Attributes attributes;
+  for (auto tag = node.child(keyword::Tag); tag;  // NOLINT
+       tag = tag.next_sibling(keyword::Tag)) {
+    if (std::string(tag.attribute(keyword::Key).value()) == keyword::Elevation) {
+      continue;
+    }
+    attributes[tag.attribute(keyword::Key).value()] = tag.attribute(keyword::Value).value();
+  }
+  return attributes;
+}
+
+bool isDeleted(const pugi::xml_node & node)
+{
+  auto action = node.attribute(keyword::Action);
+  return action && std::string(action.value()) == keyword::Delete;  // NOLINT
+}
+
+void removeAndFixPlaceholders(
+  Primitive ** toRemove, Roles & fromRoles, std::vector<UnresolvedRole> & placeholders)
+{
+  // find other placeholders that we have to fix
+  auto remIt = std::find_if(fromRoles.begin(), fromRoles.end(), [&](const Role & role) {
+    return &role.second == toRemove;
+  });
+  assert(remIt != fromRoles.end());
+  std::vector<std::pair<size_t, Primitive **>> placeholderLocations;
+  for (auto it = fromRoles.begin(); it != fromRoles.end(); ++it) {
+    if (it->second == nullptr && remIt != it) {
+      auto idx = std::distance(fromRoles.begin(), it);
+      placeholderLocations.emplace_back(it > remIt ? idx - 1 : idx, &it->second);
+    }
+  }
+  fromRoles.erase(remIt);
+  if (placeholderLocations.empty()) {
+    return;  // nothing to update
+  }
+  // get the new locations
+  std::map<Primitive **, Primitive **> newLocations;
+  for (auto & loc : placeholderLocations) {
+    newLocations.emplace(
+      loc.second, &std::next(fromRoles.begin(), static_cast<int64_t>(loc.first))->second);
+  }
+  // adapt existing locations
+  for (auto & placeholder : placeholders) {
+    auto it = newLocations.find(placeholder.location);
+    if (it != newLocations.end()) {
+      placeholder.location = it->second;
+    }
+  }
+}
+
+namespace autoware
+{
+
+class AutowareValidatorOsmFileParser
+{
+public:
+  static File read(const pugi::xml_node & fileNode, Errors * errors = nullptr)
+  {
+    AutowareValidatorOsmFileParser autowareValidatorOsmParser;
+    File file;
+    auto osmNode = fileNode.child(keyword::Osm);
+    file.nodes = autowareValidatorOsmParser.readNodes(osmNode);
+    file.ways = autowareValidatorOsmParser.readWays(osmNode, file.nodes);
+    file.relations = autowareValidatorOsmParser.readRelations(osmNode, file.nodes, file.ways);
+    if (errors != nullptr) {
+      *errors = autowareValidatorOsmParser.errors_;
+    }
+    return file;
+  }
+
+private:
+  Nodes readNodes(const pugi::xml_node & osmNode)
+  {
+    Nodes nodes;
+    for (auto node = osmNode.child(keyword::Node); node;  // NOLINT
+         node = node.next_sibling(keyword::Node)) {
+      if (isDeleted(node)) {
+        continue;
+      }
+      const auto id = node.attribute(keyword::Id).as_llong(InvalId);
+      const auto attributes = tags(node);
+      const auto lat = node.attribute(keyword::Lat).as_double(0.);
+      const auto lon = node.attribute(keyword::Lon).as_double(0.);
+      const auto elevationNode =
+        node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation);
+      if (!elevationNode) {
+        reportParseError(id, "Elevation tag is not defined for the given node.");
+      } else if (!elevationNode.attribute(keyword::Value)) {
+        reportParseError(id, "Elevation tag exists but does not have a value.");
+      }
+      const auto ele = elevationNode.attribute(keyword::Value).as_double(.0);
+
+      nodes[id] = Node{id, attributes, {lat, lon, ele}};
+    }
+    return nodes;
+  }
+
+  Ways readWays(const pugi::xml_node & osmNode, Nodes & nodes)
+  {
+    Ways ways;
+    for (auto node = osmNode.child(keyword::Way); node;  // NOLINT
+         node = node.next_sibling(keyword::Way)) {
+      if (isDeleted(node)) {
+        continue;
+      }
+      const auto id = node.attribute(keyword::Id).as_llong(InvalId);
+      const auto attributes = tags(node);
+      const auto nodeIds = [&node] {
+        Ids ids;
+        for (auto refNode = node.child(keyword::Nd); refNode;  // NOLINT
+             refNode = refNode.next_sibling(keyword::Nd)) {
+          ids.push_back(refNode.attribute(keyword::Ref).as_llong());
+        }
+        return ids;
+      }();
+      std::vector<Node *> wayNodes;
+      try {
+        wayNodes =
+          utils::transform(nodeIds, [&nodes](const auto & elem) { return &nodes.at(elem); });
+      } catch (std::out_of_range &) {
+        reportParseError(id, "Way references nonexisting points");
+      }
+      ways[id] = Way{id, attributes, wayNodes};
+    }
+    return ways;
+  }
+
+  Relations readRelations(const pugi::xml_node & osmNode, Nodes & nodes, Ways & ways)
+  {
+    Relations relations;
+    // Two-pass approach: We can resolve all roles except where relations reference relations. We
+    // insert a dummy nullptr and resolve that later on.
+    std::vector<UnresolvedRole> unresolvedRoles;
+    for (auto node = osmNode.child(keyword::Relation); node;  // NOLINT
+         node = node.next_sibling(keyword::Relation)) {
+      if (isDeleted(node)) {
+        continue;
+      }
+      const auto id = node.attribute(keyword::Id).as_llong(InvalId);
+      const auto attributes = tags(node);
+      auto & relation = relations.emplace(id, Relation{id, attributes, {}}).first->second;
+
+      // resolve members
+      auto & roles = relation.members;
+      for (auto member = node.child(keyword::Member); member;  // NOLINT
+           member = member.next_sibling(keyword::Member)) {
+        Id memberId = member.attribute(keyword::Ref).as_llong();
+        const std::string role = member.attribute(keyword::Role).value();
+        const std::string type = member.attribute(keyword::Type).value();
+        try {
+          if (type == keyword::Node) {
+            roles.emplace_back(role, &nodes.at(memberId));
+          } else if (type == keyword::Way) {
+            roles.emplace_back(role, &ways.at(memberId));
+          } else if (type == keyword::Relation) {
+            // insert a placeholder and store a pointer to it for the second pass
+            roles.emplace_back(role, nullptr);
+            unresolvedRoles.push_back(UnresolvedRole{id, memberId, &roles.back().second});
+          }
+        } catch (std::out_of_range &) {
+          reportParseError(id, "Relation has nonexistent member " + std::to_string(memberId));
+        }
+      }
+    }
+
+    // now resolve all unresolved roles that point to other relations
+    for (const auto & unresolvedRole : unresolvedRoles) {
+      try {
+        assert(*unresolvedRole.location == nullptr);
+        *unresolvedRole.location = &relations.at(unresolvedRole.referencedRelation);
+      } catch (std::out_of_range &) {
+        reportParseError(
+          unresolvedRole.relation, "Relation references nonexistent relation " +
+                                     std::to_string(unresolvedRole.referencedRelation));
+        // now it gets ugly: find placeholder and remove it. Fix all other placeholders because the
+        // pointers are invalidated after moving. This is inefficent, but its the fault of the guy
+        // loading an invalid map, not ours.
+        auto & relation = relations.at(unresolvedRole.relation);
+        removeAndFixPlaceholders(unresolvedRole.location, relation.members, unresolvedRoles);
+      }
+    }
+    return relations;
+  }
+
+  AutowareValidatorOsmFileParser() = default;
+  void reportParseError(Id id, const std::string & what)
+  {
+    auto errstr = "Error reading primitive with id " + std::to_string(id) + " from file: " + what;
+    errors_.push_back(errstr);
+  }
+  Errors errors_;
+};
+
+File read(pugi::xml_document & node, Errors * errors)
+{
+  return AutowareValidatorOsmFileParser::read(node, errors);
+}
+}  // namespace autoware
+}  // namespace lanelet::osm

map/autoware_lanelet2_map_validator/src/common/map_loader.cpp

@@ -65,19 +65,21 @@ loadAndValidateMap(
     if (!projector) {
       errors.push_back("No valid map projection type specified!");
     } else {
-      map = lanelet::load(map_file, *projector, &errors);
+      map = lanelet::load(map_file, "autoware_validator_osm_handler", *projector, &errors);
     }
     if (!map) {
-      errors.push_back("Failed to load map!");
+      errors.push_back("Failed to load the entire map!");
     }
     issues.emplace_back("general", utils::transform(errors, [](auto & error) {
                           return lanelet::validation::Issue(
-                            lanelet::validation::Severity::Error, error);
+                            lanelet::validation::Severity::Error,
+                            lanelet::validation::Primitive::Primitive, lanelet::InvalId, error);
                         }));
   } catch (lanelet::LaneletError & err) {
     issues.emplace_back("general", utils::transform(errors, [](auto & error) {
                           return lanelet::validation::Issue(
-                            lanelet::validation::Severity::Error, error);
+                            lanelet::validation::Severity::Error,
+                            lanelet::validation::Primitive::Primitive, lanelet::InvalId, error);
                         }));
   }