From e7e0972a4617b8a5df7a274ea3ba3b92e3895a35 Mon Sep 17 00:00:00 2001
From: Chris Lalancette <clalancette@gmail.com>
Date: Wed, 21 Feb 2018 15:16:52 -0500
Subject: [PATCH] Use stringstream instead of stod to work around locale
 issues. (#42)

* Use stringstream instead of stod to work around locale issues.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Use imbue to always use the "classic" locale.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Make a strToDouble facility in the utils.h header.

This will be useful elsewhere (i.e. urdfdom).

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Add some additional includes.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Add in an actual error message to the strToDouble throw.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
---
 urdf_model/include/urdf_model/pose.h  |  8 ++------
 urdf_model/include/urdf_model/utils.h | 25 +++++++++++++++++++++++++
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/urdf_model/include/urdf_model/pose.h b/urdf_model/include/urdf_model/pose.h
index 90e74a8..da65b53 100644
--- a/urdf_model/include/urdf_model/pose.h
+++ b/urdf_model/include/urdf_model/pose.h
@@ -67,14 +67,10 @@ class Vector3
     for (unsigned int i = 0; i < pieces.size(); ++i){
       if (pieces[i] != ""){
         try {
-          xyz.push_back(std::stod(pieces[i]));
-        }
-        catch (std::invalid_argument &/*e*/) {
+          xyz.push_back(strToDouble(pieces[i].c_str()));
+        } catch(std::runtime_error &) {
           throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
         }
-        catch (std::out_of_range &/*e*/) {
-          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double, out of range (while parsing a vector value)");
-        }
       }
     }
 
diff --git a/urdf_model/include/urdf_model/utils.h b/urdf_model/include/urdf_model/utils.h
index e295496..f9f59c1 100644
--- a/urdf_model/include/urdf_model/utils.h
+++ b/urdf_model/include/urdf_model/utils.h
@@ -37,6 +37,9 @@
 #ifndef URDF_INTERFACE_UTILS_H
 #define URDF_INTERFACE_UTILS_H
 
+#include <locale>
+#include <sstream>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -62,6 +65,28 @@ void split_string(std::vector<std::string> &result,
   }
 }
 
+// This is a locale-safe version of string-to-double, which is suprisingly
+// difficult to do correctly.  This function ensures that the C locale is used
+// for parsing, as that matches up with what the XSD for double specifies.
+// On success, the double is returned; on failure, a std::runtime_error is
+// thrown.
+static inline double strToDouble(const char *in)
+{
+  std::stringstream ss;
+  ss.imbue(std::locale::classic());
+
+  ss << in;
+
+  double out;
+  ss >> out;
+
+  if (ss.fail() || !ss.eof()) {
+    throw std::runtime_error("Failed converting string to double");
+  }
+
+  return out;
+}
+
 }
 
 #endif