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 +#include +#include #include #include @@ -62,6 +65,28 @@ void split_string(std::vector &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