Skip to content

Commit

Permalink
[model_io] Fix implicit dependency of URDF parsers on system locale
Browse files Browse the repository at this point in the history 
See #288
  • Loading branch information
@traversaro
traversaro authored and isaac-developers committed May 5, 2017
1 parent 595282a commit 611ade7
Show file tree
Hide file tree
Showing 6 changed files with 86 additions and 80 deletions.
25 changes: 15 additions & 10 deletions ...l_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
22 changes: 16 additions & 6 deletions src/model_io/urdf-kdl/src/urdf_sensor_import.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,13 @@ std::vector<std::string> &split(const std::string &s, std::vector<std::string> &
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<FTSensorData> & ft_sensors)
{
Expand Down Expand Up @@ -160,13 +167,16 @@ bool ftSensorsFromUrdfString(const std::string& urdf_xml, std::vector<FTSensorDa
{
return false;
}
double roll = atof(pose_elems[3].c_str());
double pitch = atof(pose_elems[4].c_str());
double yaw = atof(pose_elems[5].c_str());

double roll, pitch, yaw;
bool okParse = localStringToDoubleWithClassicLocale(pose_elems[3], roll);
okParse = okParse && localStringToDoubleWithClassicLocale(pose_elems[4], pitch);
okParse = okParse && localStringToDoubleWithClassicLocale(pose_elems[5], yaw);
new_ft.sensor_pose.M = KDL::Rotation::RPY(roll,pitch,yaw);
double x = atof(pose_elems[0].c_str());
double y = atof(pose_elems[1].c_str());
double z = atof(pose_elems[2].c_str());
double x, y, z;
okParse = okParse && localStringToDoubleWithClassicLocale(pose_elems[0], x);
okParse = okParse && localStringToDoubleWithClassicLocale(pose_elems[1], y);
okParse = okParse && localStringToDoubleWithClassicLocale(pose_elems[2], z);
new_ft.sensor_pose.p = KDL::Vector(x,y,z);
}

Expand Down
46 changes: 9 additions & 37 deletions src/model_io/urdf/src/URDFGenericSensorsImport.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,19 +27,12 @@
#include <iostream>
#include <fstream>

#include "URDFParsingUtils.h"


namespace iDynTree
{

std::vector<std::string> &splitString(const std::string &s, std::vector<std::string> &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,
Expand Down Expand Up @@ -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<std::string> rpyElems;
std::vector<std::string> 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
Expand Down
18 changes: 9 additions & 9 deletions src/model_io/urdf/src/URDFModelImport.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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;
Expand Down Expand Up @@ -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")
Expand All @@ -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")
Expand Down
28 changes: 17 additions & 11 deletions src/model_io/urdf/src/URDFParsingUtils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down
27 changes: 20 additions & 7 deletions src/model_io/urdf/src/URDFSolidShapesImport.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<SolidShape *>(pSphere);
double radiusD;
parseOk = stringToDoubleWithClassicLocale(geomXml->FirstChildElement("cylinder")->Attribute("radius"),radiusD);

if (parseOk)
{
Sphere * pSphere = new Sphere();
pSphere->radius = radiusD;
pGeom = static_cast<SolidShape *>(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<SolidShape *>(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<SolidShape *>(pCylinder);
}
}

if( geomXml->FirstChildElement("mesh") != 0 )
Expand Down

0 comments on commit 611ade7

Please sign in to comment.