Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Several improvements to TF display #1789

Merged
merged 4 commits into from
Mar 29, 2023
Merged

Several improvements to TF display #1789

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b9f72e2
Create tree nodes also if the frame is not connected to rviz' global …
rhaschke Mar 28, 2023
a21ff8a
Insert frame properties sorted
rhaschke Mar 29, 2023
9ba642a
Don't postpone insertion of frames into tree
rhaschke Mar 29, 2023
0d19929
Reparent children to root tree node when deleting a frame
rhaschke Mar 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
92 changes: 46 additions & 46 deletions src/rviz/default_plugin/tf_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -377,8 +377,6 @@ void TFDisplay::updateFrames()
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif
std::sort(frames.begin(), frames.end());

S_FrameInfo current_frames;

{
Expand All @@ -403,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();
Expand Down Expand Up @@ -445,8 +439,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_);
Expand Down Expand Up @@ -564,6 +559,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))
Expand All @@ -576,26 +574,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();
Expand Down Expand Up @@ -679,31 +675,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_);
}
}

Expand All @@ -724,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;
Expand Down
9 changes: 9 additions & 0 deletions src/rviz/properties/property.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
3 changes: 3 additions & 0 deletions src/rviz/properties/property.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down