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

Fix assumption on global locale in URDF parser #15

Merged
merged 2 commits into from
Aug 30, 2019
Merged

Fix assumption on global locale in URDF parser #15

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c8b5cbb
Fix assumption on global locale in URDF parser
traversaro Aug 24, 2019
7d81da3
Don't rely on tinyxml2 for double parsing
gergondet Aug 27, 2019
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
50 changes: 38 additions & 12 deletions src/urdf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,37 @@
#include <ciso646>
#include <cmath>
#include <iostream>
#include <locale>
#include <tinyxml2.h>

namespace mc_rbdyn_urdf
{

double attrToDouble(const tinyxml2::XMLElement & dom,
const std::string & attr,
double def = 0.0)
{
const char * attrTxt = dom.Attribute(attr.c_str());
if(attrTxt)
{
std::stringstream ss;
// Every real number in a URDF file needs to be parsed assuming that the
// decimal point separator is the period, as specified in XML Schema
// definition of xs:double. The call to imbue ensures that a suitable
// locale is used, instead of using the current C++ global locale.
// Related PR: https://github.com/ros/urdfdom_headers/pull/42 .
ss.imbue(std::locale::classic());
ss << attrTxt;
double res;
ss >> res;
if(!ss.fail())
{
return res;
}
}
return def;
}

std::vector<double> attrToList(const tinyxml2::XMLElement & dom,
const std::string & attr,
const std::vector<double> & def)
Expand All @@ -25,6 +51,12 @@ std::vector<double> attrToList(const tinyxml2::XMLElement & dom,
if(attrTxt)
{
std::stringstream ss;
// Every real number in a URDF file needs to be parsed assuming that the
// decimal point separator is the period, as specified in XML Schema
// definition of xs:double. The call to imbue ensures that a suitable
// locale is used, instead of using the current C++ global locale.
// Related PR: https://github.com/ros/urdfdom_headers/pull/42 .
ss.imbue(std::locale::classic());
ss << attrTxt;
double tmp;
while(ss.good())
Expand Down Expand Up @@ -83,9 +115,9 @@ Eigen::Matrix3d RPY(const std::vector<double> rpy)
inline Eigen::Matrix3d readInertia(const tinyxml2::XMLElement & dom)
{
Eigen::Matrix3d m;
m << dom.DoubleAttribute("ixx"), dom.DoubleAttribute("ixy"), dom.DoubleAttribute("ixz"), dom.DoubleAttribute("ixy"),
dom.DoubleAttribute("iyy"), dom.DoubleAttribute("iyz"), dom.DoubleAttribute("ixz"), dom.DoubleAttribute("iyz"),
dom.DoubleAttribute("izz");
m << attrToDouble(dom, "ixx"), attrToDouble(dom, "ixy"), attrToDouble(dom, "ixz"), attrToDouble(dom, "ixy"),
attrToDouble(dom, "iyy"), attrToDouble(dom, "iyz"), attrToDouble(dom, "ixz"), attrToDouble(dom, "iyz"),
attrToDouble(dom, "izz");
return m;
}

Expand Down Expand Up @@ -212,7 +244,7 @@ std::string parseMultiBodyGraphFromURDF(URDFParserResult & res,
comRPY = attrToList(*originDom, "rpy", {0.0, 0.0, 0.0});
}
Eigen::Matrix3d comFrame = RPY(comRPY);
mass = massDom->DoubleAttribute("value");
mass = attrToDouble(*massDom, "value");
Eigen::Matrix3d inertia = readInertia(*inertiaDom);
if(transformInertia)
{
Expand Down Expand Up @@ -240,9 +272,7 @@ std::string parseMultiBodyGraphFromURDF(URDFParserResult & res,
auto & mesh = boost::get<Geometry::Mesh>(v.geometry.data);
mesh.filename = meshDom->Attribute("filename");
// Optional scale
double scale = 1.;
meshDom->QueryDoubleAttribute("scale", &scale);
mesh.scale = scale;
mesh.scale = attrToDouble(*meshDom, "scale", 1.0);
}
else
{
Expand Down Expand Up @@ -318,11 +348,7 @@ std::string parseMultiBodyGraphFromURDF(URDFParserResult & res,
if(mimicDom)
{
std::string mimicJoint = mimicDom->Attribute("joint");
double multiplier = 1.0;
double offset = 0.0;
mimicDom->QueryDoubleAttribute("multiplier", &multiplier);
mimicDom->QueryDoubleAttribute("offset", &offset);
j.makeMimic(mimicJoint, multiplier, offset);
j.makeMimic(mimicJoint, attrToDouble(*mimicDom, "multiplier", 1.0), attrToDouble(*mimicDom, "offset"));
}

res.mbg.addJoint(j);
Expand Down