diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index 516ba3980..af8450808 100644 --- a/rviz_common/CMakeLists.txt +++ b/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 diff --git a/rviz_common/include/rviz_common/properties/regex_filter_property.hpp b/rviz_common/include/rviz_common/properties/regex_filter_property.hpp new file mode 100644 index 000000000..c4cfdeead --- /dev/null +++ b/rviz_common/include/rviz_common/properties/regex_filter_property.hpp @@ -0,0 +1,77 @@ +/* + * 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 +#include +#include +#include + +#include +#include + +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace properties +{ +class RVIZ_COMMON_PUBLIC RegexValidator : public QValidator +{ +public: + explicit RegexValidator(QLineEdit * editor); + + QValidator::State validate(QString & input, int & /*pos*/) const override; + +private: + QLineEdit * editor_; +}; + +class RVIZ_COMMON_PUBLIC RegexFilterProperty : public StringProperty +{ +public: + RegexFilterProperty(const QString & name, const std::string regex, Property * parent); + + const std::regex & regex() const; + const std::string & regex_str() const; + + QWidget * createEditor(QWidget * parent, const QStyleOptionViewItem & option) override; + +private: + std::string default_; + std::regex regex_; + std::string regex_str_; + + void onValueChanged(); +}; +} // end namespace properties +} // end namespace rviz_common +#endif // RVIZ_COMMON__PROPERTIES__REGEX_FILTER_PROPERTY_HPP_ diff --git a/rviz_common/src/rviz_common/properties/regex_filter_property.cpp b/rviz_common/src/rviz_common/properties/regex_filter_property.cpp new file mode 100644 index 000000000..fedce5f6f --- /dev/null +++ b/rviz_common/src/rviz_common/properties/regex_filter_property.cpp @@ -0,0 +1,108 @@ +/* + * 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 +#include +#include +#include + +#include + +#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_ = std::regex(default_); + regex_str_ = default_; + } else { + try { + regex_str_ = std::string(value.toLocal8Bit().constData()); + regex_.assign(regex_str_, std::regex_constants::optimize); + } catch (const std::regex_error &) { + regex_ = std::regex(default_); + regex_str_ = default_; + } + } +} + +RegexFilterProperty::RegexFilterProperty( + const QString & name, const std::string regex, + Property * parent) +: StringProperty(name, "", "regular expression", parent), default_(regex), regex_(regex), + regex_str_(regex) +{ + QObject::connect(this, &RegexFilterProperty::changed, this, [this]() {onValueChanged();}); +} + +const std::regex & RegexFilterProperty::regex() const +{ + return regex_; +} + +const std::string & RegexFilterProperty::regex_str() const +{ + return regex_str_; +} + +QWidget * RegexFilterProperty::createEditor(QWidget * parent, const QStyleOptionViewItem & option) +{ + auto * editor = qobject_cast(StringProperty::createEditor(parent, option)); + if (editor) { + editor->setValidator(new RegexValidator(editor)); + } + return editor; +} +} // end namespace properties +} // end namespace rviz_common diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp index ee68150ae..45541d560 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/tf/tf_display.hpp +++ b/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 M_FrameInfo; + 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 M_FrameInfo; M_FrameInfo frames_; typedef std::map 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_; @@ -169,7 +174,6 @@ private Q_SLOTS: void updateParentTreeProperty(FrameInfo * frame) const; void deleteObsoleteFrames(std::set & current_frames); - S_FrameInfo createOrUpdateFrames(const std::vector & frames); friend class FrameInfo; }; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp index 055d6f974..fe6353c77 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp +++ b/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 +#include +#include + #include #include #include +#include #include #include #include @@ -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::string(""), this); + filter_blacklist_property_ = new rviz_common::properties::RegexFilterProperty( + "Filter (blacklist)", std::string(), this); + frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this); all_enabled_property_ = new BoolProperty( @@ -287,32 +298,45 @@ void TFDisplay::updateFrames() { typedef std::vector V_string; V_string frames = context_->getFrameManager()->getAllFrameNames(); - std::sort(frames.begin(), frames.end()); - S_FrameInfo current_frames = createOrUpdateFrames(frames); - deleteObsoleteFrames(current_frames); + // filter frames according to white-list and black-list regular expressions + V_string::iterator it = frames.begin(); + V_string::iterator end = frames.end(); - context_->queueRender(); -} + std::vector available_frames; -S_FrameInfo TFDisplay::createOrUpdateFrames(const std::vector & frames) -{ - S_FrameInfo current_frames; - for (auto & frame : frames) { - if (frame.empty()) { - continue; + if (!filter_whitelist_property_->regex_str().empty() || + !filter_blacklist_property_->regex_str().empty()) + { + while (it != end) { + if ((filter_whitelist_property_->regex_str().empty() || + std::regex_search(*it, filter_whitelist_property_->regex())) && ( + filter_blacklist_property_->regex_str().empty() || ( + !filter_blacklist_property_->regex_str().empty() && + !std::regex_search(*it, filter_blacklist_property_->regex())))) + { + available_frames.push_back(*it); + } + ++it; } + } else { + available_frames = frames; + } + S_FrameInfo current_frames; + for (auto & frame : available_frames) { FrameInfo * info = getFrameInfo(frame); if (!info) { info = createFrame(frame); } else { updateFrame(info); } - current_frames.insert(info); } - return current_frames; + + deleteObsoleteFrames(current_frames); + + context_->queueRender(); } FrameInfo * TFDisplay::getFrameInfo(const std::string & frame) @@ -327,16 +351,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 +586,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 +619,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; }