RobotLink: Simplify material mode handling

rviz::RobotLink knows 3 material modes, showing:
- original material of the mesh
- a user-selected color
- an error color (uniform white)

Switching between these modes requires iterating through all subentities
of the link, which might be rather time-consuming. Also, this was done in
each update() cycle!

This commit simplifies the handling, performing the costly update
only if really necessary.

src/rviz/robot/robot.cpp

@@ -704,13 +704,13 @@ void Robot::update(const LinkUpdater& updater)
   {
     RobotLink* link = link_it->second;
 
-    link->setToNormalMaterial();
-
     Ogre::Vector3 visual_position, collision_position;
     Ogre::Quaternion visual_orientation, collision_orientation;
     if (updater.getLinkTransforms(link->getName(), visual_position, visual_orientation,
                                   collision_position, collision_orientation))
     {
+      link->setToNormalMaterial();
+
       // Check if visual_orientation, visual_position, collision_orientation, and collision_position are
       // NaN.
       if (visual_orientation.isNaN())

src/rviz/robot/robot_link.cpp

@@ -167,7 +167,7 @@ RobotLink::RobotLink(Robot* robot,
   , robot_alpha_(1.0)
   , only_render_depth_(false)
   , is_selectable_(true)
-  , using_color_(false)
+  , material_mode_flags_(ORIGINAL)
 {
   link_property_ = new Property(link->name.c_str(), true, "", nullptr, SLOT(updateVisibility()), this);
   link_property_->setIcon(rviz::loadPixmap("package://rviz/icons/classes/RobotLink.png"));
@@ -901,40 +901,28 @@ void RobotLink::setTransforms(const Ogre::Vector3& visual_position,
   }
 }
 
-void RobotLink::setToErrorMaterial()
+void RobotLink::setMaterialMode(unsigned char mode_flags)
 {
-  for (size_t i = 0; i < visual_meshes_.size(); i++)
-  {
-    visual_meshes_[i]->setMaterialName("BaseWhiteNoLighting",
-                                       Ogre::ResourceGroupManager::INTERNAL_RESOURCE_GROUP_NAME);
-  }
-  for (size_t i = 0; i < collision_meshes_.size(); i++)
-  {
-    collision_meshes_[i]->setMaterialName("BaseWhiteNoLighting",
-                                          Ogre::ResourceGroupManager::INTERNAL_RESOURCE_GROUP_NAME);
-  }
-}
+  if (material_mode_flags_ == mode_flags)
+    return; // nothing to change
 
-void RobotLink::setToNormalMaterial()
-{
-  if (using_color_)
-  {
-    for (size_t i = 0; i < visual_meshes_.size(); i++)
-    {
-      visual_meshes_[i]->setMaterial(color_material_);
-    }
-    for (size_t i = 0; i < collision_meshes_.size(); i++)
-    {
-      collision_meshes_[i]->setMaterial(color_material_);
-    }
-  }
-  else
+  material_mode_flags_ = mode_flags;
+
+  if (mode_flags == ORIGINAL)
   {
     for (const auto& item : materials_)
-    {
       item.first->setMaterial(item.second.first);
-    }
+    return;
   }
+
+  auto error_material = Ogre::MaterialManager::getSingleton().getByName(
+      "BaseWhiteNoLighting", Ogre::ResourceGroupManager::INTERNAL_RESOURCE_GROUP_NAME);
+  auto material = mode_flags == COLOR ? color_material_ : error_material;
+
+  for (const auto& mesh : visual_meshes_)
+    mesh->setMaterial(material);
+  for (const auto& mesh : collision_meshes_)
+    mesh->setMaterial(material);
 }
 
 void RobotLink::setColor(float red, float green, float blue)
@@ -946,14 +934,7 @@ void RobotLink::setColor(float red, float green, float blue)
   color_material_->getTechnique(0)->setAmbient(0.5 * color);
   color_material_->getTechnique(0)->setDiffuse(color);
 
-  using_color_ = true;
-  setToNormalMaterial();
-}
-
-void RobotLink::unsetColor()
-{
-  using_color_ = false;
-  setToNormalMaterial();
+  setMaterialMode(COLOR | (material_mode_flags_ & ERROR));
 }
 
 bool RobotLink::setSelectable(bool selectable)

src/rviz/robot/robot_link.h

@@ -79,6 +79,14 @@ typedef boost::shared_ptr<RobotLinkSelectionHandler> RobotLinkSelectionHandlerPt
 class RobotLink : public QObject
 {
   Q_OBJECT
+
+  enum MaterialMode
+  {
+    ORIGINAL = 0,
+    COLOR = 1,
+    ERROR = 2,
+  };
+
 public:
   RobotLink(Robot* robot,
             const urdf::LinkConstSharedPtr& link,
@@ -132,11 +140,20 @@ class RobotLink : public QObject
   // hide or show all sub properties (hide to make tree easier to see)
   virtual void hideSubProperties(bool hide);
 
-  void setToErrorMaterial();
-  void setToNormalMaterial();
+  inline void setToErrorMaterial()
+  {
+    setMaterialMode(material_mode_flags_ | ERROR);
+  }
+  inline void setToNormalMaterial()
+  {
+    setMaterialMode(material_mode_flags_ & ~ERROR);
+  }
 
   void setColor(float red, float green, float blue);
-  void unsetColor();
+  void unsetColor()
+  {
+    setMaterialMode(ORIGINAL | (material_mode_flags_ & ERROR));
+  }
 
   /// set whether the link is selectable.  If false objects behind/inside the link can be
   /// selected/manipulated.  Returns old value.
@@ -177,6 +194,7 @@ private Q_SLOTS:
   void updateAxes();
 
 private:
+  void setMaterialMode(unsigned char mode_flags);
   void setRenderQueueGroup(Ogre::uint8 group);
   bool getEnabled() const;
   void createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link,
@@ -246,7 +264,7 @@ private Q_SLOTS:
   RobotLinkSelectionHandlerPtr selection_handler_;
 
   Ogre::MaterialPtr color_material_;
-  bool using_color_;
+  unsigned char material_mode_flags_;
 
   friend class RobotLinkSelectionHandler;
 };