From b9f72e2cd053e420d5d4ae63ea9e8c348df49217 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Mar 2023 22:38:09 +0200 Subject: [PATCH 1/4] Create tree nodes also if the frame is not connected to rviz' global frame --- src/rviz/default_plugin/tf_display.cpp | 33 +++++++++++++------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index 63247c6d31..dad3f5b1d1 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -564,6 +564,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)) @@ -576,26 +579,24 @@ 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)); + 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->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(); From a21ff8a061720c1b6448ff39e20f7690778b0ef8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 Mar 2023 08:22:43 +0200 Subject: [PATCH 2/4] Insert frame properties sorted Sorting frame names before insertion just ensures that frames are inserted initially in sorted fashion. To also insert frames showing up later in a sorted fashion, we need to explicitly insert them sorted. --- src/rviz/default_plugin/tf_display.cpp | 12 ++++++------ src/rviz/properties/property.cpp | 9 +++++++++ src/rviz/properties/property.h | 3 +++ 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index dad3f5b1d1..5a64bdb0b3 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -377,8 +377,6 @@ void TFDisplay::updateFrames() #ifndef _WIN32 #pragma GCC diagnostic pop #endif - std::sort(frames.begin(), frames.end()); - S_FrameInfo current_frames; { @@ -445,8 +443,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_); @@ -698,13 +697,14 @@ void TFDisplay::updateFrame(FrameInfo* frame) else if (!frame->tree_property_) // create new property { frame->tree_property_ = - new Property(QString::fromStdString(frame->name_), QVariant(), "", parent_tree_property); + new Property(QString::fromStdString(frame->name_), QVariant(), ""); + parent_tree_property->insertChildSorted(frame->tree_property_); } else // update property { // re-parent the tree property frame->tree_property_->getParent()->takeChild(frame->tree_property_); - parent_tree_property->addChild(frame->tree_property_); + parent_tree_property->insertChildSorted(frame->tree_property_); } } diff --git a/src/rviz/properties/property.cpp b/src/rviz/properties/property.cpp index fd1f3c0244..2495c9bf65 100644 --- a/src/rviz/properties/property.cpp +++ b/src/rviz/properties/property.cpp @@ -381,6 +381,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 3b8cb80f44..81bd1229f4 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); From 9ba642a0989847190156b4c43d42c703032345cc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 Mar 2023 09:19:45 +0200 Subject: [PATCH 3/4] Don't postpone insertion of frames into tree --- src/rviz/default_plugin/tf_display.cpp | 29 ++++++++++++-------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index 5a64bdb0b3..27856a56d9 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -679,30 +679,27 @@ 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(), ""); + 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->insertChildSorted(frame->tree_property_); } From 0d1992970211af3354634b6a1a68ac9eb10c858a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 Mar 2023 09:30:20 +0200 Subject: [PATCH 4/4] Reparent children to root tree node when deleting a frame --- src/rviz/default_plugin/tf_display.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index 27856a56d9..813a0fe7de 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -401,16 +401,12 @@ void TFDisplay::updateFrames() } } + 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(); @@ -722,6 +718,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;