From e787fc89b8a3d38ba8fdef03e6678d694c4184b3 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 8 Feb 2019 11:57:43 +0100
Subject: [PATCH] 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 =
+    "<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 =