Added regex filter field for TF display

rviz_common/CMakeLists.txt

@@ -102,6 +102,7 @@ set(rviz_common_headers_to_moc
   include/rviz_common/properties/property_tree_model.hpp
   include/rviz_common/properties/property_tree_with_help.hpp
   include/rviz_common/properties/qos_profile_property.hpp
+  include/rviz_common/properties/regex_filter_property.hpp
   include/rviz_common/properties/ros_topic_property.hpp
   include/rviz_common/properties/status_list.hpp
   include/rviz_common/properties/status_property.hpp
@@ -184,6 +185,7 @@ set(rviz_common_source_files
   src/rviz_common/properties/ros_topic_property.cpp
   src/rviz_common/properties/quaternion_property.cpp
   src/rviz_common/properties/qos_profile_property.cpp
+  src/rviz_common/properties/regex_filter_property.cpp
   src/rviz_common/properties/splitter_handle.cpp
   src/rviz_common/properties/status_list.cpp
   src/rviz_common/properties/status_property.cpp

rviz_common/include/rviz_common/properties/regex_filter_property.hpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef RVIZ_COMMON__PROPERTIES__REGEX_FILTER_PROPERTY_HPP_
+#define RVIZ_COMMON__PROPERTIES__REGEX_FILTER_PROPERTY_HPP_
+
+#include <QValidator>
+#include <QLineEdit>
+#include <QToolTip>
+#include <QWidget>
+
+#include <regex>
+
+#include "rviz_common/properties/string_property.hpp"
+
+namespace rviz_common
+{
+namespace properties
+{
+class RegexValidator : public QValidator
+{
+  QLineEdit * editor_;
+
+public:
+  explicit RegexValidator(QLineEdit * editor);
+
+  QValidator::State validate(QString & input, int & /*pos*/) const override;
+};
+
+class RegexFilterProperty : public StringProperty
+{
+  std::regex default_;
+  std::regex regex_;
+
+  void onValueChanged();
+
+public:
+  RegexFilterProperty(const QString & name, const std::regex regex, Property * parent);
+
+  const std::regex & regex() const;
+
+  QWidget * createEditor(QWidget * parent, const QStyleOptionViewItem & option) override;
+};
+}  // end namespace properties
+}  // end namespace rviz_common
+#endif  // RVIZ_COMMON__PROPERTIES__REGEX_FILTER_PROPERTY_HPP_

rviz_common/src/rviz_common/properties/regex_filter_property.cpp

@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "rviz_common/properties/regex_filter_property.hpp"
+
+#include <QValidator>
+#include <QLineEdit>
+#include <QToolTip>
+#include <QWidget>
+
+#include <regex>
+
+#include "rviz_common/properties/string_property.hpp"
+
+namespace rviz_common
+{
+namespace properties
+{
+RegexValidator::RegexValidator(QLineEdit * editor)
+: QValidator(editor), editor_(editor)
+{
+}
+
+QValidator::State RegexValidator::validate(QString & input, int & /*pos*/) const
+{
+  try {
+    std::regex(input.toLocal8Bit().constData());
+    editor_->setStyleSheet(QString());
+    QToolTip::hideText();
+    return QValidator::Acceptable;
+  } catch (const std::regex_error & e) {
+    editor_->setStyleSheet("background: #ffe4e4");
+    QToolTip::showText(editor_->mapToGlobal(QPoint(0, 5)), tr(e.what()), editor_, QRect(), 5000);
+    return QValidator::Intermediate;
+  }
+}
+
+void RegexFilterProperty::onValueChanged()
+{
+  const auto & value = getString();
+  if (value.isEmpty()) {
+    regex_ = default_;
+  } else {
+    try {
+      regex_.assign(value.toLocal8Bit().constData(), std::regex_constants::optimize);
+    } catch (const std::regex_error & e) {
+      regex_ = default_;
+    }
+  }
+}
+
+RegexFilterProperty::RegexFilterProperty(
+  const QString & name, const std::regex regex,
+  Property * parent)
+: StringProperty(name, "", "regular expression", parent), default_(regex), regex_(regex)
+{
+  QObject::connect(this, &RegexFilterProperty::changed, this, [this]() {onValueChanged();});
+}
+
+const std::regex & RegexFilterProperty::regex() const
+{
+  return regex_;
+}
+
+QWidget * RegexFilterProperty::createEditor(QWidget * parent, const QStyleOptionViewItem & option)
+{
+  auto * editor = qobject_cast<QLineEdit *>(StringProperty::createEditor(parent, option));
+  if (editor) {
+    editor->setValidator(new RegexValidator(editor));
+  }
+  return editor;
+}
+}  // end namespace properties
+}  // end namespace rviz_common

rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp

@@ -72,6 +72,7 @@ namespace properties
 class BoolProperty;
 class FloatProperty;
 class QuaternionProperty;
+class RegexFilterProperty;
 class StringProperty;
 class VectorProperty;
 }  // namespace properties
@@ -118,6 +119,8 @@ private Q_SLOTS:
   FrameInfo * createFrame(const std::string & frame);
   void updateFrame(FrameInfo * frame);
   void deleteFrame(FrameInfo * frame, bool delete_properties);
+  typedef std::map<std::string, FrameInfo *> M_FrameInfo;
+  TFDisplay::M_FrameInfo::iterator deleteFrame(M_FrameInfo::iterator it, bool delete_properties);
   FrameInfo * getFrameInfo(const std::string & frame);
   void clear();
 
@@ -129,7 +132,6 @@ private Q_SLOTS:
   Ogre::SceneNode * arrows_node_;
   Ogre::SceneNode * axes_node_;
 
-  typedef std::map<std::string, FrameInfo *> M_FrameInfo;
   M_FrameInfo frames_;
 
   typedef std::map<std::string, bool> M_EnabledState;
@@ -146,6 +148,9 @@ private Q_SLOTS:
 
   rviz_common::properties::FloatProperty * scale_property_;
 
+  rviz_common::properties::RegexFilterProperty * filter_whitelist_property_;
+  rviz_common::properties::RegexFilterProperty * filter_blacklist_property_;
+
   rviz_common::properties::Property * frames_category_;
   rviz_common::properties::Property * tree_category_;
 

rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp

@@ -30,9 +30,14 @@
 
 #include "rviz_default_plugins/displays/tf/tf_display.hpp"
 
+#include <QValidator>
+#include <QLineEdit>
+#include <QToolTip>
+
 #include <algorithm>
 #include <cassert>
 #include <memory>
+#include <regex>
 #include <set>
 #include <string>
 #include <vector>
@@ -54,6 +59,7 @@
 #include "rviz_common/properties/bool_property.hpp"
 #include "rviz_common/properties/float_property.hpp"
 #include "rviz_common/properties/quaternion_property.hpp"
+#include "rviz_common/properties/regex_filter_property.hpp"
 #include "rviz_common/properties/string_property.hpp"
 #include "rviz_common/properties/vector_property.hpp"
 #include "rviz_common/interaction/forwards.hpp"
@@ -133,6 +139,11 @@ TFDisplay::TFDisplay()
     this);
   frame_timeout_property_->setMin(1);
 
