From 44d429d192fef0b0af672a273f7b9c6ee505e6c2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Jul 2024 12:48:02 +0000 Subject: [PATCH] style(pre-commit): autofix --- .../io/autoware_multi_osm_parser.hpp | 234 ++-- .../lib/autoware_multi_osm_parser.cpp | 1004 ++++++++--------- 2 files changed, 619 insertions(+), 619 deletions(-) diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/io/autoware_multi_osm_parser.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/io/autoware_multi_osm_parser.hpp index 3b208b8..0f98ca7 100644 --- a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/io/autoware_multi_osm_parser.hpp +++ b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/io/autoware_multi_osm_parser.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ -#define LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ +#ifndef AUTOWARE_LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ +#define AUTOWARE_LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ #include "lanelet2_io/Exceptions.h" #include "lanelet2_io/io_handlers/Factory.h" @@ -36,153 +36,153 @@ using std::string_literals::operator""s; namespace lanelet { - namespace io_handlers - { - - class MultiOsmParser : public Parser - { - public: - using Parser::Parser; - - std::unique_ptr parse( - const std::string & lanelet2_filename, ErrorMessages & errors) const override; - - std::unique_ptr parse( - const std::vector & lanelet2_filenames, ErrorMessages & errors) const; - - std::unique_ptr fromOsmFile(const osm::File & file, ErrorMessages & errors) const; - - std::unique_ptr fromOsmFile( - const std::map & files_map, ErrorMessages & errors) const; +namespace io_handlers +{ + +class MultiOsmParser : public Parser +{ +public: + using Parser::Parser; + + std::unique_ptr parse( + const std::string & lanelet2_filename, ErrorMessages & errors) const override; + + std::unique_ptr parse( + const std::vector & lanelet2_filenames, ErrorMessages & errors) const; - static void parseVersions( - const std::string & filename, std::string * format_version, std::string * map_version); + std::unique_ptr fromOsmFile(const osm::File & file, ErrorMessages & errors) const; - static constexpr const char * extension() { return ".osm"; } - - static constexpr const char * name() { return "autoware_multi_osm_handler"; } - }; + std::unique_ptr fromOsmFile( + const std::map & files_map, ErrorMessages & errors) const; - namespace - { - RegisterParser regParser; + static void parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version); - class MultiFileLoader - { - public: - static std::unique_ptr loadMap( - const osm::File & file, const Projector & projector, ErrorMessages & errors) - { - MultiFileLoader loader; - loader.loadNodes(file.nodes, projector); - loader.loadWays(file.ways); - auto laneletsWithRelation = loader.loadLanelets(file.relations); - auto areasWithRelation = loader.loadAreas(file.relations); - loader.loadRegulatoryElements(file.relations); - loader.addRegulatoryElements(laneletsWithRelation); - loader.addRegulatoryElements(areasWithRelation); - errors = std::move(loader.errors_); + static constexpr const char * extension() { return ".osm"; } + + static constexpr const char * name() { return "autoware_multi_osm_handler"; } +}; + +namespace +{ +RegisterParser regParser; + +class MultiFileLoader +{ +public: + static std::unique_ptr loadMap( + const osm::File & file, const Projector & projector, ErrorMessages & errors) + { + MultiFileLoader loader; + loader.loadNodes(file.nodes, projector); + loader.loadWays(file.ways); + auto laneletsWithRelation = loader.loadLanelets(file.relations); + auto areasWithRelation = loader.loadAreas(file.relations); + loader.loadRegulatoryElements(file.relations); + loader.addRegulatoryElements(laneletsWithRelation); + loader.addRegulatoryElements(areasWithRelation); + errors = std::move(loader.errors_); - return std::make_unique( - loader.lanelets_, loader.areas_, loader.regulatoryElements_, loader.polygons_, - loader.lineStrings_, loader.points_); - } + return std::make_unique( + loader.lanelets_, loader.areas_, loader.regulatoryElements_, loader.polygons_, + loader.lineStrings_, loader.points_); + } - static std::unique_ptr loadMap( - const std::map & files_map, const Projector & projector, - ErrorMessages & errors) - { - MultiFileLoader loader; + static std::unique_ptr loadMap( + const std::map & files_map, const Projector & projector, + ErrorMessages & errors) + { + MultiFileLoader loader; - std::for_each(files_map.begin(), files_map.end(), [&loader, &projector](const auto & file) { - loader.loadNodes(file.second.nodes, projector); - }); + std::for_each(files_map.begin(), files_map.end(), [&loader, &projector](const auto & file) { + loader.loadNodes(file.second.nodes, projector); + }); - std::for_each(files_map.begin(), files_map.end(), [&loader](const auto & file) { - loader.loadWays(file.second.ways); - }); + std::for_each(files_map.begin(), files_map.end(), [&loader](const auto & file) { + loader.loadWays(file.second.ways); + }); - LaneletsWithRegulatoryElements laneletsWithRelation; - for (const auto & file : files_map) { - laneletsWithRelation = loader.loadLanelets(file.second.relations); - } + LaneletsWithRegulatoryElements laneletsWithRelation; + for (const auto & file : files_map) { + laneletsWithRelation = loader.loadLanelets(file.second.relations); + } - AreasWithRegulatoryElements areasWithRelation; - for (const auto & file : files_map) { - areasWithRelation = loader.loadAreas(file.second.relations); - } + AreasWithRegulatoryElements areasWithRelation; + for (const auto & file : files_map) { + areasWithRelation = loader.loadAreas(file.second.relations); + } - for (const auto & file : files_map) { - loader.loadRegulatoryElements(file.second.relations); - } - loader.addRegulatoryElements(laneletsWithRelation); - loader.addRegulatoryElements(areasWithRelation); + for (const auto & file : files_map) { + loader.loadRegulatoryElements(file.second.relations); + } + loader.addRegulatoryElements(laneletsWithRelation); + loader.addRegulatoryElements(areasWithRelation); - errors = std::move(loader.errors_); - return std::make_unique( - loader.lanelets_, loader.areas_, loader.regulatoryElements_, loader.polygons_, - loader.lineStrings_, loader.points_); - } + errors = std::move(loader.errors_); + return std::make_unique( + loader.lanelets_, loader.areas_, loader.regulatoryElements_, loader.polygons_, + loader.lineStrings_, loader.points_); + } - private: - std::vector errors_; - LaneletLayer::Map lanelets_; - AreaLayer::Map areas_; - RegulatoryElementLayer::Map regulatoryElements_; - PolygonLayer::Map polygons_; - LineStringLayer::Map lineStrings_; - PointLayer::Map points_; +private: + std::vector errors_; + LaneletLayer::Map lanelets_; + AreaLayer::Map areas_; + RegulatoryElementLayer::Map regulatoryElements_; + PolygonLayer::Map polygons_; + LineStringLayer::Map lineStrings_; + PointLayer::Map points_; - public: - template - using PrimitiveWithRegulatoryElement = std::pair; +public: + template + using PrimitiveWithRegulatoryElement = std::pair; - template - using PrimitivesWithRegulatoryElement = std::vector>; + template + using PrimitivesWithRegulatoryElement = std::vector>; - using AreasWithRegulatoryElements = PrimitivesWithRegulatoryElement; - using LaneletsWithRegulatoryElements = PrimitivesWithRegulatoryElement; + using AreasWithRegulatoryElements = PrimitivesWithRegulatoryElement; + using LaneletsWithRegulatoryElements = PrimitivesWithRegulatoryElement; - MultiFileLoader() = default; + MultiFileLoader() = default; - void loadNodes(const lanelet::osm::Nodes & nodes, const Projector & projector); + void loadNodes(const lanelet::osm::Nodes & nodes, const Projector & projector); - void loadWays(const lanelet::osm::Ways & ways); + void loadWays(const lanelet::osm::Ways & ways); - LaneletsWithRegulatoryElements loadLanelets(const lanelet::osm::Relations & relations); + LaneletsWithRegulatoryElements loadLanelets(const lanelet::osm::Relations & relations); - AreasWithRegulatoryElements loadAreas(const lanelet::osm::Relations & relations); + AreasWithRegulatoryElements loadAreas(const lanelet::osm::Relations & relations); - void loadRegulatoryElements(const osm::Relations & relations); + void loadRegulatoryElements(const osm::Relations & relations); - template - void addRegulatoryElements(std::vector> & addTos); + template + void addRegulatoryElements(std::vector> & addTos); - template - bool isType(const lanelet::osm::Relation & relation); + template + bool isType(const lanelet::osm::Relation & relation); - static lanelet::AttributeMap getAttributes(const lanelet::osm::Attributes & osmAttributes); + static lanelet::AttributeMap getAttributes(const lanelet::osm::Attributes & osmAttributes); - LineString3d getLaneletBorder(const osm::Relation & llElem, const std::string & role); + LineString3d getLaneletBorder(const osm::Relation & llElem, const std::string & role); - LineStrings3d getLinestrings(const osm::Roles & roles, const std::string & roleName, Id refId); + LineStrings3d getLinestrings(const osm::Roles & roles, const std::string & roleName, Id refId); - LineStrings3d getOuterRing(const osm::Relation & area); + LineStrings3d getOuterRing(const osm::Relation & area); - std::vector getInnerRing(const osm::Relation & area); + std::vector getInnerRing(const osm::Relation & area); - RuleParameterMap getRulesForRegulatoryElement(Id currElemId, const osm::Roles & roles); + RuleParameterMap getRulesForRegulatoryElement(Id currElemId, const osm::Roles & roles); - std::vector assembleBoundary(LineStrings3d lineStrings, Id id); + std::vector assembleBoundary(LineStrings3d lineStrings, Id id); - template - PrimT getOrGetDummy( - const typename std::unordered_map & map, Id id, Id currentPrimitiveId); + template + PrimT getOrGetDummy( + const typename std::unordered_map & map, Id id, Id currentPrimitiveId); - void parserError(Id id, const std::string & what); - }; - } // namespace - } // namespace io_handlers + void parserError(Id id, const std::string & what); +}; +} // namespace +} // namespace io_handlers }; // namespace lanelet -#endif // LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ +#endif // AUTOWARE_LANELET2_EXTENSION__IO__AUTOWARE_MULTI_OSM_PARSER_HPP_ diff --git a/autoware_lanelet2_extension/lib/autoware_multi_osm_parser.cpp b/autoware_lanelet2_extension/lib/autoware_multi_osm_parser.cpp index 7d7ef8f..dc5b7d7 100644 --- a/autoware_lanelet2_extension/lib/autoware_multi_osm_parser.cpp +++ b/autoware_lanelet2_extension/lib/autoware_multi_osm_parser.cpp @@ -30,520 +30,520 @@ namespace lanelet { - namespace io_handlers - { - using Errors = std::vector; - - template - using PrimitiveWithRegulatoryElement = std::pair; - - template - using PrimitivesWithRegulatoryElement = std::vector>; - - using AreasWithRegulatoryElements = PrimitivesWithRegulatoryElement; - using LaneletsWithRegulatoryElements = PrimitivesWithRegulatoryElement; - - bool isValid(const LineStrings3d & lss) - { - BasicPolygon2d ls(utils::concatenate( - lss, [](const auto & elem) { return traits::to2D(elem).basicLineString(); })); - return boost::geometry::is_valid(ls); - } - - void reverse(LineStrings3d & lss) - { - for (auto & ls : lss) { - ls = ls.invert(); - } - std::reverse(lss.begin(), lss.end()); - } - - template - PrimT getDummy(Id id) - { - return PrimT(id); - } - - template <> - RegulatoryElementPtr getDummy(Id id) - { - return std::make_shared(std::make_shared(id)); - } - - Errors buildErrorMessage(const std::string & errorIntro, const Errors & errors) - { - if (errors.empty()) { - return {}; - } - Errors message{errorIntro}; - message.reserve(errors.size() + 1); - for (const auto & error : errors) { - message.push_back("\t- " + error); - } - return message; - } - - void MultiFileLoader::loadNodes( - const lanelet::osm::Nodes & nodes, const lanelet::Projector & projector) - { - for (const auto & nodeElem : nodes) { - const auto & node = nodeElem.second; - try { - points_.emplace( - node.id, Point3d(node.id, projector.forward(node.point), getAttributes(node.attributes))); - } catch (ForwardProjectionError & e) { - parserError(node.id, e.what()); - } - } - } - - void MultiFileLoader::loadWays(const lanelet::osm::Ways & ways) - { - for (const auto & wayElem : ways) { - const auto & way = wayElem.second; - // reconstruct points - Points3d points; - points = utils::transform(way.nodes, [this, &way](const auto & n) { - return this->getOrGetDummy(points_, n->id, way.id); - }); - if (points.empty()) { - parserError(way.id, "Ways must have at least one point!"); - continue; - } - - const auto id = way.id; - const auto attributes = getAttributes(way.attributes); - - // determine area or way - auto isArea = attributes.find(AttributeNamesString::Area); - if (isArea != attributes.end() && isArea->second.asBool().get_value_or(false)) { - polygons_.emplace(id, Polygon3d(id, points, attributes)); - } else { - lineStrings_.emplace(id, LineString3d(id, points, attributes)); - } - } - } - - LaneletsWithRegulatoryElements MultiFileLoader::loadLanelets( - const lanelet::osm::Relations & relations) - { - // The regulatory elements are not parsed yet. We store lanelets with one - // for - // later. - LaneletsWithRegulatoryElements llWithRegulatoryElement; - for (const auto & relElem : relations) { - const auto & llElem = relElem.second; - if (!isType(llElem)) { - continue; - } - const auto id = llElem.id; - const auto attributes = getAttributes(llElem.attributes); - auto left = getLaneletBorder(llElem, RoleNameString::Left); - auto right = getLaneletBorder(llElem, RoleNameString::Right); - - // correct their orientation - std::tie(left, right) = geometry::align(left, right); - - // look for optional centerline - Lanelet lanelet(id, left, right, attributes); - if (findRole(llElem.members, RoleNameString::Centerline) != llElem.members.end()) { - auto center = getLaneletBorder(llElem, RoleNameString::Centerline); - lanelet.setCenterline(center); - } - - lanelets_.emplace(id, lanelet); - - // check for regulatory elements - if (findRole(llElem.members, RoleNameString::RegulatoryElement) != llElem.members.end()) { - llWithRegulatoryElement.push_back(std::make_pair(lanelet, &llElem)); - } - } - return llWithRegulatoryElement; - } - - AreasWithRegulatoryElements MultiFileLoader::loadAreas(const lanelet::osm::Relations & relations) - { - // regElems are not parsed yet. We store areas with one for later. - AreasWithRegulatoryElements arWithRegulatoryElement; - for (const auto & relElem : relations) { - const auto & arElem = relElem.second; - if (!isType(arElem)) { - continue; - } - const auto id = arElem.id; - const auto attributes = getAttributes(arElem.attributes); - - auto outerRing = getOuterRing(arElem); - if (outerRing.empty()) { - // getOuter ring repors errors for us - continue; - } - - Area area(id, outerRing, getInnerRing(arElem), attributes); - areas_.emplace(id, area); - - // check for regulatory elements - if (findRole(arElem.members, RoleNameString::RegulatoryElement) != arElem.members.end()) { - arWithRegulatoryElement.push_back(std::make_pair(area, &arElem)); - } - } - return arWithRegulatoryElement; - } - - void MultiFileLoader::loadRegulatoryElements(const osm::Relations & relations) - { - for (const auto & relElem : relations) { - const auto & regElem = relElem.second; - if (!isType(regElem)) { - continue; - } - const auto id = regElem.id; - const auto attributes = getAttributes(regElem.attributes); - const auto type = attributes.find(AttributeName::Subtype); - if (type == attributes.end()) { - parserError(id, "Regulatory element has no 'subtype' tag."); - continue; - } - auto rules = getRulesForRegulatoryElement(id, regElem.members); - auto regelemData = std::make_shared(id, rules, attributes); - auto regelemType = type->second.value(); - try { - auto regElement = RegulatoryElementFactory::create(regelemType, regelemData); - regulatoryElements_.emplace(id, regElement); - } catch (std::exception & e) { - parserError( - id, "Creating a regulatory element of type "s + regelemType + " failed: " + e.what()); - } - } - } - - template - void MultiFileLoader::addRegulatoryElements( - std::vector> & addTos) - { - for (auto & addTo : addTos) { - osm::forEachMember( - addTo.second->members, RoleNameString::RegulatoryElement, [&](const osm::Role & role) { - auto regElem = getOrGetDummy(regulatoryElements_, role.second->id, addTo.first.id()); - addTo.first.addRegulatoryElement(regElem); - }); - } - } - - template - bool MultiFileLoader::isType(const lanelet::osm::Relation & relation) - { - auto attr = relation.attributes.find(AttributeNamesString::Type); - return attr != relation.attributes.end() && attr->second == Type; - } - - lanelet::AttributeMap MultiFileLoader::getAttributes(const lanelet::osm::Attributes & osmAttributes) - { - lanelet::AttributeMap attributes; - for (const auto & osmAttr : osmAttributes) { - attributes.insert(std::make_pair(osmAttr.first, lanelet::Attribute(osmAttr.second))); - } - return attributes; - } - - LineString3d MultiFileLoader::getLaneletBorder( - const osm::Relation & llElem, const std::string & role) - { - size_t numMembers = 0; - osm::forEachMember(llElem.members, role, [&](auto & /*role*/) { ++numMembers; }); - if (numMembers != 1) { - parserError(llElem.id, "Lanelet has not exactly one "s + role + " border!"); - return LineString3d(llElem.id); - } - auto member = osm::findRole(llElem.members, role); - if (member->second->type() != AttributeValueString::Way) { - parserError(llElem.id, "Lanelet "s + role + " border is not of type way!"); - return LineString3d(llElem.id); - } - return getOrGetDummy(lineStrings_, member->second->id, llElem.id); - } - - LineStrings3d MultiFileLoader::getLinestrings( - const osm::Roles & roles, const std::string & roleName, lanelet::Id refId) - { - LineStrings3d linestrings; - osm::forEachMember(roles, roleName, [&](auto & member) { - if (member.second->type() != AttributeValueString::Way) { - auto msg = roleName + " ring must consist of ways but id " + - std::to_string(member.second->id) + " is of type " + member.second->type() + "!"; - msg[0] = std::toupper(msg[0]); - this->parserError(refId, msg); - return; - } - auto elem = lineStrings_.find(member.second->id); - if (elem == lineStrings_.end()) { - this->parserError( - refId, "Failed to get id "s + std::to_string(member.second->id) + " from map"); - return; - } - linestrings.push_back(elem->second); - }); - return linestrings; - } +namespace io_handlers +{ +using Errors = std::vector; - LineStrings3d MultiFileLoader::getOuterRing(const osm::Relation & area) - { - auto outerLs = getLinestrings(area.members, RoleNameString::Outer, area.id); - if (outerLs.empty()) { - parserError(area.id, "Areas must have at least one outer border!"); - return {}; - } - auto outerRings = assembleBoundary(outerLs, area.id); - if (outerRings.size() != 1) { - parserError(area.id, "Areas must have exactly one outer ring!"); - return {}; - } - return outerRings.front(); - } +template +using PrimitiveWithRegulatoryElement = std::pair; - std::vector MultiFileLoader::getInnerRing(const osm::Relation & area) - { - auto innerLs = getLinestrings(area.members, RoleNameString::Inner, area.id); - return assembleBoundary(innerLs, area.id); - } +template +using PrimitivesWithRegulatoryElement = std::vector>; - RuleParameterMap MultiFileLoader::getRulesForRegulatoryElement( - lanelet::Id currElemId, const osm::Roles & roles) - { - RuleParameterMap rules; - for (const auto & memberPair : roles) { - const auto & member = memberPair.second; - if (member->type() == AttributeValueString::Node) { - auto newMember = getOrGetDummy(points_, member->id, currElemId); - rules[memberPair.first].emplace_back(newMember); - } else if (member->type() == AttributeValueString::Way) { - // can either be linestring or polygon - if (polygons_.find(member->id) != polygons_.end()) { - auto newMember = getOrGetDummy(polygons_, member->id, currElemId); - rules[memberPair.first].emplace_back(newMember); - } else { - auto newMember = getOrGetDummy(lineStrings_, member->id, currElemId); - rules[memberPair.first].emplace_back(newMember); - } - } else if (member->type() == AttributeValueString::Relation) { - // could be lanelet or area. regulatory element is not allowed. - auto type = member->attributes.find(AttributeNamesString::Type); - if (type == member->attributes.end()) { - parserError( - currElemId, "Relation refers to another relation "s + std::to_string(member->id) + - " without a type tag!"); - } else if (type->second == AttributeValueString::Lanelet) { - auto newMember = getOrGetDummy(lanelets_, member->id, currElemId); - rules[memberPair.first].emplace_back(newMember); - } else if (type->second == AttributeValueString::Multipolygon) { - auto newMember = getOrGetDummy(areas_, member->id, currElemId); - rules[memberPair.first].emplace_back(newMember); - } else if (type->second == AttributeValueString::RegulatoryElement) { - parserError( - currElemId, - "Regulatory element refers to another " - "regulatory element. This is not " - "supported."); - } else { - parserError( - currElemId, "Member of regulatory_element has unsupported type "s + type->second); - } - } - } - return rules; - } +using AreasWithRegulatoryElements = PrimitivesWithRegulatoryElement; +using LaneletsWithRegulatoryElements = PrimitivesWithRegulatoryElement; - std::vector MultiFileLoader::assembleBoundary( - lanelet::LineStrings3d lineStrings, lanelet::Id id) - { - std::reverse(lineStrings.begin(), lineStrings.end()); // its easier to pop from a vector... - std::vector rings; - rings.emplace_back(LineStrings3d()); - while (!lineStrings.empty()) { - auto & currRing = rings.back(); - if (currRing.empty()) { - currRing.push_back(lineStrings.back()); - lineStrings.pop_back(); - } else { - const auto lastId = currRing.back().back().id(); - auto elem = - std::find_if(lineStrings.rbegin(), lineStrings.rend(), [lastId](const auto & elem) { - return elem.back().id() == lastId || elem.front().id() == lastId; - }); - // we are unable to close the current ring - if (elem == lineStrings.rend()) { - parserError( - id, - "Could not complete boundary around linestring " + std::to_string(currRing.back().id())); - rings.back() = LineStrings3d(); - continue; - } - // we found the matching next linestring. add it in the correct order - auto newLineString = *elem; - lineStrings.erase(std::next(elem).base()); - if (newLineString.back().id() == lastId) { - newLineString = newLineString.invert(); - } - currRing.push_back(newLineString); - } - - // check if we closed the ring - if (currRing.back().back().id() == currRing.front().front().id()) { - // wohoo. Check the clockwise requirement. - if (!isValid(currRing)) { - reverse(currRing); - if (!isValid(currRing)) { - // most probably self-intersecting... - parserError(id, "Failed to generate boundary (self-intersecting?)"); - rings.pop_back(); - } - } - rings.emplace_back(LineStrings3d()); - } - } - rings.pop_back(); // last ring will be empty or invalid - return rings; - } +bool isValid(const LineStrings3d & lss) +{ + BasicPolygon2d ls(utils::concatenate( + lss, [](const auto & elem) { return traits::to2D(elem).basicLineString(); })); + return boost::geometry::is_valid(ls); +} - template - PrimT MultiFileLoader::getOrGetDummy( - const typename std::unordered_map & map, lanelet::Id id, - lanelet::Id currentPrimitiveId) - { - try { - return map.at(id); - } catch (std::out_of_range &) { - parserError(currentPrimitiveId, "Failed to get id "s + std::to_string(id) + " from map"); - return getDummy(id); - } - } +void reverse(LineStrings3d & lss) +{ + for (auto & ls : lss) { + ls = ls.invert(); + } + std::reverse(lss.begin(), lss.end()); +} + +template +PrimT getDummy(Id id) +{ + return PrimT(id); +} - void MultiFileLoader::parserError(lanelet::Id id, const std::string & what) - { - auto errstr = "Error parsing primitive "s + std::to_string(id) + ": " + what; - errors_.push_back(errstr); - } +template <> +RegulatoryElementPtr getDummy(Id id) +{ + return std::make_shared(std::make_shared(id)); +} - template - void registerIds(const MapT & map) - { - if (!map.empty()) { - utils::registerId(map.rbegin()->first); - } - } +Errors buildErrorMessage(const std::string & errorIntro, const Errors & errors) +{ + if (errors.empty()) { + return {}; + } + Errors message{errorIntro}; + message.reserve(errors.size() + 1); + for (const auto & error : errors) { + message.push_back("\t- " + error); + } + return message; +} + +void MultiFileLoader::loadNodes( + const lanelet::osm::Nodes & nodes, const lanelet::Projector & projector) +{ + for (const auto & nodeElem : nodes) { + const auto & node = nodeElem.second; + try { + points_.emplace( + node.id, Point3d(node.id, projector.forward(node.point), getAttributes(node.attributes))); + } catch (ForwardProjectionError & e) { + parserError(node.id, e.what()); + } + } +} + +void MultiFileLoader::loadWays(const lanelet::osm::Ways & ways) +{ + for (const auto & wayElem : ways) { + const auto & way = wayElem.second; + // reconstruct points + Points3d points; + points = utils::transform(way.nodes, [this, &way](const auto & n) { + return this->getOrGetDummy(points_, n->id, way.id); + }); + if (points.empty()) { + parserError(way.id, "Ways must have at least one point!"); + continue; + } + + const auto id = way.id; + const auto attributes = getAttributes(way.attributes); + + // determine area or way + auto isArea = attributes.find(AttributeNamesString::Area); + if (isArea != attributes.end() && isArea->second.asBool().get_value_or(false)) { + polygons_.emplace(id, Polygon3d(id, points, attributes)); + } else { + lineStrings_.emplace(id, LineString3d(id, points, attributes)); + } + } +} + +LaneletsWithRegulatoryElements MultiFileLoader::loadLanelets( + const lanelet::osm::Relations & relations) +{ + // The regulatory elements are not parsed yet. We store lanelets with one + // for + // later. + LaneletsWithRegulatoryElements llWithRegulatoryElement; + for (const auto & relElem : relations) { + const auto & llElem = relElem.second; + if (!isType(llElem)) { + continue; + } + const auto id = llElem.id; + const auto attributes = getAttributes(llElem.attributes); + auto left = getLaneletBorder(llElem, RoleNameString::Left); + auto right = getLaneletBorder(llElem, RoleNameString::Right); + + // correct their orientation + std::tie(left, right) = geometry::align(left, right); + + // look for optional centerline + Lanelet lanelet(id, left, right, attributes); + if (findRole(llElem.members, RoleNameString::Centerline) != llElem.members.end()) { + auto center = getLaneletBorder(llElem, RoleNameString::Centerline); + lanelet.setCenterline(center); + } + + lanelets_.emplace(id, lanelet); + + // check for regulatory elements + if (findRole(llElem.members, RoleNameString::RegulatoryElement) != llElem.members.end()) { + llWithRegulatoryElement.push_back(std::make_pair(lanelet, &llElem)); + } + } + return llWithRegulatoryElement; +} + +AreasWithRegulatoryElements MultiFileLoader::loadAreas(const lanelet::osm::Relations & relations) +{ + // regElems are not parsed yet. We store areas with one for later. + AreasWithRegulatoryElements arWithRegulatoryElement; + for (const auto & relElem : relations) { + const auto & arElem = relElem.second; + if (!isType(arElem)) { + continue; + } + const auto id = arElem.id; + const auto attributes = getAttributes(arElem.attributes); + + auto outerRing = getOuterRing(arElem); + if (outerRing.empty()) { + // getOuter ring repors errors for us + continue; + } + + Area area(id, outerRing, getInnerRing(arElem), attributes); + areas_.emplace(id, area); + + // check for regulatory elements + if (findRole(arElem.members, RoleNameString::RegulatoryElement) != arElem.members.end()) { + arWithRegulatoryElement.push_back(std::make_pair(area, &arElem)); + } + } + return arWithRegulatoryElement; +} + +void MultiFileLoader::loadRegulatoryElements(const osm::Relations & relations) +{ + for (const auto & relElem : relations) { + const auto & regElem = relElem.second; + if (!isType(regElem)) { + continue; + } + const auto id = regElem.id; + const auto attributes = getAttributes(regElem.attributes); + const auto type = attributes.find(AttributeName::Subtype); + if (type == attributes.end()) { + parserError(id, "Regulatory element has no 'subtype' tag."); + continue; + } + auto rules = getRulesForRegulatoryElement(id, regElem.members); + auto regelemData = std::make_shared(id, rules, attributes); + auto regelemType = type->second.value(); + try { + auto regElement = RegulatoryElementFactory::create(regelemType, regelemData); + regulatoryElements_.emplace(id, regElement); + } catch (std::exception & e) { + parserError( + id, "Creating a regulatory element of type "s + regelemType + " failed: " + e.what()); + } + } +} + +template +void MultiFileLoader::addRegulatoryElements( + std::vector> & addTos) +{ + for (auto & addTo : addTos) { + osm::forEachMember( + addTo.second->members, RoleNameString::RegulatoryElement, [&](const osm::Role & role) { + auto regElem = getOrGetDummy(regulatoryElements_, role.second->id, addTo.first.id()); + addTo.first.addRegulatoryElement(regElem); + }); + } +} + +template +bool MultiFileLoader::isType(const lanelet::osm::Relation & relation) +{ + auto attr = relation.attributes.find(AttributeNamesString::Type); + return attr != relation.attributes.end() && attr->second == Type; +} - 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(); - } - } +lanelet::AttributeMap MultiFileLoader::getAttributes(const lanelet::osm::Attributes & osmAttributes) +{ + lanelet::AttributeMap attributes; + for (const auto & osmAttr : osmAttributes) { + attributes.insert(std::make_pair(osmAttr.first, lanelet::Attribute(osmAttr.second))); + } + return attributes; +} + +LineString3d MultiFileLoader::getLaneletBorder( + const osm::Relation & llElem, const std::string & role) +{ + size_t numMembers = 0; + osm::forEachMember(llElem.members, role, [&](auto & /*role*/) { ++numMembers; }); + if (numMembers != 1) { + parserError(llElem.id, "Lanelet has not exactly one "s + role + " border!"); + return LineString3d(llElem.id); + } + auto member = osm::findRole(llElem.members, role); + if (member->second->type() != AttributeValueString::Way) { + parserError(llElem.id, "Lanelet "s + role + " border is not of type way!"); + return LineString3d(llElem.id); + } + return getOrGetDummy(lineStrings_, member->second->id, llElem.id); +} + +LineStrings3d MultiFileLoader::getLinestrings( + const osm::Roles & roles, const std::string & roleName, lanelet::Id refId) +{ + LineStrings3d linestrings; + osm::forEachMember(roles, roleName, [&](auto & member) { + if (member.second->type() != AttributeValueString::Way) { + auto msg = roleName + " ring must consist of ways but id " + + std::to_string(member.second->id) + " is of type " + member.second->type() + "!"; + msg[0] = std::toupper(msg[0]); + this->parserError(refId, msg); + return; + } + auto elem = lineStrings_.find(member.second->id); + if (elem == lineStrings_.end()) { + this->parserError( + refId, "Failed to get id "s + std::to_string(member.second->id) + " from map"); + return; + } + linestrings.push_back(elem->second); + }); + return linestrings; +} + +LineStrings3d MultiFileLoader::getOuterRing(const osm::Relation & area) +{ + auto outerLs = getLinestrings(area.members, RoleNameString::Outer, area.id); + if (outerLs.empty()) { + parserError(area.id, "Areas must have at least one outer border!"); + return {}; + } + auto outerRings = assembleBoundary(outerLs, area.id); + if (outerRings.size() != 1) { + parserError(area.id, "Areas must have exactly one outer ring!"); + return {}; + } + return outerRings.front(); +} + +std::vector MultiFileLoader::getInnerRing(const osm::Relation & area) +{ + auto innerLs = getLinestrings(area.members, RoleNameString::Inner, area.id); + return assembleBoundary(innerLs, area.id); +} - std::unique_ptr MultiOsmParser::parse( - const std::vector & lanelet2_filenames, lanelet::ErrorMessages & errors) const - { - std::map files; - - osm::Errors osmReadErrors; - - std::for_each( - lanelet2_filenames.begin(), lanelet2_filenames.end(), - [&files, &osmReadErrors](const auto & file_name) { - pugi::xml_document doc; - auto result = doc.load_file(file_name.c_str()); - if (!result) { - throw lanelet::ParseError( - "Errors occured while parsing osm file: "s + result.description()); - } - testAndPrintLocaleWarning(osmReadErrors); - - files[file_name] = lanelet::osm::read(doc, &osmReadErrors); - }); - - auto map = fromOsmFile(files, errors); - - // make sure ids in the file are known to Lanelet2 id management. - for (const auto & file : files) { - registerIds(file.second.nodes); - registerIds(file.second.ways); - registerIds(file.second.relations); - } - - for (lanelet::Lanelet lanelet : map->laneletLayer) { - auto left = lanelet.leftBound(); - auto right = lanelet.rightBound(); - std::tie(left, right) = lanelet::geometry::align(left, right); - lanelet.setLeftBound(left); - lanelet.setRightBound(right); - } - - errors = buildErrorMessage( - "Errors ocurred while parsing Lanelet Map:", utils::concatenate({osmReadErrors, errors})); - return map; +RuleParameterMap MultiFileLoader::getRulesForRegulatoryElement( + lanelet::Id currElemId, const osm::Roles & roles) +{ + RuleParameterMap rules; + for (const auto & memberPair : roles) { + const auto & member = memberPair.second; + if (member->type() == AttributeValueString::Node) { + auto newMember = getOrGetDummy(points_, member->id, currElemId); + rules[memberPair.first].emplace_back(newMember); + } else if (member->type() == AttributeValueString::Way) { + // can either be linestring or polygon + if (polygons_.find(member->id) != polygons_.end()) { + auto newMember = getOrGetDummy(polygons_, member->id, currElemId); + rules[memberPair.first].emplace_back(newMember); + } else { + auto newMember = getOrGetDummy(lineStrings_, member->id, currElemId); + rules[memberPair.first].emplace_back(newMember); + } + } else if (member->type() == AttributeValueString::Relation) { + // could be lanelet or area. regulatory element is not allowed. + auto type = member->attributes.find(AttributeNamesString::Type); + if (type == member->attributes.end()) { + parserError( + currElemId, "Relation refers to another relation "s + std::to_string(member->id) + + " without a type tag!"); + } else if (type->second == AttributeValueString::Lanelet) { + auto newMember = getOrGetDummy(lanelets_, member->id, currElemId); + rules[memberPair.first].emplace_back(newMember); + } else if (type->second == AttributeValueString::Multipolygon) { + auto newMember = getOrGetDummy(areas_, member->id, currElemId); + rules[memberPair.first].emplace_back(newMember); + } else if (type->second == AttributeValueString::RegulatoryElement) { + parserError( + currElemId, + "Regulatory element refers to another " + "regulatory element. This is not " + "supported."); + } else { + parserError( + currElemId, "Member of regulatory_element has unsupported type "s + type->second); + } + } + } + return rules; +} + +std::vector MultiFileLoader::assembleBoundary( + lanelet::LineStrings3d lineStrings, lanelet::Id id) +{ + std::reverse(lineStrings.begin(), lineStrings.end()); // its easier to pop from a vector... + std::vector rings; + rings.emplace_back(LineStrings3d()); + while (!lineStrings.empty()) { + auto & currRing = rings.back(); + if (currRing.empty()) { + currRing.push_back(lineStrings.back()); + lineStrings.pop_back(); + } else { + const auto lastId = currRing.back().back().id(); + auto elem = + std::find_if(lineStrings.rbegin(), lineStrings.rend(), [lastId](const auto & elem) { + return elem.back().id() == lastId || elem.front().id() == lastId; + }); + // we are unable to close the current ring + if (elem == lineStrings.rend()) { + parserError( + id, + "Could not complete boundary around linestring " + std::to_string(currRing.back().id())); + rings.back() = LineStrings3d(); + continue; + } + // we found the matching next linestring. add it in the correct order + auto newLineString = *elem; + lineStrings.erase(std::next(elem).base()); + if (newLineString.back().id() == lastId) { + newLineString = newLineString.invert(); + } + currRing.push_back(newLineString); + } + + // check if we closed the ring + if (currRing.back().back().id() == currRing.front().front().id()) { + // wohoo. Check the clockwise requirement. + if (!isValid(currRing)) { + reverse(currRing); + if (!isValid(currRing)) { + // most probably self-intersecting... + parserError(id, "Failed to generate boundary (self-intersecting?)"); + rings.pop_back(); } + } + rings.emplace_back(LineStrings3d()); + } + } + rings.pop_back(); // last ring will be empty or invalid + return rings; +} + +template +PrimT MultiFileLoader::getOrGetDummy( + const typename std::unordered_map & map, lanelet::Id id, + lanelet::Id currentPrimitiveId) +{ + try { + return map.at(id); + } catch (std::out_of_range &) { + parserError(currentPrimitiveId, "Failed to get id "s + std::to_string(id) + " from map"); + return getDummy(id); + } +} + +void MultiFileLoader::parserError(lanelet::Id id, const std::string & what) +{ + auto errstr = "Error parsing primitive "s + std::to_string(id) + ": " + what; + errors_.push_back(errstr); +} - std::unique_ptr MultiOsmParser::parse( - const std::string & filename, ErrorMessages & errors) const - { - pugi::xml_document doc; - auto result = doc.load_file(filename.c_str()); - if (!result) { - throw lanelet::ParseError("Errors occured while parsing osm file: "s + result.description()); - } - osm::Errors osmReadErrors; - testAndPrintLocaleWarning(osmReadErrors); - auto file = lanelet::osm::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 = buildErrorMessage( - "Errors ocurred while parsing Lanelet Map:", utils::concatenate({osmReadErrors, errors})); - return map; - } +template +void registerIds(const MapT & map) +{ + if (!map.empty()) { + utils::registerId(map.rbegin()->first); + } +} - std::unique_ptr MultiOsmParser::fromOsmFile( - const std::map & files_map, ErrorMessages & errors) const - { - return MultiFileLoader::loadMap(files_map, projector(), errors); - } +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(); + } +} + +std::unique_ptr MultiOsmParser::parse( + const std::vector & lanelet2_filenames, lanelet::ErrorMessages & errors) const +{ + std::map files; + + osm::Errors osmReadErrors; + + std::for_each( + lanelet2_filenames.begin(), lanelet2_filenames.end(), + [&files, &osmReadErrors](const auto & file_name) { + pugi::xml_document doc; + auto result = doc.load_file(file_name.c_str()); + if (!result) { + throw lanelet::ParseError( + "Errors occured while parsing osm file: "s + result.description()); + } + testAndPrintLocaleWarning(osmReadErrors); + + files[file_name] = lanelet::osm::read(doc, &osmReadErrors); + }); + + auto map = fromOsmFile(files, errors); + + // make sure ids in the file are known to Lanelet2 id management. + for (const auto & file : files) { + registerIds(file.second.nodes); + registerIds(file.second.ways); + registerIds(file.second.relations); + } + + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + + errors = buildErrorMessage( + "Errors ocurred while parsing Lanelet Map:", utils::concatenate({osmReadErrors, errors})); + return map; +} + +std::unique_ptr MultiOsmParser::parse( + const std::string & filename, ErrorMessages & errors) const +{ + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + throw lanelet::ParseError("Errors occured while parsing osm file: "s + result.description()); + } + osm::Errors osmReadErrors; + testAndPrintLocaleWarning(osmReadErrors); + auto file = lanelet::osm::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 = buildErrorMessage( + "Errors ocurred while parsing Lanelet Map:", utils::concatenate({osmReadErrors, errors})); + return map; +} + +std::unique_ptr MultiOsmParser::fromOsmFile( + const std::map & files_map, ErrorMessages & errors) const +{ + return MultiFileLoader::loadMap(files_map, projector(), errors); +} - std::unique_ptr MultiOsmParser::fromOsmFile( - const osm::File & file, lanelet::ErrorMessages & errors) const - { - return MultiFileLoader::loadMap(file, projector(), errors); - } +std::unique_ptr MultiOsmParser::fromOsmFile( + const osm::File & file, lanelet::ErrorMessages & errors) const +{ + return MultiFileLoader::loadMap(file, projector(), errors); +} - void MultiOsmParser::parseVersions( - const std::string & filename, std::string * format_version, std::string * map_version) - { - if (format_version == nullptr || map_version == nullptr) { - std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!"; - return; - } - - pugi::xml_document doc; - auto result = doc.load_file(filename.c_str()); - if (!result) { - throw lanelet::ParseError( - std::string("Errors occurred while parsing osm file: ") + result.description()); - } - - auto osmNode = doc.child("osm"); - auto metainfo = osmNode.child("MetaInfo"); - if (metainfo.attribute("format_version")) { - *format_version = metainfo.attribute("format_version").value(); - } - if (metainfo.attribute("map_version")) { - *map_version = metainfo.attribute("map_version").value(); - } - } - } // namespace io_handlers +void MultiOsmParser::parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version) +{ + if (format_version == nullptr || map_version == nullptr) { + std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!"; + return; + } + + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + throw lanelet::ParseError( + std::string("Errors occurred while parsing osm file: ") + result.description()); + } + + auto osmNode = doc.child("osm"); + auto metainfo = osmNode.child("MetaInfo"); + if (metainfo.attribute("format_version")) { + *format_version = metainfo.attribute("format_version").value(); + } + if (metainfo.attribute("map_version")) { + *map_version = metainfo.attribute("map_version").value(); + } +} +} // namespace io_handlers } // namespace lanelet