diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index 9ce72407ce..6855905423 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -28,9 +28,13 @@ */ #include +#include #include #include +#include +#include +#include #include #include @@ -156,6 +160,75 @@ void FrameSelectionHandler::setOrientation(const Ogre::Quaternion& orientation) } } +class RegexValidator : public QValidator +{ + QLineEdit* editor_; + +public: + RegexValidator(QLineEdit* editor) : QValidator(editor), editor_(editor) + { + } + QValidator::State validate(QString& input, int& /*pos*/) const override + { + 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; + } + } +}; + +class RegexFilterProperty : public StringProperty +{ + std::regex default_; + std::regex regex_; + + void 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_; + } + } + } + +public: + 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& regex() const + { + return regex_; + } + + QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option) override + { + auto* editor = qobject_cast(StringProperty::createEditor(parent, option)); + if (editor) + editor->setValidator(new RegexValidator(editor)); + return editor; + } +}; + typedef std::set S_FrameInfo; TFDisplay::TFDisplay() : Display(), update_timer_(0.0f), changing_single_frame_enabled_state_(false) @@ -193,6 +266,9 @@ TFDisplay::TFDisplay() : Display(), update_timer_(0.0f), changing_single_frame_e this); frame_timeout_property_->setMin(1); + filter_whitelist_property_ = new RegexFilterProperty("Filter (whitelist)", std::regex(""), this); + filter_blacklist_property_ = new RegexFilterProperty("Filter (blacklist)", std::regex(), this); + frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this); all_enabled_property_ = @@ -365,42 +441,40 @@ void TFDisplay::updateFrames() typedef std::vector V_string; V_string frames; context_->getTF2BufferPtr()->_getFrameStrings(frames); - std::sort(frames.begin(), frames.end()); - S_FrameInfo current_frames; + // filter frames according to white- 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; + } + S_FrameInfo current_frames; + for (it = frames.begin(); it != end; ++it) { - for (const std::string& frame : frames) + FrameInfo* info = getFrameInfo(*it); + if (!info) { - if (frame.empty()) - { - continue; - } - - FrameInfo* info = getFrameInfo(frame); - if (!info) - { - info = createFrame(frame); - } - else - { - updateFrame(info); - } - - current_frames.insert(info); + info = createFrame(*it); } + else + { + updateFrame(info); + } + + current_frames.insert(info); } + for (auto frame_it = frames_.begin(), frame_end = frames_.end(); frame_it != frame_end;) { - M_FrameInfo::iterator frame_it = frames_.begin(); - M_FrameInfo::iterator frame_end = frames_.end(); - while (frame_it != frame_end) - { - if (current_frames.find(frame_it->second) == current_frames.end()) - frame_it = deleteFrame(frame_it, true); - else - ++frame_it; - } + if (current_frames.find(frame_it->second) == current_frames.end()) + frame_it = deleteFrame(frame_it, true); + else + ++frame_it; } context_->queueRender(); @@ -433,8 +507,9 @@ FrameInfo* TFDisplay::createFrame(const std::string& frame) info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); info->enabled_property_ = new BoolProperty(QString::fromStdString(info->name_), true, - "Enable or disable this individual frame.", - frames_category_, SLOT(updateVisibilityFromFrame()), info); + "Enable or disable this individual frame.", nullptr, + SLOT(updateVisibilityFromFrame()), info); + frames_category_->insertChildSorted(info->enabled_property_); info->parent_property_ = new StringProperty("Parent", "", "Parent of this frame. (Not editable)", info->enabled_property_); @@ -544,6 +619,9 @@ void TFDisplay::updateFrame(FrameInfo* frame) setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); + float scale = scale_property_->getFloat(); + bool frame_enabled = frame->enabled_property_->getBool(); + Ogre::Vector3 position; Ogre::Quaternion orientation; if (!context_->getFrameManager()->getTransform(frame->name_, ros::Time(), position, orientation)) @@ -556,28 +634,25 @@ void TFDisplay::updateFrame(FrameInfo* frame) frame->name_node_->setVisible(false); frame->axes_->getSceneNode()->setVisible(false); frame->parent_arrow_->getSceneNode()->setVisible(false); - return; } + else + { + frame->selection_handler_->setPosition(position); + frame->selection_handler_->setOrientation(orientation); - frame->selection_handler_->setPosition(position); - frame->selection_handler_->setOrientation(orientation); - - bool frame_enabled = frame->enabled_property_->getBool(); - - frame->axes_->setPosition(position); - frame->axes_->setOrientation(orientation); - frame->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() && frame_enabled); - float scale = scale_property_->getFloat(); - frame->axes_->setScale(Ogre::Vector3(scale, scale, scale)); - float alpha = alpha_property_->getFloat(); - frame->axes_->updateAlpha(alpha); + frame->axes_->setPosition(position); + frame->axes_->setOrientation(orientation); + frame->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() && frame_enabled); + frame->axes_->setScale(Ogre::Vector3(scale, scale, scale)); + frame->axes_->updateAlpha(alpha_property_->getFloat()); - frame->name_node_->setPosition(position); - frame->name_node_->setVisible(show_names_property_->getBool() && frame_enabled); - frame->name_node_->setScale(scale, scale, scale); + frame->name_node_->setPosition(position); + frame->name_node_->setVisible(show_names_property_->getBool() && frame_enabled); + frame->name_node_->setScale(scale, scale, scale); - frame->position_property_->setVector(position); - frame->orientation_property_->setQuaternion(orientation); + frame->position_property_->setVector(position); + frame->orientation_property_->setQuaternion(orientation); + } std::string old_parent = frame->parent_; frame->parent_.clear(); @@ -651,31 +726,29 @@ void TFDisplay::updateFrame(FrameInfo* frame) frame->parent_arrow_->getSceneNode()->setVisible(false); } - // If this frame has no tree property or the parent has changed, - if (!frame->tree_property_ || old_parent != frame->parent_) + // If this frame has no tree property yet or the parent has changed, + if (!frame->tree_property_ || old_parent != frame->parent_ || + // or its actual parent was not yet created + (has_parent && frame->tree_property_->getParent() == tree_category_)) { // Look up the new parent. FrameInfo* parent = has_parent ? getFrameInfo(frame->parent_) : nullptr; // Retrieve tree property to add the new child at - rviz::Property* parent_tree_property = has_parent ? nullptr : tree_category_; - if (parent && parent->tree_property_) + rviz::Property* parent_tree_property; + if (parent && parent->tree_property_) // parent already created parent_tree_property = parent->tree_property_; - else if (has_parent) // otherwise reset parent_ to retry if the parent property was created - frame->parent_ = old_parent; + else // create (temporarily) at root + parent_tree_property = tree_category_; - // If the parent has a tree property, make a new tree property for this frame. - if (!parent_tree_property) - ; // nothing more to do - else if (!frame->tree_property_) // create new property - { - frame->tree_property_ = - new Property(QString::fromStdString(frame->name_), QVariant(), "", parent_tree_property); + if (!frame->tree_property_) + { // create new tree node + frame->tree_property_ = new Property(QString::fromStdString(frame->name_)); + parent_tree_property->insertChildSorted(frame->tree_property_); } - else // update property - { - // re-parent the tree property + else if (frame->tree_property_->getParent() != parent_tree_property) + { // re-parent existing tree property frame->tree_property_->getParent()->takeChild(frame->tree_property_); - parent_tree_property->addChild(frame->tree_property_); + parent_tree_property->insertChildSorted(frame->tree_property_); } } @@ -696,6 +769,12 @@ TFDisplay::M_FrameInfo::iterator TFDisplay::deleteFrame(M_FrameInfo::iterator it if (delete_properties) { delete frame->enabled_property_; + // re-parent all children to root tree node + for (int i = frame->tree_property_->numChildren() - 1; i >= 0; --i) + { + auto* child = frame->tree_property_->takeChild(frame->tree_property_->childAtUnchecked(i)); + tree_category_->insertChildSorted(child); + } delete frame->tree_property_; } delete frame; diff --git a/src/rviz/default_plugin/tf_display.h b/src/rviz/default_plugin/tf_display.h index 1d993f351d..f0ebd529a8 100644 --- a/src/rviz/default_plugin/tf_display.h +++ b/src/rviz/default_plugin/tf_display.h @@ -59,6 +59,7 @@ class VectorProperty; class FrameInfo; class FrameSelectionHandler; typedef boost::shared_ptr FrameSelectionHandlerPtr; +class RegexFilterProperty; /** @brief Displays a visual representation of the TF hierarchy. */ class TFDisplay : public Display @@ -121,6 +122,8 @@ private Q_SLOTS: FloatProperty* scale_property_; FloatProperty* alpha_property_; + RegexFilterProperty* filter_whitelist_property_; + RegexFilterProperty* filter_blacklist_property_; Property* frames_category_; Property* tree_category_; diff --git a/src/rviz/properties/property.cpp b/src/rviz/properties/property.cpp index 3268857c1d..9f8c152da0 100644 --- a/src/rviz/properties/property.cpp +++ b/src/rviz/properties/property.cpp @@ -382,6 +382,15 @@ void Property::addChild(Property* child, int index) Q_EMIT childListChanged(this); } +void Property::insertChildSorted(Property* child) +{ + auto it = std::lower_bound(children_.cbegin(), children_.cend(), child, + [](Property* element, Property* child) { + return element->getName() < child->getName(); + }); + addChild(child, it - children_.cbegin()); +} + void Property::setModel(PropertyTreeModel* model) { model_ = model; diff --git a/src/rviz/properties/property.h b/src/rviz/properties/property.h index bf0fb77074..aa1c4b7bba 100644 --- a/src/rviz/properties/property.h +++ b/src/rviz/properties/property.h @@ -346,6 +346,9 @@ class RVIZ_EXPORT Property : public QObject * child will be added at the end. */ virtual void addChild(Property* child, int index = -1); + /** @brief Insert a child property, sorted by name */ + void insertChildSorted(Property* child); + /** @brief Set the model managing this Property and all its child properties, recursively. */ void setModel(PropertyTreeModel* model); diff --git a/src/rviz/robot/robot_joint.cpp b/src/rviz/robot/robot_joint.cpp index cf8b5c4276..cc04b43416 100644 --- a/src/rviz/robot/robot_joint.cpp +++ b/src/rviz/robot/robot_joint.cpp @@ -387,10 +387,9 @@ void RobotJoint::updateAxis() axis_->getSceneNode()->setVisible(getEnabled()); axis_->setPosition(position_property_->getVector()); - axis_->setOrientation(orientation_property_->getQuaternion()); + axis_->setDirection(orientation_property_->getQuaternion() * axis_property_->getVector()); - // TODO(lucasw) store an Ogre::ColorValue and set it according to - // joint type. + // TODO(lucasw) store an Ogre::ColorValue and set it according to joint type. axis_->setColor(0.0, 0.8, 0.0, 1.0); } } @@ -421,8 +420,7 @@ void RobotJoint::setTransforms(const Ogre::Vector3& parent_link_position, if (axis_) { axis_->setPosition(position); - axis_->setOrientation(orientation); - axis_->setDirection(parent_link_orientation * axis_property_->getVector()); + axis_->setDirection(orientation * axis_property_->getVector()); } }