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.

Author: rhaschke

urdf_parser/src/model.cpp

@@ -86,6 +86,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);
@@ -126,13 +139,15 @@ ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
     material.reset(new Material);
 
     try {
-      parseMaterial(*material, material_xml, false); // material needs to be fully defined here
-      if (model->getMaterial(material->name))
+      if (!parseMaterial(*material, material_xml, false))
+        throw ParseError("");
+      if (const MaterialSharedPtr& other = model->getMaterial(material->name))
       {
-        CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
-        material.reset();
-        model.reset();
-        return model;
+        if (*material != *other)  // accept redefinition of same material
+        {
+          CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
+          throw ParseError("");
+        }
       }
       else
       {

urdf_parser/test/urdf_unit_test.cpp

@@ -332,6 +332,57 @@ TEST(URDF_UNIT_TEST, parse_color_doubles)
   EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
 }
 
+TEST(URDF_UNIT_TEST, parse_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, parse_invalid_materials)
+{
+  std::string urdf_str =
+    "<robot name=\"test\">"
+    "  <material missing_name=\"red\">"
+    "    <color rgba=\"1 0 0 1\"/>"
+    "  </material>"
+    "  <link name=\"dummy\"/>"
+    "</robot>";
+
+  urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
+  EXPECT_FALSE(static_cast<bool>(urdf));
+
+  urdf_str =
+    "<robot name=\"test\">"
+    "  <material name=\"red\"/>"
+    "  <link name=\"dummy\"/>"
+    "</robot>";
+  urdf = urdf::parseURDF(urdf_str);
+  EXPECT_FALSE(static_cast<bool>(urdf));  // different materials cause failure
+}
 
 int main(int argc, char **argv)
 {