+  filter_whitelist_property_ = new rviz_common::properties::RegexFilterProperty(
+    "Filter (whitelist)", std::regex(""), this);
+  filter_blacklist_property_ = new rviz_common::properties::RegexFilterProperty(
+    "Filter (blacklist)", std::regex(), this);
+
   frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this);
 
   all_enabled_property_ = new BoolProperty(
@@ -287,9 +298,33 @@ void TFDisplay::updateFrames()
 {
   typedef std::vector<std::string> V_string;
   V_string frames = context_->getFrameManager()->getAllFrameNames();
-  std::sort(frames.begin(), frames.end());
 
-  S_FrameInfo current_frames = createOrUpdateFrames(frames);
+  // filter frames according to white-list and black-list regular expressions
+  auto it = frames.begin(), end = frames.end();
+  while (it != end) {
+    if (it->empty() || !std::regex_search(*it, filter_whitelist_property_->regex()) ||
+      std::regex_search(*it, filter_blacklist_property_->regex()))
+    {
+      std::swap(*it, *--end);  // swap current to-be-dropped name with last one
+    } else {
+      ++it;
+    }
+  }
+
+  std::sort(frames.begin(), end);
+
+  S_FrameInfo current_frames;
+  for (it = frames.begin(); it != end; ++it) {
+    FrameInfo * info = getFrameInfo(*it);
+    if (!info) {
+      info = createFrame(*it);
+    } else {
+      updateFrame(info);
+    }
+
+    current_frames.insert(info);
+  }
+
   deleteObsoleteFrames(current_frames);
 
   context_->queueRender();
@@ -327,16 +362,13 @@ FrameInfo * TFDisplay::getFrameInfo(const std::string & frame)
 
 void TFDisplay::deleteObsoleteFrames(S_FrameInfo & current_frames)
 {
-  S_FrameInfo to_delete;
-  for (auto & frame : frames_) {
-    if (current_frames.find(frame.second) == current_frames.end()) {
-      to_delete.insert(frame.second);
+  for (auto frame_it = frames_.begin(), frame_end = frames_.end(); frame_it != frame_end; ) {
+    if (current_frames.find(frame_it->second) == current_frames.end()) {
+      frame_it = deleteFrame(frame_it, false);
+    } else {
+      ++frame_it;
     }
   }
-
-  for (auto & frame : to_delete) {
-    deleteFrame(frame, true);
-  }
 }
 
 FrameInfo * TFDisplay::createFrame(const std::string & frame)
@@ -565,6 +597,25 @@ void TFDisplay::updateParentArrowIfTransformExists(
   }
 }
 
+TFDisplay::M_FrameInfo::iterator TFDisplay::deleteFrame(
+  M_FrameInfo::iterator it,
+  bool delete_properties)
+{
+  FrameInfo * frame = it->second;
+  it = frames_.erase(it);
+
+  delete frame->axes_;
+  context_->getHandlerManager()->removeHandler(frame->axes_coll_);
+  delete frame->parent_arrow_;
+  delete frame->name_text_;
+  scene_manager_->destroySceneNode(frame->name_node_);
+  if (delete_properties) {
+    delete frame->enabled_property_;
+    delete frame->tree_property_;
+  }
+  delete frame;
+  return it;
+}
 
 void TFDisplay::deleteFrame(FrameInfo * frame, bool delete_properties)
 {
@@ -579,8 +630,12 @@ void TFDisplay::deleteFrame(FrameInfo * frame, bool delete_properties)
   delete frame->name_text_;
   scene_manager_->destroySceneNode(frame->name_node_);
   if (delete_properties) {
-    delete frame->enabled_property_;
-    delete frame->tree_property_;
+    if (frame->enabled_property_) {
+      delete frame->enabled_property_;
+    }
+    if (frame->tree_property_) {
+      delete frame->tree_property_;
+    }
   }
   delete frame;
 }