Add getRootNode to TaskComposerGraph and update TaskComposerNodeInfo …

…to store root_uuid
tesseract-robotics · Aug 16, 2024 · a345a15 · a345a15
1 parent c2c6e17
commit a345a15
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 11 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
@@ -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
@@ -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
@@ -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.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp
@@ -141,8 +141,6 @@ int TaskComposerNode::run(TaskComposerContext& context, OptionalTaskComposerExec
   {
     auto info = std::make_unique<TaskComposerNodeInfo>(*this);
     info->start_time = start_time;
-    info->input_keys = input_keys_;
-    info->output_keys = output_keys_;
     info->return_value = 0;
     info->color = "grey";
     info->status_code = 0;

diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp
@@ -59,8 +59,10 @@ 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();
+    assert(!root_uuid.is_nil());
   }
 }
 
@@ -72,6 +74,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 +107,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
@@ -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!");