From 381f03a9a0ebc4b303d0b7ad37ae84d3a33e0cef Mon Sep 17 00:00:00 2001
From: "Pasch, Frederik" <frederik.pasch@intel.com>
Date: Tue, 26 Feb 2019 11:10:18 +0100
Subject: [PATCH] fix parsing of OpenDrive geoReference exported by RoadRunner

---
 CHANGELOG.md                                |  1 +
 LibCarla/source/carla/client/GnssSensor.cpp | 22 +++++++++++++--------
 LibCarla/source/carla/client/GnssSensor.h   |  3 +++
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6def321e5..824c9ab81 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,4 +1,5 @@
 ## Latest Changes
+  * Fix parsing of OpenDrive geoReference exported by RoadRunner
   * Added manual_control_steeringwheel.py to control agents using Logitech G29 steering wheels (and maybe others).
   * Fixed `manual_control.py` and `no_rendering_mode.py` to prevent crashes when used in "no rendering mode"
   * Added movable props present in the map (e.g. chairs and tables) as actors so they can be controlled from Python
diff --git a/LibCarla/source/carla/client/GnssSensor.cpp b/LibCarla/source/carla/client/GnssSensor.cpp
index 062bb7ab6..2e98b695e 100644
--- a/LibCarla/source/carla/client/GnssSensor.cpp
+++ b/LibCarla/source/carla/client/GnssSensor.cpp
@@ -93,16 +93,11 @@ namespace client {
       if (geo_ref_key_value.size() != 2u) {
         continue;
       }
-      std::istringstream istr(geo_ref_key_value[1]);
-      istr.imbue(std::locale("C"));
+
       if (geo_ref_key_value[0] == "+lat_0") {
-        istr >> _map_latitude;
+        _map_latitude = ParseDouble(geo_ref_key_value[1]);
       } else if (geo_ref_key_value[0] == "+lon_0") {
-        istr >> _map_longitude;
-      }
-      if (istr.fail() || !istr.eof()) {
-        _map_latitude = std::numeric_limits<double>::quiet_NaN();
-        _map_longitude = std::numeric_limits<double>::quiet_NaN();
+        _map_longitude = ParseDouble(geo_ref_key_value[1]);
       }
     }
 
@@ -130,6 +125,17 @@ namespace client {
     _is_listening = true;
   }
 
+  double GnssSensor::ParseDouble(std::string const &stringValue) const {
+    double value;
+    std::istringstream istr(stringValue);
+    istr.imbue(std::locale("C"));
+    istr >> value;
+    if (istr.fail() || !istr.eof()) {
+      value = std::numeric_limits<double>::quiet_NaN();
+    }
+    return value;
+  }
+
   SharedPtr<sensor::SensorData> GnssSensor::TickGnssSensor(
       const Timestamp &timestamp) {
     try {
diff --git a/LibCarla/source/carla/client/GnssSensor.h b/LibCarla/source/carla/client/GnssSensor.h
index 33147d723..de37c52a5 100644
--- a/LibCarla/source/carla/client/GnssSensor.h
+++ b/LibCarla/source/carla/client/GnssSensor.h
@@ -6,6 +6,7 @@
 #pragma once
 
 #include "carla/client/Sensor.h"
+#include <string>
 
 namespace carla {
 namespace client {
@@ -42,6 +43,8 @@ namespace client {
 
     SharedPtr<sensor::SensorData> TickGnssSensor(const Timestamp &timestamp);
 
+    double ParseDouble(std::string const &stringValue) const;
+
     double _map_latitude;
 
     double _map_longitude;