Parse and consider OpenDRIVE geo reference offset

Author: berndgassmann

ad_map_opendrive_reader/include/opendrive/parser/GeoReferenceParser.h

@@ -13,6 +13,8 @@
 #pragma once
 
 #include <string>
+#include <pugixml.hpp>
+
 #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

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
 

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;

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<double>::quiet_NaN(), std::numeric_limits<double>::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;
 }
 

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())

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