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 error handling in material parsing #122

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

fix error handling in material parsing #122

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0bcfe32
Cleanup error checking in model.cpp.
clalancette Nov 20, 2017
595a948
Slightly cleanup link.cpp.
clalancette Nov 20, 2017
4253575
Add in tests for new, more restrictive parsing.
clalancette Apr 16, 2018
a063726
Quiet down the tests.
clalancette Feb 7, 2019
c5eaa16
fix error handling in material parsing
rhaschke Feb 8, 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
15 changes: 5 additions & 10 deletions urdf_parser/src/link.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -443,16 +443,14 @@ bool parseLink(Link &link, TiXmlElement* config)

VisualSharedPtr vis;
vis.reset(new Visual());
if (parseVisual(*vis, vis_xml))
{
link.visual_array.push_back(vis);
}
else
if (!parseVisual(*vis, vis_xml))
{
vis.reset();
CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str());
return false;
}

link.visual_array.push_back(vis);
}

// Visual (optional)
Expand All @@ -465,16 +463,13 @@ bool parseLink(Link &link, TiXmlElement* config)
{
CollisionSharedPtr col;
col.reset(new Collision());
if (parseCollision(*col, col_xml))
{
link.collision_array.push_back(col);
}
else
if (!parseCollision(*col, col_xml))
{
col.reset();
CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]", link.name.c_str());
return false;
}
link.collision_array.push_back(col);
}

// Collision (optional)
Expand Down
117 changes: 74 additions & 43 deletions urdf_parser/src/model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,19 @@ bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& mode
return true;
}

bool operator==(const Material& lhs, const Material& rhs)
{
return lhs.texture_filename == rhs.texture_filename &&
lhs.color.r == rhs.color.r &&
lhs.color.g == rhs.color.g &&
lhs.color.b == rhs.color.b &&
lhs.color.a == rhs.color.a;
}
inline bool operator!=(const Material& lhs, const Material& rhs)
{
return !(lhs == rhs);
}

ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
ModelInterfaceSharedPtr model(new ModelInterface);
Expand Down Expand Up @@ -142,27 +155,31 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
MaterialSharedPtr material;
material.reset(new Material);

bool success;
try {
parseMaterial(*material, material_xml, false); // material needs to be fully defined here
if (model->getMaterial(material->name))
success = parseMaterial(*material, material_xml, false); // material needs to be fully defined here
} catch(ParseError &) {
CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
success = false;
}

if (const MaterialSharedPtr& other = model->getMaterial(material->name))
{
if (*material != *other)
{
CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
material.reset();
model.reset();
return model;
}
else
{
model->materials_.insert(make_pair(material->name,material));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
success = false;
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("material xml is not initialized correctly");

if (!success) {
material.reset();
model.reset();
return model;
}

model->materials_.insert(make_pair(material->name,material));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
}

// Get all Link elements
Expand All @@ -171,15 +188,31 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
LinkSharedPtr link;
link.reset(new Link);

bool success;
try {
parseLink(*link, link_xml);
if (model->getLink(link->name))
{
CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}
else
success = parseLink(*link, link_xml);
} catch (ParseError &) {
success = false;
}

if (!success) {
CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}

if (model->getLink(link->name))
{
CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}

// set link visual material
CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
if (link->visual)
{
if (!link->visual->material_name.empty())
{
// set link visual(s) material
CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
Expand All @@ -191,17 +224,13 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
assignMaterial(visual, model, link->name.c_str());
}

model->links_.insert(make_pair(link->name,link));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}

model->links_.insert(make_pair(link->name, link));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}

if (model->links_.empty()){
CONSOLE_BRIDGE_logError("No link elements found in urdf file");
model.reset();
Expand All @@ -214,26 +243,28 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
JointSharedPtr joint;
joint.reset(new Joint);

if (parseJoint(*joint, joint_xml))
{
if (model->getJoint(joint->name))
{
CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}
else
{
model->joints_.insert(make_pair(joint->name,joint));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}
bool success;
try {
success = parseJoint(*joint, joint_xml);
} catch(ParseError &) {
success = false;
}
else
{

if (!success) {
CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
model.reset();
return model;
}

if (model->getJoint(joint->name))
{
CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}

model->joints_.insert(make_pair(joint->name,joint));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}


Expand Down
87 changes: 61 additions & 26 deletions urdf_parser/test/urdf_unit_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,19 +27,6 @@ bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
std::abs(l[3] + r[3]) < epsilon);
}

