assign correct MaterialPtr to all visuals (#114)

* assign correct MaterialPtr to all visuals

So far, only the first visual in visual_array got the correct MaterialPtr assigned.

* fix errors

* tolerate undefined materials with a warning

* Minor style fixups.

Signed-off-by: Chris Lalancette <[email protected]>

Author: rhaschke

urdf_parser/src/link.cpp

@@ -381,7 +381,7 @@ bool parseVisual(Visual &vis, TiXmlElement *config)
     vis.material.reset(new Material());
     if (!parseMaterial(*vis.material, mat, true))
     {
-      CONSOLE_BRIDGE_logDebug("urdfdom: material has only name, actual material definition may be in the model");
+      vis.material.reset();
     }
   }
 

urdf_parser/src/model.cpp

@@ -59,6 +59,33 @@ ModelInterfaceSharedPtr  parseURDFFile(const std::string &path)
     return urdf::parseURDF( xml_str );
 }
 
+bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& model, const char* link_name)
+{
+  if (visual->material_name.empty())
+    return true;
+
+  const MaterialSharedPtr& material = model->getMaterial(visual->material_name);
+  if (material)
+  {
+    CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link_name, visual->material_name.c_str());
+    visual->material = material;
+  }
+  else
+  {
+    if (visual->material)
+    {
+      CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link_name, visual->material_name.c_str());
+      model->materials_.insert(make_pair(visual->material->name, visual->material));
+    }
+    else
+    {
+      CONSOLE_BRIDGE_logWarn("link '%s' material '%s' undefined.", link_name,visual->material_name.c_str());
+      return false;
+    }
+  }
+  return true;
+}
+
 ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
 {
   ModelInterfaceSharedPtr model(new ModelInterface);
@@ -137,32 +164,15 @@ ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
       }
       else
       {
-        // set link visual material
+        // set link visual(s) material
         CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
         if (link->visual)
         {
-          if (!link->visual->material_name.empty())
-          {
-            if (model->getMaterial(link->visual->material_name))
-            {
-              CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
-              link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
-            }
-            else
-            {
-              if (link->visual->material)
-              {
-                CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
-                model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
-              }
-              else
-              {
-                CONSOLE_BRIDGE_logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
-                model.reset();
-                return model;
-              }
-            }
-          }
+          assignMaterial(link->visual, model, link->name.c_str());
+        }
+        for (const auto& visual : link->visual_array)
+        {
+          assignMaterial(visual, model, link->name.c_str());
         }
 
         model->links_.insert(make_pair(link->name,link));