Changed Marker Displays to allow toggling visibility of namespaces (#1402)

Co-authored-by: Alejandro Hernandez Cordero <[email protected]>

Author: StefanFabian

rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp

@@ -126,6 +126,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon
   resource_retriever::Retriever * getResourceRetriever();
 
 private:
+  /** @brief Change the visibility for all markers in the given namespace. */
+  void setVisibilityForMarkersInNamespace(const std::string & ns, bool visible);
   /** @brief Delete all the markers within the given namespace. */
   void deleteMarkersInNamespace(const std::string & ns);
 
@@ -167,7 +169,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon
   typedef QHash<QString, MarkerNamespace *> M_Namespace;
   M_Namespace namespaces_;
 
-  rviz_common::properties::Property * namespaces_category_;
+  rviz_common::properties::BoolProperty * namespaces_category_;
 
   typedef std::map<QString, bool> M_EnabledState;
   M_EnabledState namespace_config_enabled_state_;
@@ -180,6 +182,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon
 
   resource_retriever::Retriever retriever_;
 
+  bool all_namespaces_enabled_ = true;
+
   friend class MarkerNamespace;
 };
 

rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/marker_base.hpp

@@ -122,6 +122,10 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerBase
 
   virtual S_MaterialPtr getMaterials() {return S_MaterialPtr();}
 
+  bool isVisible() const;
+
+  void setVisible(bool visible);
+
 protected:
   bool transform(
     const MarkerConstSharedPtr & message,
@@ -144,6 +148,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerBase
   rclcpp::Time expiration_;
 
   std::shared_ptr<MarkerSelectionHandler> handler_;
+
+  bool visible_ = true;
 };
 
 }  // namespace markers

rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_common.cpp

@@ -60,8 +60,8 @@ namespace displays
 MarkerCommon::MarkerCommon(rviz_common::Display * display)
 : display_(display)
 {
-  namespaces_category_ = new rviz_common::properties::Property(
-    "Namespaces", QVariant(), "", display_);
+  namespaces_category_ = new rviz_common::properties::BoolProperty(
+    "Namespaces", true, "Toggle to toggle all namespaces", display_);
   marker_factory_ = std::make_unique<markers::MarkerFactory>();
 }
 
@@ -118,6 +118,15 @@ void MarkerCommon::deleteMarker(MarkerID id)
   }
 }
 