std::ostream &operator<<(std::ostream &os, const urdf::Rotation& rot)
{
double roll, pitch, yaw;
double x, y, z, w;
rot.getRPY(roll, pitch, yaw);
rot.getQuaternion(x, y, z, w);
os << std::setprecision(9)
<< "x: " << x << " y: " << y << " z: " << z << " w: " << w
<< " roll: " << roll << " pitch: " << pitch << " yaw: "<< yaw;
return os;
}


void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
{
urdf::Rotation rot0;
Expand All @@ -48,12 +35,6 @@ void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
rot0.getRPY(roll, pitch, yaw);
urdf::Rotation rot1;
rot1.setFromRPY(roll, pitch, yaw);
if (true) {
std::cout << "\n"
<< "before " << rot0 << "\n"
<< "after " << rot1 << "\n"
<< "ok " << quat_are_near(rot0, rot1) << "\n";
}
EXPECT_TRUE(quat_are_near(rot0, rot1));
}

Expand All @@ -66,12 +47,6 @@ void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y)
urdf::Rotation rot1;
rot1.setFromRPY(roll, pitch, yaw);
bool ok = quat_are_near(rot0, rot1);
if (!ok) {
std::cout << "initial rpy: " << r << " " << p << " " << y << "\n"
<< "before " << rot0 << "\n"
<< "after " << rot1 << "\n"
<< "ok " << ok << "\n";
}
EXPECT_TRUE(ok);
}

Expand Down Expand Up @@ -272,7 +247,6 @@ TEST(URDF_UNIT_TEST, parse_link_doubles)
EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
}


TEST(URDF_UNIT_TEST, parse_color_doubles)
{
std::string joint_str =
Expand Down Expand Up @@ -346,6 +320,67 @@ TEST(URDF_UNIT_TEST, parse_color_doubles)
EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
}

TEST(URDF_UNIT_TEST, material_no_name)
{
std::string joint_str =
"<robot name=\"test\">"
" <material/>"
" <link name=\"l1\"/>"
"</robot>";
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str);
ASSERT_EQ(nullptr, urdf);
}

TEST(URDF_UNIT_TEST, materials_no_rgb)
{
std::string urdf_str =
"<robot name=\"test\">"
" <material name=\"red\"/>"
" <link name=\"dummy\"/>"
"</robot>";
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
EXPECT_FALSE(static_cast<bool>(urdf)); // different materials cause failure
}

TEST(URDF_UNIT_TEST, duplicate_materials)
{
std::string urdf_str =
"<robot name=\"test\">"
" <material name=\"red\">"
" <color rgba=\"1 0 0 1\"/>"
" </material>"
" <material name=\"red\">"
" <color rgba=\"1 0 0 1\"/>"
" </material>"
" <link name=\"dummy\"/>"
"</robot>";

urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
EXPECT_TRUE(static_cast<bool>(urdf)); // identical materials are fine

urdf_str =
"<robot name=\"test\">"
" <material name=\"red\">"
" <color rgba=\"1 0 0 1\"/>"
" </material>"
" <material name=\"red\">"
" <color rgba=\"0 1 0 1\"/>"
" </material>"
" <link name=\"dummy\"/>"
"</robot>";
urdf = urdf::parseURDF(urdf_str);
EXPECT_FALSE(static_cast<bool>(urdf)); // different materials cause failure
}

TEST(URDF_UNIT_TEST, link_no_name)
{
std::string joint_str =
"<robot name=\"test\">"
" <link/>"
"</robot>";
urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str);
ASSERT_EQ(nullptr, urdf);
}

int main(int argc, char **argv)
{
Expand Down