diff --git a/ad_map_opendrive_reader/include/opendrive/parser/GeoReferenceParser.h b/ad_map_opendrive_reader/include/opendrive/parser/GeoReferenceParser.h index abac4f7cac..8dc4ffa285 100644 --- a/ad_map_opendrive_reader/include/opendrive/parser/GeoReferenceParser.h +++ b/ad_map_opendrive_reader/include/opendrive/parser/GeoReferenceParser.h @@ -13,6 +13,8 @@ #pragma once #include +#include + #include "opendrive/types.hpp" namespace opendrive { @@ -21,7 +23,7 @@ namespace parser { class GeoReferenceParser { public: - static ::opendrive::geom::GeoLocation Parse(const std::string &geo_reference_string); + static ::opendrive::geom::GeoLocation Parse(const std::string &geo_reference_string, const pugi::xml_node &geo_offset = pugi::xml_node()); }; } // namespace parser diff --git a/ad_map_opendrive_reader/include/opendrive/types.hpp b/ad_map_opendrive_reader/include/opendrive/types.hpp index 0370591383..a98018cb2d 100644 --- a/ad_map_opendrive_reader/include/opendrive/types.hpp +++ b/ad_map_opendrive_reader/include/opendrive/types.hpp @@ -736,6 +736,11 @@ struct GeoLocation double altitude{0.}; std::string projection{""}; std::string projectionSetAfterLoad{""}; + double offset_x{0.0}; + double offset_y{0.0}; + double offset_z{0.0}; + double offset_hdg_sin{0.0}; + double offset_hdg_cos{1.0}; }; } // namespace geom diff --git a/ad_map_opendrive_reader/src/geometry/GeometryGenerator.cpp b/ad_map_opendrive_reader/src/geometry/GeometryGenerator.cpp index ba6c595432..d9b0abccba 100644 --- a/ad_map_opendrive_reader/src/geometry/GeometryGenerator.cpp +++ b/ad_map_opendrive_reader/src/geometry/GeometryGenerator.cpp @@ -965,15 +965,22 @@ bool convertToGeoPoints(opendrive::OpenDriveData &mapData) } } - auto convertENUToGeo = [&projPtr](Point &point) { + auto convertENUToGeo = [&projPtr, &mapData](Point &point) { if (!point.isValid()) { spdlog::error("ConvertENUToGeo: Input point invalid {}, {}", point.x, point.y); } point.ensureValid(); + + double tx = point.x + mapData.geoReference.offset_x; + double ty = point.y + mapData.geoReference.offset_y; + + double x_rot = (tx * mapData.geoReference.offset_hdg_cos) - (ty * mapData.geoReference.offset_hdg_sin); + double y_rot = (tx * mapData.geoReference.offset_hdg_sin) + (ty * mapData.geoReference.offset_hdg_cos); + projXY enuPoint; - enuPoint.u = point.x; - enuPoint.v = point.y; + enuPoint.u = x_rot; + enuPoint.v = y_rot; auto geoPoint = pj_inv(enuPoint, projPtr); point.x = geoPoint.u * RAD_TO_DEG; diff --git a/ad_map_opendrive_reader/src/parser/GeoReferenceParser.cpp b/ad_map_opendrive_reader/src/parser/GeoReferenceParser.cpp index 904f9e0d06..4aab110b34 100644 --- a/ad_map_opendrive_reader/src/parser/GeoReferenceParser.cpp +++ b/ad_map_opendrive_reader/src/parser/GeoReferenceParser.cpp @@ -24,7 +24,21 @@ namespace opendrive { namespace parser { -::opendrive::geom::GeoLocation GeoReferenceParser::Parse(const std::string &geo_reference_string) +double GetAttributeOrDefault(const pugi::xml_node &node, const std::string &attribute, double const defaultValue) +{ + auto attr = node.attribute(attribute.c_str()); + if (attr.empty()) + { + return defaultValue; + } + else + { + return attr.as_double(); + } +} + +::opendrive::geom::GeoLocation GeoReferenceParser::Parse(const std::string &geo_reference_string, + const pugi::xml_node &geo_offset) { ::opendrive::geom::GeoLocation result{ std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.0, ""}; @@ -65,6 +79,14 @@ ::opendrive::geom::GeoLocation GeoReferenceParser::Parse(const std::string &geo_ } } + // parse optional offset + result.offset_x = GetAttributeOrDefault(geo_offset, "x", 0.0); + result.offset_y = GetAttributeOrDefault(geo_offset, "y", 0.0); + result.offset_z = GetAttributeOrDefault(geo_offset, "z", 0.0); + auto offset_hdg = GetAttributeOrDefault(geo_offset, "hdg", 0.0); + result.offset_hdg_sin = std::sin(offset_hdg); + result.offset_hdg_cos = std::cos(offset_hdg); + return result; } diff --git a/ad_map_opendrive_reader/src/parser/OpenDriveParser.cpp b/ad_map_opendrive_reader/src/parser/OpenDriveParser.cpp index 68d04d41fe..d559fd5d99 100644 --- a/ad_map_opendrive_reader/src/parser/OpenDriveParser.cpp +++ b/ad_map_opendrive_reader/src/parser/OpenDriveParser.cpp @@ -135,7 +135,8 @@ bool OpenDriveParser::Parse(const char *xml, } out_open_drive_data.geoReference - = odp::GeoReferenceParser::Parse(xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference")); + = odp::GeoReferenceParser::Parse(xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference"), + xmlDoc.child("OpenDRIVE").child("header").child("offset")); auto userData = xmlDoc.child("OpenDRIVE").child("header").child("userData"); if (!userData.empty()) diff --git a/doc/CHANGELOG.md b/doc/CHANGELOG.md index 4543d301c8..670fec0db4 100644 --- a/doc/CHANGELOG.md +++ b/doc/CHANGELOG.md @@ -1,4 +1,7 @@ ## Latest changes +#### :rocket: New Features +* Add support for OpenDRIVE offset-tag in header + ## Release 3.0.0 #### :rocket: New Features * Renamed branch: master -> main