Maintain original mesh materials for RobotLinks

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_;