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

@@ -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;

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 =
+    "<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 =