diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index e7cbdf18..e20439df 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -494,16 +494,14 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config, VisualSharedPtr vis; vis.reset(new Visual()); - if (parseVisual(*vis, vis_xml, version)) - { - link.visual_array.push_back(vis); - } - else + if (!parseVisual(*vis, vis_xml, version)) { 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) @@ -516,16 +514,13 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config, { CollisionSharedPtr col; col.reset(new Collision()); - if (parseCollision(*col, col_xml, version)) - { - link.collision_array.push_back(col); - } - else + if (!parseCollision(*col, col_xml, version)) { 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) diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 2998827f..5fba5d95 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -91,6 +91,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); @@ -152,27 +165,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)) - { - 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()); - } - } - catch (ParseError &/*e*/) { + success = parseMaterial(*material, material_xml, false); // material needs to be fully defined here + } catch(ParseError & /*e*/) { CONSOLE_BRIDGE_logError("material xml is not initialized correctly"); + success = false; + } + + if (success && material) { + if (const MaterialSharedPtr& other = model->getMaterial(material->name); + other && *material != *other) + { + CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); + success = false; + } + } + + 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 @@ -181,37 +198,41 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) LinkSharedPtr link; link.reset(new Link); + bool success; try { - parseLink(*link, link_xml, version); - if (model->getLink(link->name)) - { - CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str()); - model.reset(); - return model; - } - else - { - // set link visual(s) material - CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str()); - if (link->visual) - { - assignMaterial(link->visual, model, link->name.c_str()); - } - for (const auto& visual : link->visual_array) - { - 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()); - } + success = parseLink(*link, link_xml, version); + } catch (ParseError & /*e*/) { + success = false; } - catch (ParseError &/*e*/) { + + 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(s) material + CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str()); + if (link->visual) + { + assignMaterial(link->visual, model, link->name.c_str()); + } + for (const auto& visual : link->visual_array) + { + 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()); } + if (model->links_.empty()){ CONSOLE_BRIDGE_logError("No link elements found in urdf file"); model.reset(); @@ -224,26 +245,28 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) JointSharedPtr joint; joint.reset(new Joint); - if (parseJoint(*joint, joint_xml, version)) - { - 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, version); + } catch(ParseError & /*e*/) { + 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()); } diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 6e8d87e1..e83fe64f 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); } @@ -592,7 +567,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 = @@ -666,6 +640,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 = + "" + " " + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, materials_no_rgb) +{ + std::string urdf_str = + "" + " " + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + EXPECT_EQ(nullptr, urdf); // material missing RGB +} + +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_EQ(nullptr, urdf); // different materials cause failure +} + +TEST(URDF_UNIT_TEST, link_no_name) +{ + std::string urdf_str = + "" + " " + ""; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + EXPECT_EQ(nullptr, urdf); +} int main(int argc, char **argv) {