Skip to content

Commit

Permalink
Gracefully handle loops in TF tree (#1820)
Browse files Browse the repository at this point in the history
TF display was segfaulting when a loop was introduced in the tree_category_.
  • Loading branch information
rhaschke authored Apr 17, 2024
1 parent 9911c32 commit 02ea03e
Showing 1 changed file with 15 additions and 0 deletions.
15 changes: 15 additions & 0 deletions src/rviz/default_plugin/tf_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -556,6 +556,17 @@ Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourVa
return start * t + end * (1 - t);
}

bool hasLoop(rviz::Property* child, rviz::Property* parent, rviz::Property* root)
{
while (child != root)
{
if (child == parent)
return true;
child = child->getParent();
}
return false;
}

void TFDisplay::updateFrame(FrameInfo* frame)
{
auto tf = context_->getTF2BufferPtr();
Expand Down Expand Up @@ -736,7 +747,11 @@ void TFDisplay::updateFrame(FrameInfo* frame)
// Retrieve tree property to add the new child at
rviz::Property* parent_tree_property;
if (parent && parent->tree_property_) // parent already created
{
parent_tree_property = parent->tree_property_;
if (hasLoop(parent_tree_property, frame->tree_property_, tree_category_))
parent_tree_property = tree_category_; // insert loops at root node
}
else // create (temporarily) at root
parent_tree_property = tree_category_;

Expand Down

0 comments on commit 02ea03e

Please sign in to comment.