From a44ff8f684e3c33185cfaf1dc99a341d25d42c61 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 17 Apr 2024 11:05:37 +0200
Subject: [PATCH] Gracefully handle loops in TF tree

TF display was segfaulting when a loop was introduced in the tree_category_.
---
 src/rviz/default_plugin/tf_display.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp
index 994bafba5..cb1983cbf 100644
--- a/src/rviz/default_plugin/tf_display.cpp
+++ b/src/rviz/default_plugin/tf_display.cpp
@@ -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();
@@ -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_;