Maintain original (mesh) materials for RobotLinks (#1704)

rviz modifies a custom material (loaded together with a mesh model) within several functions, e.g. updateAlpha().
This destroys the original properties of this material, e.g. transparency.
To be able to restore the original material, we store it additionally to the active one.

src/rviz/robot/robot_link.cpp

@@ -383,32 +383,35 @@ void RobotLink::setOnlyRenderDepth(bool onlyRenderDepth)
 void RobotLink::updateAlpha()
 {
   float link_alpha = alpha_property_->getFloat();
-  M_SubEntityToMaterial::iterator it = materials_.begin();
-  M_SubEntityToMaterial::iterator end = materials_.end();
-  for (; it != end; ++it)
+  for (auto& item : materials_)
   {
-    const Ogre::MaterialPtr& material = it->second;
+    Ogre::MaterialPtr& active = item.second.first;
+    const Ogre::MaterialPtr& original = item.second.second;
 
     if (only_render_depth_)
     {
-      material->setColourWriteEnabled(false);
-      material->setDepthWriteEnabled(true);
+      active->setColourWriteEnabled(false);
+      active->setDepthWriteEnabled(true);
     }
     else
     {
-      Ogre::ColourValue color = material->getTechnique(0)->getPass(0)->getDiffuse();
+      Ogre::ColourValue color = active->getTechnique(0)->getPass(0)->getDiffuse();
       color.a = robot_alpha_ * material_alpha_ * link_alpha;
-      material->setDiffuse(color);
+      active->setDiffuse(color);
 
-      if (color.a < 0.9998)
+      if (color.a < 0.9998) // transparent
       {
-        material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
-        material->setDepthWriteEnabled(false);
+        active->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+        active->setDepthWriteEnabled(false);
       }
-      else
+      else if (active == original)
+      {
+        active->setSceneBlending(Ogre::SBT_REPLACE);
+        active->setDepthWriteEnabled(true);
+      }
+      else // restore original material
       {
-        material->setSceneBlending(Ogre::SBT_REPLACE);
-        material->setDepthWriteEnabled(true);
+        original->copyDetailsTo(active);
       }
     }
   }
@@ -655,19 +658,23 @@ void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& l
       Ogre::SubEntity* sub = entity->getSubEntity(i);
       const std::string& material_name = sub->getMaterialName();
 
+      Ogre::MaterialPtr active, original;
       if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting")
       {
-        sub->setMaterial(default_material_);
+        sub->setMaterial(active = default_material_);
+        original = active; // we don't need a backup copy of the default material
       }
       else
       {
-        // create a new material copy for each instance of a RobotLink
-        Ogre::MaterialPtr mat = Ogre::MaterialPtr(new Ogre::Material(
+        // create a new material copy for each instance of a RobotLink to allow modification per link
+        active = Ogre::MaterialPtr(new Ogre::Material(
             nullptr, material_name, 0, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME));
-        *mat = *sub->getMaterial();
-        sub->setMaterial(mat);
+        *active = *sub->getMaterial();
+        sub->setMaterial(active);
+        // create a backup of the material as we will modify the active one e.g. in updateAlpha()
+        original = active->clone(sub->getMaterial()->getName() + "_original");
       }
-      materials_[sub] = sub->getMaterial();
+      materials_[sub] = std::make_pair(active, original);
     }
   }
 }
@@ -923,11 +930,9 @@ void RobotLink::setToNormalMaterial()
   }
   else
   {
-    M_SubEntityToMaterial::iterator it = materials_.begin();
-    M_SubEntityToMaterial::iterator end = materials_.end();
-    for (; it != end; ++it)
+    for (const auto& item : materials_)
     {
-      it->first->setMaterial(it->second);
+      item.first->setMaterial(item.second.first);
     }
   }
 }

src/rviz/robot/robot_link.h

@@ -214,7 +214,9 @@ private Q_SLOTS:
   FloatProperty* alpha_property_;
 
 private:
-  typedef std::map<Ogre::SubEntity*, Ogre::MaterialPtr> M_SubEntityToMaterial;
+  // maintain the original material of each SubEntity to restore it after unsetColor()
+  using M_SubEntityToMaterial =
+      std::map<Ogre::SubEntity*, std::pair<Ogre::MaterialPtr, Ogre::MaterialPtr>>;
   M_SubEntityToMaterial materials_;
   Ogre::MaterialPtr default_material_;
   std::string default_material_name_;