+void MarkerCommon::setVisibilityForMarkersInNamespace(const std::string & ns, bool visible)
+{
+  for (auto const & marker : markers_) {
+    if (marker.first.first == ns) {
+      marker.second->setVisible(visible);
+    }
+  }
+}
+
 void MarkerCommon::deleteMarkersInNamespace(const std::string & ns)
 {
   std::vector<MarkerID> to_delete;
@@ -273,12 +282,6 @@ QHash<QString, MarkerNamespace *>::const_iterator MarkerCommon::getMarkerNamespa
 
 void MarkerCommon::processAdd(const visualization_msgs::msg::Marker::ConstSharedPtr message)
 {
-  auto ns_it = getMarkerNamespace(message);
-
-  if (!ns_it.value()->isEnabled() ) {
-    return;
-  }
-
   deleteMarkerStatus(MarkerID(message->ns, message->id));
 
   MarkerBasePtr marker = createOrGetOldMarker(message);
@@ -320,6 +323,10 @@ void MarkerCommon::configureMarker(
 {
   marker->setMessage(message);
 
+  // Visibility needs to be set after changes to the marker as they might make it visible again
+  auto ns_it = getMarkerNamespace(message);
+  marker->setVisible(ns_it.value()->isEnabled());
+
   if (rclcpp::Duration(message->lifetime).nanoseconds() > 100000) {
     markers_with_expiration_.insert(marker);
   }
@@ -347,6 +354,14 @@ void MarkerCommon::update(float wall_dt, float ros_dt)
   processNewMessages(local_queue);
   removeExpiredMarkers();
   updateMarkersWithLockedFrame();
+
+  // Workaround because this is not a QObject
+  if (all_namespaces_enabled_ != namespaces_category_->getBool()) {
+    all_namespaces_enabled_ = namespaces_category_->getBool();
+    for (auto const & ns : namespaces_) {
+      ns->setValue(all_namespaces_enabled_);
+    }
+  }
 }
 
 MarkerCommon::V_MarkerMessage MarkerCommon::takeSnapshotOfMessageQueue()
@@ -401,9 +416,7 @@ MarkerNamespace::MarkerNamespace(
 
 void MarkerNamespace::onEnableChanged()
 {
-  if (!isEnabled()) {
-    owner_->deleteMarkersInNamespace(getName().toStdString());
-  }
+  owner_->setVisibilityForMarkersInNamespace(getName().toStdString(), isEnabled());
 
   // Update the configuration that stores the enabled state of all markers
   owner_->namespace_config_enabled_state_[getName()] = isEnabled();

rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_base.cpp

@@ -155,6 +155,17 @@ const Ogre::Quaternion & MarkerBase::getOrientation()
   return scene_node_->getOrientation();
 }
 
+bool MarkerBase::isVisible() const
+{
+  return visible_;
+}
+
+void MarkerBase::setVisible(bool visible)
+{
+  visible_ = visible;
+  scene_node_->setVisible(visible);
+}
+
 void MarkerBase::extractMaterials(Ogre::Entity * entity, S_MaterialPtr & materials)
 {
   uint64_t num_sub_entities = entity->getNumSubEntities();

rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp

@@ -498,14 +498,19 @@ TEST_F(MarkerCommonFixture, onEnableChanged_in_namespace_removes_all_markers_in_
   marker->ns = "new_ns";
   common_->processMessage(marker);
 
-  EXPECT_TRUE(rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()));
+  auto pointcloud = rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode());
+  ASSERT_TRUE(pointcloud);
+  EXPECT_TRUE(pointcloud->getVisible());
   EXPECT_TRUE(rviz_default_plugins::findOneMovableText(scene_manager_->getRootSceneNode()));
 
   auto namespace_property = dynamic_cast<rviz_default_plugins::displays::MarkerNamespace *>(
     display_->findProperty("Namespaces")->childAt(0));
   namespace_property->setValue(false);
 
-  EXPECT_FALSE(rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()));
+  // Pointcloud should still exist but not visible
+  pointcloud = rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode());
+  ASSERT_TRUE(pointcloud != nullptr);
+  EXPECT_FALSE(pointcloud->getVisible());
   EXPECT_TRUE(rviz_default_plugins::findOneMovableText(scene_manager_->getRootSceneNode()));
 }
 
@@ -518,15 +523,18 @@ TEST_F(MarkerCommonFixture, processMessage_does_not_add_message_with_disabled_na
   // this is necessary to initialize namespace as we don't load a config
   common_->processMessage(marker);
 
-  EXPECT_TRUE(rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()));
+  auto pointclouds = rviz_default_plugins::findAllPointClouds(scene_manager_->getRootSceneNode());
+  ASSERT_EQ(pointclouds.size(), 1);
+  EXPECT_TRUE(pointclouds[0]->getVisible());
 
   auto namespace_property = dynamic_cast<rviz_default_plugins::displays::MarkerNamespace *>(
     display_->findProperty("Namespaces")->childAt(0));
   namespace_property->setValue(false);
 
-  marker->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
   common_->processMessage(marker);
 
-  EXPECT_FALSE(rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()));
-  EXPECT_FALSE(rviz_default_plugins::findOneMovableText(scene_manager_->getRootSceneNode()));
+  // Pointcloud should still exist but not visible
+  pointclouds = rviz_default_plugins::findAllPointClouds(scene_manager_->getRootSceneNode());
+  ASSERT_EQ(pointclouds.size(), 1);
+  EXPECT_FALSE(pointclouds[0]->getVisible());
 }