Add elapsed time for pipelines and include in dot graph

tesseract-robotics · Jul 17, 2023 · bbaa385 · bbaa385
1 parent 08213e8
commit bbaa385
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 3 deletions.
diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp
@@ -253,7 +253,6 @@ std::string TaskComposerGraph::dump(std::ostream& os,
   std::ostringstream sub_graphs;
   const std::string tmp = toString(uuid_);
   os << "subgraph cluster_" << tmp << " {\n color=black;\n label = \"" << name_ << "\\n(" << uuid_str_ << ")";
-  os << "\\n Conditional: " << ((conditional_) ? "True" : "False");
   os << "\\n Inputs: [";
   for (std::size_t i = 0; i < input_keys_.size(); ++i)
   {
@@ -271,6 +270,13 @@ std::string TaskComposerGraph::dump(std::ostream& os,
       os << ", ";
   }
   os << "]";
+  os << "\\n Conditional: " << ((conditional_) ? "True" : "False");
+  if (getType() == TaskComposerNodeType::PIPELINE)
+  {
+    auto it = results_map.find(getUUID());
+    if (it != results_map.end())
+      os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s";
+  }
   os << "\";";
   for (const auto& pair : nodes_)
   {
@@ -287,7 +293,6 @@ std::string TaskComposerGraph::dump(std::ostream& os,
       const std::vector<std::string>& input_keys = node->getInputKeys();
       const std::vector<std::string>& output_keys = node->getOutputKeys();
       os << std::endl << tmp << " [shape=box3d, label=\"Subgraph: " << node->name_ << "\\n(" << node->uuid_str_ << ")";
-      os << "\\n Conditional: " << ((node->isConditional()) ? "True" : "False");
       os << "\\n Inputs: [";
       for (std::size_t i = 0; i < input_keys.size(); ++i)
       {
@@ -304,7 +309,13 @@ std::string TaskComposerGraph::dump(std::ostream& os,
         if (i < output_keys.size() - 1)
           os << ", ";
       }
-      os << "]\", margin=\"0.1\", color=" << color << "];\n";  // NOLINT
+      os << "]";
+
+      os << "\\n Conditional: " << ((node->isConditional()) ? "True" : "False");
+      if (node->getType() == TaskComposerNodeType::PIPELINE && (it != results_map.end()))
+        os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s";
+
+      os << "\", margin=\"0.1\", color=" << color << "];\n";  // NOLINT
       node->dump(sub_graphs, this, results_map);
     }
   }

diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp
@@ -90,6 +90,8 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu
   if (terminals_.empty())
     throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + "' does not have terminals!");
 
+  tesseract_common::Timer timer;
+  timer.start();
   boost::uuids::uuid root_node{};
   for (const auto& pair : nodes_)
   {
@@ -110,10 +112,12 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu
     auto node_info = input.task_infos.getInfo(terminals_[i]);
     if (node_info != nullptr)
     {
+      timer.stop();
       auto info = std::make_unique<TaskComposerNodeInfo>(*this);
       info->return_value = static_cast<int>(i);
       info->color = node_info->color;
       info->message = node_info->message;
+      info->elapsed_time = timer.elapsedSeconds();
       return info;
     }
   }