Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[model_io] Fix implicit dependency of URDF parsers on system locale #289

Merged
merged 1 commit into from
May 8, 2017
Merged

[model_io] Fix implicit dependency of URDF parsers on system locale #289

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 15 additions & 10 deletions ...l_io/urdf-kdl/external/urdfdom_headers/urdf_model/include/urdf_model/boost_replacements.h
View file
Open in desktop
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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just a curiosity, why not using a templated function now that inside there are no differences between the toDouble and toInt and toUnsigned functions?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point, I think we can do it once we fixed the bug.

{
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there any reason for the code duplication?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, for historical reason the urdf_model/boost_replacements.h headers is only included in Windows, and these are private functions of the library. We can think of moving this functions to a public header, but I would prefer to to that in a separate pull request, as any additional public function then needs to be maintained, documented and modified with care to avoid breaking downstream code.

{
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
Open in desktop
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