From 0bcfe32f8f0ea95f4f455c6fc4a8e181959aa345 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Nov 2017 15:36:03 -0500 Subject: [PATCH 1/5] Cleanup error checking in model.cpp. There are 2 main things done here: 1. Check the return value from parseMaterial, parseLink, and parseJoint. While these can throw a ParseError exception, most often they return false when they fail to parse. 2. Revamp the error checking around those calls so that the code is indented a lot less. It is much easier to read this way. Signed-off-by: Chris Lalancette --- urdf_parser/src/model.cpp | 107 ++++++++++++++++++++++---------------- 1 file changed, 62 insertions(+), 45 deletions(-) diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index fc79f759..206ebe60 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -142,27 +142,30 @@ 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)) - { - 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 = parseMaterial(*material, material_xml, false); // material needs to be fully defined here + } catch(ParseError &) { + success = false; } - catch (ParseError &/*e*/) { + + if (!success) { CONSOLE_BRIDGE_logError("material xml is not initialized correctly"); material.reset(); model.reset(); return model; } + + if (model->getMaterial(material->name)) + { + CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); + 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 @@ -171,15 +174,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()); @@ -191,17 +210,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(); @@ -214,26 +229,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()); } From 595a9481997495501635fede5579fe3b87d34cb8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Nov 2017 15:42:02 -0500 Subject: [PATCH 2/5] Slightly cleanup link.cpp. Just make the code flow easier to understand. Signed-off-by: Chris Lalancette --- urdf_parser/src/link.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index d3a7b2f2..8b5cf6d7 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -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) @@ -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) From 42535754937f47e88aa55376cdacda9b969bcc20 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Apr 2018 14:22:34 +0000 Subject: [PATCH 3/5] Add in tests for new, more restrictive parsing. Signed-off-by: Chris Lalancette --- urdf_parser/test/urdf_unit_test.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 8349b1de..3e4e6a78 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -272,7 +272,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 = @@ -346,6 +345,26 @@ 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 = + "" + " " + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + ASSERT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, link_no_name) +{ + std::string joint_str = + "" + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + ASSERT_EQ(nullptr, urdf); +} int main(int argc, char **argv) { From a063726ae16aa85a43a0fb2ca353b86aa8cade49 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Feb 2019 15:27:50 +0000 Subject: [PATCH 4/5] Quiet down the tests. Signed-off-by: Chris Lalancette --- urdf_parser/test/urdf_unit_test.cpp | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 3e4e6a78..cbbf1a92 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -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; @@ -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)); } @@ -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); } From c5eaa16327238ace45cbeb3e662f74e3c51daed6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 8 Feb 2019 11:57:43 +0100 Subject: [PATCH 5/5] fix error handling in material parsing We should consider the return value of parseMaterial(). Redefining the same material again, should be accepted. This often occurs when composing URDFs via xacro. --- urdf_parser/src/model.cpp | 30 +++++++++++++++------ urdf_parser/test/urdf_unit_test.cpp | 41 +++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+), 8 deletions(-) diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 206ebe60..89a58d21 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -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); @@ -146,19 +159,20 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) try { 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 (!success) { - CONSOLE_BRIDGE_logError("material xml is not initialized correctly"); - material.reset(); - model.reset(); - return model; + if (const MaterialSharedPtr& other = model->getMaterial(material->name)) + { + if (*material != *other) + { + CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); + success = false; + } } - if (model->getMaterial(material->name)) - { - CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); + if (!success) { material.reset(); model.reset(); return model; diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index cbbf1a92..2e6d23fb 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -331,6 +331,47 @@ TEST(URDF_UNIT_TEST, material_no_name) ASSERT_EQ(nullptr, urdf); } +TEST(URDF_UNIT_TEST, materials_no_rgb) +{ + std::string urdf_str = + "" + " " + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + EXPECT_FALSE(static_cast(urdf)); // different materials cause failure +} + +TEST(URDF_UNIT_TEST, duplicate_materials) +{ + std::string urdf_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + EXPECT_TRUE(static_cast(urdf)); // identical materials are fine + + urdf_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + urdf = urdf::parseURDF(urdf_str); + EXPECT_FALSE(static_cast(urdf)); // different materials cause failure +} + TEST(URDF_UNIT_TEST, link_no_name) { std::string joint_str =