diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index d3a7b2f2..909b967f 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -64,7 +64,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o CONSOLE_BRIDGE_logError("Material must contain a name attribute"); return false; } - + material.name = config->Attribute("name"); // texture @@ -88,7 +88,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o material.color.init(c->Attribute("rgba")); has_rgb = true; } - catch (ParseError &e) { + catch (ParseError &e) { material.color.clear(); CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); } @@ -133,7 +133,7 @@ bool parseSphere(Sphere &s, TiXmlElement *c) bool parseBox(Box &b, TiXmlElement *c) { b.clear(); - + b.type = Geometry::BOX; if (!c->Attribute("size")) { @@ -255,14 +255,14 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g) Mesh *m = new Mesh(); geom.reset(m); if (parseMesh(*m, shape)) - return geom; + return geom; } else { CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); return geom; } - + return GeometrySharedPtr(); } @@ -376,7 +376,7 @@ bool parseVisual(Visual &vis, TiXmlElement *config) return false; } vis.material_name = mat->Attribute("name"); - + // try to parse material element in place vis.material.reset(new Material()); if (!parseMaterial(*vis.material, mat, true)) @@ -384,12 +384,12 @@ bool parseVisual(Visual &vis, TiXmlElement *config) vis.material.reset(); } } - + return true; } bool parseCollision(Collision &col, TiXmlElement* config) -{ +{ col.clear(); // Origin @@ -398,7 +398,7 @@ bool parseCollision(Collision &col, TiXmlElement* config) if (!parsePose(col.origin, o)) return false; } - + // Geometry TiXmlElement *geom = config->FirstChildElement("geometry"); col.geometry = parseGeometry(geom); @@ -414,7 +414,7 @@ bool parseCollision(Collision &col, TiXmlElement* config) bool parseLink(Link &link, TiXmlElement* config) { - + link.clear(); const char *name_char = config->Attribute("name"); @@ -443,41 +443,36 @@ 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) // Assign the first visual to the .visual ptr, if it exists if (!link.visual_array.empty()) link.visual = link.visual_array[0]; - + // Multiple Collisions (optional) for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { 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) + + // Collision (optional) // Assign the first collision to the .collision ptr, if it exists if (!link.collision_array.empty()) link.collision = link.collision_array[0]; @@ -601,7 +596,7 @@ bool exportInertial(Inertial &i, TiXmlElement *xml) inertial_xml->LinkEndChild(inertia_xml); xml->LinkEndChild(inertial_xml); - + return true; } @@ -629,7 +624,7 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) } bool exportCollision(Collision &col, TiXmlElement* xml) -{ +{ // // // diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 7e9bc309..3c03d3b9 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -125,27 +125,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 @@ -154,15 +157,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()); @@ -174,17 +193,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(); @@ -197,26 +212,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()); } diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index c46dd326..f03cfd65 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -24,19 +24,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; @@ -45,12 +32,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)); } @@ -63,12 +44,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); } @@ -258,7 +233,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 = @@ -332,6 +306,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) {