From 4d662cf613bb1784eb1e5eb55748e08abc394f83 Mon Sep 17 00:00:00 2001
From: Peter Mitrano <pmitrano@armstorm>
Date: Tue, 9 Feb 2021 22:17:52 -0500
Subject: [PATCH] first pass at toggling visibility

---
 src/rviz/default_plugin/marker_display.cpp    | 24 ++++++++++++++-----
 src/rviz/default_plugin/marker_display.h      |  2 ++
 .../default_plugin/markers/marker_base.cpp    |  5 ++++
 src/rviz/default_plugin/markers/marker_base.h |  2 ++
 4 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/src/rviz/default_plugin/marker_display.cpp b/src/rviz/default_plugin/marker_display.cpp
index 79edefd7eb..de906262d9 100644
--- a/src/rviz/default_plugin/marker_display.cpp
+++ b/src/rviz/default_plugin/marker_display.cpp
@@ -319,11 +319,6 @@ void MarkerDisplay::processAdd(const visualization_msgs::Marker::ConstPtr& messa
     }
   }
 
-  if (!ns_it.value()->isEnabled())
-  {
-    return;
-  }
-
   bool create = true;
   MarkerBasePtr marker;
 
@@ -355,6 +350,8 @@ void MarkerDisplay::processAdd(const visualization_msgs::Marker::ConstPtr& messa
   {
     marker->setMessage(message);
 
+    marker->setVisible(ns_it.value()->isEnabled());
+
     if (message->lifetime.toSec() > 0.0001f)
     {
       markers_with_expiration_.insert(marker);
@@ -445,6 +442,17 @@ void MarkerDisplay::setTopic(const QString& topic, const QString& /*datatype*/)
   marker_topic_property_->setString(topic);
 }
 
+void MarkerDisplay::setVisibilityForNamespace(const std::string& ns, bool visible)
+{
+  for (auto const &marker_it : markers_)
+  {
+    if (marker_it.first.first == ns)
+    {
+      marker_it.second->setVisible(visible);
+    }
+  }
+}
+
 /////////////////////////////////////////////////////////////////////////////////
 // MarkerNamespace
 
@@ -462,7 +470,11 @@ void MarkerNamespace::onEnableChanged()
 {
   if (!isEnabled())
   {
-    owner_->deleteMarkersInNamespace(getName().toStdString());
+    owner_->setVisibilityForNamespace(getName().toStdString(), false);
+  }
+  else
+  {
+    owner_->setVisibilityForNamespace(getName().toStdString(), true);
   }
 
   // Update the configuration that stores the enabled state of all markers
diff --git a/src/rviz/default_plugin/marker_display.h b/src/rviz/default_plugin/marker_display.h
index 6396ad1211..04e139de4d 100644
--- a/src/rviz/default_plugin/marker_display.h
+++ b/src/rviz/default_plugin/marker_display.h
@@ -91,6 +91,8 @@ class MarkerDisplay : public Display
 
   void setTopic(const QString& topic, const QString& datatype) override;
 
+  void setVisibilityForNamespace(const std::string& ns, bool visible);
+
 protected:
   void deleteMarkerInternal(const MarkerID& id);
 
diff --git a/src/rviz/default_plugin/markers/marker_base.cpp b/src/rviz/default_plugin/markers/marker_base.cpp
index 86e26ac7f1..db904c0393 100644
--- a/src/rviz/default_plugin/markers/marker_base.cpp
+++ b/src/rviz/default_plugin/markers/marker_base.cpp
@@ -141,6 +141,11 @@ const Ogre::Quaternion& MarkerBase::getOrientation() const
   return scene_node_->getOrientation();
 }
 
+void MarkerBase::setVisible(bool visible)
+{
+  scene_node_->setVisible(visible);
+}
+
 void MarkerBase::extractMaterials(Ogre::Entity* entity, S_MaterialPtr& materials)
 {
   uint32_t num_sub_entities = entity->getNumSubEntities();
diff --git a/src/rviz/default_plugin/markers/marker_base.h b/src/rviz/default_plugin/markers/marker_base.h
index f68cb92c31..97243a3467 100644
--- a/src/rviz/default_plugin/markers/marker_base.h
+++ b/src/rviz/default_plugin/markers/marker_base.h
@@ -95,6 +95,8 @@ class MarkerBase
     return S_MaterialPtr();
   }
 
+  void setVisible(bool visible);
+
 protected:
   bool transform(const MarkerConstPtr& message,
                  Ogre::Vector3& pos,