From 26f6618f00b640c676b8bbe842c5b44f964ad910 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwincoumans@google.com>
Date: Fri, 17 Jun 2016 12:07:38 -0700
Subject: [PATCH] colors for sdf

---
 data/kuka_iiwa/model.sdf                      |  8 ++-
 .../Importers/ImportURDFDemo/UrdfParser.cpp   | 60 ++++++++++++-------
 2 files changed, 45 insertions(+), 23 deletions(-)

diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf
index 1b6f8320d..0df20cff7 100644
--- a/data/kuka_iiwa/model.sdf
+++ b/data/kuka_iiwa/model.sdf
@@ -32,6 +32,12 @@
             <uri>meshes/link_0.stl</uri>
           </mesh>
         </geometry>
+        <material>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0 0 1 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+          <emissive>0 0 0 0</emissive>
+        </material>
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
@@ -407,4 +413,4 @@
     </joint>
   </model>
  </world>
-</sdf>
\ No newline at end of file
+</sdf>
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index 430593b5e..17f6ef93f 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -490,29 +490,45 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
   // Material
   TiXmlElement *mat = config->FirstChildElement("material");
 //todo(erwincoumans) skip materials in SDF for now (due to complexity)
-  if (mat && !m_parseSDF)
+  if (mat)
   {
-	  // get material name
-	  if (!mat->Attribute("name")) 
-	  {
-		  logger->reportError("Visual material must contain a name attribute");
-		  return false;
-	  }
-	  visual.m_materialName = mat->Attribute("name");
-	  
-	  // try to parse material element in place
-	  
-	  TiXmlElement *t = mat->FirstChildElement("texture");
-	  TiXmlElement *c = mat->FirstChildElement("color");
-	  if (t||c)
-	  {
-		  if (parseMaterial(visual.m_localMaterial, mat,logger))
-		  {
-			  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
-			  model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
-			  visual.m_hasLocalMaterial = true;
-		  }
-	  }
+    if (m_parseSDF)
+    {
+        UrdfMaterial* matPtr = new UrdfMaterial;
+        matPtr->m_name = "mat";
+        std::string diffuseText = mat->FirstChildElement("diffuse")->GetText();
+        btVector4 rgba(1,0,0,1);
+        parseVector4(rgba,diffuseText);
+        matPtr->m_rgbaColor = rgba;
+        matPtr->m_textureFilename = "textureTest.png";
+        model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+        visual.m_materialName = "mat";
+        visual.m_hasLocalMaterial = true;
+    } 
+    else
+      {
+          // get material name
+          if (!mat->Attribute("name")) 
+          {
+              logger->reportError("Visual material must contain a name attribute");
+              return false;
+          }
+          visual.m_materialName = mat->Attribute("name");
+          
+          // try to parse material element in place
+          
+          TiXmlElement *t = mat->FirstChildElement("texture");
+          TiXmlElement *c = mat->FirstChildElement("color");
+          if (t||c)
+          {
+              if (parseMaterial(visual.m_localMaterial, mat,logger))
+              {
+                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
+                  model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+                  visual.m_hasLocalMaterial = true;
+              }
+          }
+      }
   }
   
   return true;