When a link has multiple visual tags, use appropriate material for each

Fixes #1293

Author: ANogin

rviz_default_plugins/include/rviz_default_plugins/robot/robot_link.hpp

@@ -177,13 +177,18 @@ private Q_SLOTS:
   Ogre::Entity * createEntityForGeometryElement(
     const urdf::LinkConstSharedPtr & link,
     const urdf::Geometry & geom, const urdf::Pose & origin,
-    std::string material_name, Ogre::SceneNode * scene_node);
+    Ogre::SceneNode * scene_node);
   void assignMaterialsToEntities(
     const urdf::LinkConstSharedPtr & link,
     const std::string & material_name,
     const Ogre::Entity * entity);
   Ogre::MaterialPtr getMaterialForLink(
     const urdf::LinkConstSharedPtr & link, std::string material_name = "");
+  Ogre::MaterialPtr getMaterialForVisual(const urdf::VisualSharedPtr & visual);
+  void getMaterialForVisualizable(Ogre::Entity * entity, const urdf::VisualSharedPtr & visual);
+  void getMaterialForVisualizable(
+    Ogre::Entity * /* entity */,
+    const urdf::CollisionSharedPtr & /* collision */ ) {};
   urdf::VisualSharedPtr getVisualWithMaterial(
     const urdf::LinkConstSharedPtr & link, const std::string & material_name) const;
   void loadMaterialFromTexture(
@@ -212,8 +217,10 @@ private Q_SLOTS:
       T link_visual_element = vector_element;
       if (link_visual_element && link_visual_element->geometry) {
         Ogre::Entity * mesh = createEntityForGeometryElement(
-          link, *link_visual_element->geometry, link_visual_element->origin, "", scene_node);
+          link, *link_visual_element->geometry, link_visual_element->origin, scene_node);
         if (mesh) {
+          getMaterialForVisualizable(mesh, link_visual_element);
+          assignMaterialsToEntities(link, "", mesh);
           meshes_vector.push_back(mesh);
           valid_visualizable_found = true;
         }
@@ -222,8 +229,10 @@ private Q_SLOTS:
 
     if (!valid_visualizable_found && visualizable_element && visualizable_element->geometry) {
       Ogre::Entity * mesh = createEntityForGeometryElement(
-        link, *visualizable_element->geometry, visualizable_element->origin, "", scene_node);
+        link, *visualizable_element->geometry, visualizable_element->origin, scene_node);
       if (mesh) {
+        getMaterialForVisualizable(mesh, visualizable_element);
+        assignMaterialsToEntities(link, "", mesh);
         meshes_vector.push_back(mesh);
       }
     }

rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp

@@ -595,7 +595,6 @@ Ogre::Entity * RobotLink::createEntityForGeometryElement(
   const urdf::LinkConstSharedPtr & link,
   const urdf::Geometry & geom,
   const urdf::Pose & origin,
-  const std::string material_name,
   Ogre::SceneNode * scene_node)
 {
   Ogre::Entity * entity = nullptr;  // default in case nothing works.
@@ -700,7 +699,6 @@ Ogre::Entity * RobotLink::createEntityForGeometryElement(
     offset_node->setPosition(offset_position);
     offset_node->setOrientation(offset_orientation);
 
-    assignMaterialsToEntities(link, material_name, entity);
   }
   return entity;
 }
@@ -757,14 +755,19 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink(
     return Ogre::MaterialManager::getSingleton().getByName("RVIZ/ShadedRed");
   }
 
+  urdf::VisualSharedPtr visual = getVisualWithMaterial(link, material_name);
+
+  return getMaterialForVisual(visual);
+}
+
+Ogre::MaterialPtr RobotLink::getMaterialForVisual(const urdf::VisualSharedPtr & visual)
+{
   static int count = 0;
-  std::string link_material_name = "Robot Link Material" + std::to_string(count++);
+  std::string link_material_name = "Robot Material" + std::to_string(count++);
 
   auto material_for_link =
     rviz_rendering::MaterialManager::createMaterialWithShadowsAndLighting(link_material_name);
 
-  urdf::VisualSharedPtr visual = getVisualWithMaterial(link, material_name);
-
   if (visual->material->texture_filename.empty()) {
     const urdf::Color & color = visual->material->color;
     material_for_link->getTechnique(0)->setAmbient(color.r * 0.5f, color.g * 0.5f, color.b * 0.5f);
@@ -778,14 +781,27 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink(
   return material_for_link;
 }
 
+void RobotLink::getMaterialForVisualizable(
+  Ogre::Entity * entity,
+  const urdf::VisualSharedPtr & visual)
+{
+    if (!visual->material_name.empty() || 
+      (visual->material && !visual->material->texture_filename.empty()))
+    {
+      Ogre::MaterialPtr material = getMaterialForVisual(visual);
+      // Sets uniformly for all subentities
+      entity -> setMaterial(material);
+      entity -> setMaterialName(material->getName());
+    }
+}
+
 urdf::VisualSharedPtr RobotLink::getVisualWithMaterial(
   const urdf::LinkConstSharedPtr & link, const std::string & material_name) const
 {
   urdf::VisualSharedPtr visual = link->visual;
   for (const auto & visual_array_element : link->visual_array) {
-    if (visual_array_element &&
-      !material_name.empty() &&
-      visual_array_element->material_name == material_name)
+    if (visual_array_element && !visual_array_element->material_name.empty() &&
+      (material_name.empty() || visual_array_element->material_name == material_name))
     {
       visual = visual_array_element;
       break;