diff --git a/src/model_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h b/src/model_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h index dd4bb243aaf..d65d0679254 100644 --- a/src/model_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h +++ b/src/model_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h @@ -48,24 +48,29 @@ namespace urdf { * Convert a string to a double * */ -bool inline stringToDouble(const std::string & inStr, double & outDouble) +bool inline stringToDoubleWithClassicLocale(const std::string & inStr, double & outDouble) { - outDouble = std::atof(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outDouble; + return !(ss.fail()); } -bool inline stringToInt(const std::string & inStr, int & outInt) +bool inline stringToIntWithClassicLocale(const std::string & inStr, int & outInt) { - outInt = std::atoi(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outInt; + return !(ss.fail()); } -bool inline stringToUnsignedInt(const std::string & inStr, unsigned int & outInt) +bool inline stringToUnsignedIntWithClassicLocale(const std::string & inStr, unsigned int & outInt) { - outInt = (unsigned int)std::atoi(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outInt; + return !(ss.fail()); } - std::string inline intToString(const int inInt) { std::stringstream ss; diff --git a/src/model_io/urdf-kdl/src/urdf_sensor_import.cpp b/src/model_io/urdf-kdl/src/urdf_sensor_import.cpp index db2c62fb7d3..d06592c2ec8 100644 --- a/src/model_io/urdf-kdl/src/urdf_sensor_import.cpp +++ b/src/model_io/urdf-kdl/src/urdf_sensor_import.cpp @@ -74,6 +74,13 @@ std::vector &split(const std::string &s, std::vector & return elems; } +bool inline localStringToDoubleWithClassicLocale(const std::string & inStr, double & outDouble) +{ + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outDouble; + return !(ss.fail()); +} bool ftSensorsFromUrdfString(const std::string& urdf_xml, std::vector & ft_sensors) { @@ -160,13 +167,16 @@ bool ftSensorsFromUrdfString(const std::string& urdf_xml, std::vector #include +#include "URDFParsingUtils.h" + + namespace iDynTree { -std::vector &splitString(const std::string &s, std::vector &elems) -{ - std::stringstream ss(s); - std::string item; - while (ss >> item) - { - elems.push_back(item); - } - return elems; -} bool findJointParentsAndChild(TiXmlElement* robotXml, const std::string queriedJointName, @@ -197,36 +190,15 @@ bool sensorsFromURDFString(const std::string& urdfXml, // Parse origin element Transform link_H_pose; TiXmlElement* originTag = sensorXml->FirstChildElement("origin"); - if( originTag ) - { - std::string rpyText(originTag->Attribute("rpy")); - std::string xyzText(originTag->Attribute("xyz")); - std::vector rpyElems; - std::vector xyzElems; - splitString(rpyText,rpyElems); - splitString(xyzText,xyzElems); + bool ok = transformFromURDFXML(originTag, link_H_pose); - if( rpyElems.size() != 3 && xyzElems.size() !=3 ) - { - std::cerr<<"[ERROR] Pose attribute for sensor " << sensorName << " specified incorrectly, parsing failed."; - parsingSuccessful = false; - return false; - } - double roll = atof(rpyElems[0].c_str()); - double pitch = atof(rpyElems[1].c_str()); - double yaw = atof(rpyElems[2].c_str()); - iDynTree::Rotation rot = iDynTree::Rotation::RPY(roll,pitch,yaw); - double x = atof(xyzElems[0].c_str()); - double y = atof(xyzElems[1].c_str()); - double z = atof(xyzElems[2].c_str()); - iDynTree::Position pos(x,y,z); - link_H_pose = iDynTree::Transform(rot,pos); - } - else + if (!ok) { - // default value - link_H_pose = iDynTree::Transform::Identity(); + std::cerr<< "[ERROR] impossible to parse origin element of " << sensorName << + ", parsing failed." << std::endl; + parsingSuccessful = false; + return false; } // Parse parent element diff --git a/src/model_io/urdf/src/URDFModelImport.cpp b/src/model_io/urdf/src/URDFModelImport.cpp index 38d385a3fe2..0cf4da358ea 100644 --- a/src/model_io/urdf/src/URDFModelImport.cpp +++ b/src/model_io/urdf/src/URDFModelImport.cpp @@ -61,7 +61,7 @@ bool inertiaFromURDFXML(TiXmlElement * inertiaXml, } double mass; - if( !stringToDouble(mass_xml->Attribute("value"),mass) ) + if( !stringToDoubleWithClassicLocale(mass_xml->Attribute("value"),mass) ) { std::stringstream stm; stm << "Inertial: mass [" << mass_xml->Attribute("value") @@ -85,12 +85,12 @@ bool inertiaFromURDFXML(TiXmlElement * inertiaXml, } double ixx, ixy, ixz, iyy, iyz, izz; - if( !stringToDouble(inertia_xml->Attribute("ixx"),ixx) - || !stringToDouble(inertia_xml->Attribute("ixy"),ixy) - || !stringToDouble(inertia_xml->Attribute("ixz"),ixz) - || !stringToDouble(inertia_xml->Attribute("iyy"),iyy) - || !stringToDouble(inertia_xml->Attribute("iyz"),iyz) - || !stringToDouble(inertia_xml->Attribute("izz"),izz) ) + if( !stringToDoubleWithClassicLocale(inertia_xml->Attribute("ixx"),ixx) + || !stringToDoubleWithClassicLocale(inertia_xml->Attribute("ixy"),ixy) + || !stringToDoubleWithClassicLocale(inertia_xml->Attribute("ixz"),ixz) + || !stringToDoubleWithClassicLocale(inertia_xml->Attribute("iyy"),iyy) + || !stringToDoubleWithClassicLocale(inertia_xml->Attribute("iyz"),iyz) + || !stringToDoubleWithClassicLocale(inertia_xml->Attribute("izz"),izz) ) { std::stringstream stm; @@ -179,7 +179,7 @@ bool posLimitsFromURDFXML(TiXmlElement* limXml, { if ( limXml->Attribute("lower") ) { - if( !stringToDouble(limXml->Attribute("lower"),posLimMin) ) + if( !stringToDoubleWithClassicLocale(limXml->Attribute("lower"),posLimMin) ) { std::stringstream stm; stm << " [" << limXml->Attribute("lower") @@ -196,7 +196,7 @@ bool posLimitsFromURDFXML(TiXmlElement* limXml, if ( limXml->Attribute("upper") ) { - if( !stringToDouble(limXml->Attribute("upper"),posLimMax) ) + if( !stringToDoubleWithClassicLocale(limXml->Attribute("upper"),posLimMax) ) { std::stringstream stm; stm << " [" << limXml->Attribute("upper") diff --git a/src/model_io/urdf/src/URDFParsingUtils.h b/src/model_io/urdf/src/URDFParsingUtils.h index 1cb5ed6442d..77d1f01a119 100644 --- a/src/model_io/urdf/src/URDFParsingUtils.h +++ b/src/model_io/urdf/src/URDFParsingUtils.h @@ -21,22 +21,28 @@ namespace iDynTree { -bool inline stringToDouble(const std::string & inStr, double & outDouble) +bool inline stringToDoubleWithClassicLocale(const std::string & inStr, double & outDouble) { - outDouble = std::atof(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outDouble; + return !(ss.fail()); } -bool inline stringToInt(const std::string & inStr, int & outInt) +bool inline stringToIntWithClassicLocale(const std::string & inStr, int & outInt) { - outInt = std::atoi(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outInt; + return !(ss.fail()); } -bool inline stringToUnsignedInt(const std::string & inStr, unsigned int & outInt) +bool inline stringToUnsignedIntWithClassicLocale(const std::string & inStr, unsigned int & outInt) { - outInt = (unsigned int)std::atoi(inStr.c_str()); - return true; + std::istringstream ss(inStr); + ss.imbue(std::locale::classic()); + ss >> outInt; + return !(ss.fail()); } std::string inline intToString(const int inInt) @@ -81,7 +87,7 @@ bool inline vector3FromString(const std::string & vector_str, Vector3 & out) { if (pieces[i] != ""){ double newDouble; - if( stringToDouble(pieces[i],newDouble) ) + if( stringToDoubleWithClassicLocale(pieces[i],newDouble) ) { xyz.push_back(newDouble); } @@ -157,7 +163,7 @@ bool inline vector4FromString(const std::string & vector_str, Vector4 & out) { if (pieces[i] != ""){ double newDouble; - if( stringToDouble(pieces[i],newDouble) ) + if( stringToDoubleWithClassicLocale(pieces[i],newDouble) ) { rgba.push_back(newDouble); } diff --git a/src/model_io/urdf/src/URDFSolidShapesImport.cpp b/src/model_io/urdf/src/URDFSolidShapesImport.cpp index 1433f2c2d98..396f322d62a 100644 --- a/src/model_io/urdf/src/URDFSolidShapesImport.cpp +++ b/src/model_io/urdf/src/URDFSolidShapesImport.cpp @@ -269,17 +269,30 @@ bool addURDFGeometryToModelGeometries(const iDynTree::Model& model, if( geomXml->FirstChildElement("sphere") != 0 ) { - Sphere * pSphere = new Sphere(); - pSphere->radius = std::atof(geomXml->FirstChildElement("sphere")->Attribute("radius")); - pGeom = static_cast(pSphere); + double radiusD; + parseOk = stringToDoubleWithClassicLocale(geomXml->FirstChildElement("cylinder")->Attribute("radius"),radiusD); + + if (parseOk) + { + Sphere * pSphere = new Sphere(); + pSphere->radius = radiusD; + pGeom = static_cast(pSphere); + } } if( geomXml->FirstChildElement("cylinder") != 0 ) { - Cylinder * pCylinder = new Cylinder(); - pCylinder->radius = std::atof(geomXml->FirstChildElement("cylinder")->Attribute("radius")); - pCylinder->length = std::atof(geomXml->FirstChildElement("cylinder")->Attribute("length")); - pGeom = static_cast(pCylinder); + double radiusD, lengthD; + parseOk = stringToDoubleWithClassicLocale(geomXml->FirstChildElement("cylinder")->Attribute("radius"),radiusD); + parseOk = parseOk && stringToDoubleWithClassicLocale(geomXml->FirstChildElement("cylinder")->Attribute("length"),lengthD); + + if (parseOk) + { + Cylinder * pCylinder = new Cylinder(); + pCylinder->radius = radiusD; + pCylinder->length = lengthD; + pGeom = static_cast(pCylinder); + } } if( geomXml->FirstChildElement("mesh") != 0 )