From 78796912642765ab7beefb2dfe6b4b8f9f142bd5 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Thu, 15 Aug 2024 14:20:10 -0500
Subject: [PATCH] Add getRootNode to TaskComposerGraph and update
 TaskComposerNodeInfo to store root_uuid

---
 .../core/task_composer_graph.h                     |  6 ++++++
 .../core/task_composer_node_info.h                 |  3 +++
 .../core/src/task_composer_graph.cpp               | 14 ++++++++++++++
 .../core/src/task_composer_node_info.cpp           |  3 +++
 .../core/src/task_composer_pipeline.cpp            | 10 +---------
 5 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h
index 2132a16e4b..49de1d3d75 100644
--- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h
+++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h
@@ -59,6 +59,12 @@ class TaskComposerGraph : public TaskComposerNode
   TaskComposerGraph(TaskComposerGraph&&) = delete;
   TaskComposerGraph& operator=(TaskComposerGraph&&) = delete;
 
+  /**
+   * @brief Get the root node of the graph
+   * @return The root node uuid
+   */
+  boost::uuids::uuid getRootNode() const;
+
   /**
    * @brief Add a node to the pipeline
    * @return The node ID which should be used with adding edges
diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h
index 6bce2b33f6..1a19e9fb78 100644
--- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h
+++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h
@@ -72,6 +72,9 @@ class TaskComposerNodeInfo
   /** @brief The task uuid */
   boost::uuids::uuid uuid{};
 
+  /** @brief If type is Pipeline or Graph this will be the root node of the pipeline or graph, otherwise null */
+  boost::uuids::uuid root_uuid{};
+
   /**
    * @brief The parent uuid
    * @details This is set when the node is added to a graph
diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp
index 76fb10b5e0..046a81f506 100644
--- a/tesseract_task_composer/core/src/task_composer_graph.cpp
+++ b/tesseract_task_composer/core/src/task_composer_graph.cpp
@@ -267,6 +267,20 @@ std::unique_ptr<TaskComposerNodeInfo> TaskComposerGraph::runImpl(TaskComposerCon
   throw std::runtime_error("TaskComposerGraph, with name '" + name_ + "' has no node info for any of the leaf nodes!");
 }
 
+boost::uuids::uuid TaskComposerGraph::getRootNode() const
+{
+  boost::uuids::uuid root_node{};
+  for (const auto& pair : nodes_)
+  {
+    if (pair.second->getInboundEdges().empty())
+    {
+      root_node = pair.first;
+      break;
+    }
+  }
+  return root_node;
+}
+
 boost::uuids::uuid TaskComposerGraph::addNode(std::unique_ptr<TaskComposerNode> task_node)
 {
   boost::uuids::uuid uuid = task_node->getUUID();
diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp
index 9860ab722e..90a3682999 100644
--- a/tesseract_task_composer/core/src/task_composer_node_info.cpp
+++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp
@@ -59,6 +59,7 @@ TaskComposerNodeInfo::TaskComposerNodeInfo(const TaskComposerNode& node)
   if (type == TaskComposerNodeType::GRAPH || type == TaskComposerNodeType::PIPELINE)
   {
     const auto& graph = static_cast<const TaskComposerGraph&>(node);
+    root_uuid = graph.getRootNode();
     terminals = graph.getTerminals();
     abort_terminal = graph.getAbortTerminalIndex();
   }
@@ -72,6 +73,7 @@ bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const
   equal &= name == rhs.name;
   equal &= ns == rhs.ns;
   equal &= uuid == rhs.uuid;
+  equal &= root_uuid == rhs.root_uuid;
   equal &= parent_uuid == rhs.parent_uuid;
   equal &= type == rhs.type;
   equal &= type_hash_code == rhs.type_hash_code;
@@ -104,6 +106,7 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/
   ar& boost::serialization::make_nvp("name", name);
   ar& boost::serialization::make_nvp("ns", ns);
   ar& boost::serialization::make_nvp("uuid", uuid);
+  ar& boost::serialization::make_nvp("root_uuid", root_uuid);
   ar& boost::serialization::make_nvp("parent_uuid", parent_uuid);
   ar& boost::serialization::make_nvp("type", type);
   ar& boost::serialization::make_nvp("type_hash_code", type_hash_code);
diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp
index 4c8fb4543e..f793780215 100644
--- a/tesseract_task_composer/core/src/task_composer_pipeline.cpp
+++ b/tesseract_task_composer/core/src/task_composer_pipeline.cpp
@@ -58,15 +58,7 @@ std::unique_ptr<TaskComposerNodeInfo> TaskComposerPipeline::runImpl(TaskComposer
 
   tesseract_common::Timer timer;
   timer.start();
-  boost::uuids::uuid root_node{};
-  for (const auto& pair : nodes_)
-  {
-    if (pair.second->getInboundEdges().empty())
-    {
-      root_node = pair.first;
-      break;
-    }
-  }
+  boost::uuids::uuid root_node = getRootNode();
 
   if (root_node.is_nil())
     throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + "' does not have a root node!");