Store dotgraph in TaskComposerLog

tesseract-robotics · Oct 12, 2024 · e6f96b1 · e6f96b1
1 parent 7a86023
commit e6f96b1
Show file tree

Hide file tree

Showing 5 changed files with 59 additions and 4 deletions.
diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_log.h
@@ -35,11 +35,13 @@ class TaskComposerContext;
 class TaskComposerLog
 {
 public:
+  TaskComposerLog(std::string desc = "");
   virtual ~TaskComposerLog() = default;
 
+  std::string description;
   TaskComposerDataStorage initial_data;
   std::shared_ptr<TaskComposerContext> context;
-  std::string description;
+  std::string dotgraph;
 
   bool operator==(const TaskComposerLog& rhs) const;
   bool operator!=(const TaskComposerLog& rhs) const;

diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h
@@ -147,6 +147,14 @@ class TaskComposerNode
   /** @brief Get the ports associated with the node */
   TaskComposerNodePorts getPorts() const;
 
+  /** @brief Generate the Dotgraph as a string */
+  std::string
+  getDotgraph(const std::map<boost::uuids::uuid, std::unique_ptr<TaskComposerNodeInfo>>& results_map = {}) const;
+
+  /** @brief Generate the Dotgraph and save to file */
+  bool saveDotgraph(const std::string& filepath,
+                    const std::map<boost::uuids::uuid, std::unique_ptr<TaskComposerNodeInfo>>& results_map = {}) const;
+
   /** @brief Rename input keys */
   virtual void renameInputKeys(const std::map<std::string, std::string>& input_keys);
 

diff --git a/tesseract_task_composer/core/src/task_composer_log.cpp b/tesseract_task_composer/core/src/task_composer_log.cpp
@@ -23,9 +23,12 @@
 
 namespace tesseract_planning
 {
+TaskComposerLog::TaskComposerLog(std::string desc) : description(std::move(desc)) {}
+
 bool TaskComposerLog::operator==(const TaskComposerLog& rhs) const
 {
-  return ((initial_data == rhs.initial_data) && (*context == *rhs.context) && (description == rhs.description));
+  return ((description == rhs.description) && (initial_data == rhs.initial_data) && (*context == *rhs.context) &&
+          (dotgraph == rhs.dotgraph));
 }
 
 // LCOV_EXCL_START
@@ -35,9 +38,10 @@ bool TaskComposerLog::operator!=(const TaskComposerLog& rhs) const { return !ope
 template <class Archive>
 void TaskComposerLog::serialize(Archive& ar, const unsigned int /*version*/)
 {
+  ar& BOOST_SERIALIZATION_NVP(description);
   ar& BOOST_SERIALIZATION_NVP(initial_data);
   ar& BOOST_SERIALIZATION_NVP(context);
-  ar& BOOST_SERIALIZATION_NVP(description);
+  ar& BOOST_SERIALIZATION_NVP(dotgraph);
 }
 
 }  // namespace tesseract_planning

diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp
@@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/uuid/uuid_io.hpp>
 #include <boost/uuid/uuid_serialize.hpp>
 #include <yaml-cpp/yaml.h>
+#include <console_bridge/console.h>
 #include <tesseract_common/serialization.h>
 #include <tesseract_common/stopwatch.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -383,6 +384,45 @@ const TaskComposerKeys& TaskComposerNode::getOutputKeys() const { return output_
 
 TaskComposerNodePorts TaskComposerNode::getPorts() const { return ports_; }
 
+std::string TaskComposerNode::getDotgraph(
+    const std::map<boost::uuids::uuid, std::unique_ptr<TaskComposerNodeInfo>>& results_map) const
+{
+  try
+  {
+    // Save dot graph
+    std::stringstream dotgraph;
+    dump(dotgraph, nullptr, results_map);
+    return dotgraph.str();
+  }
+  catch (const std::exception& e)
+  {
+    CONSOLE_BRIDGE_logError("Failed to generated DOT Graph: '%s'!", e.what());
+  }
+
+  return {};
+}
+
+bool TaskComposerNode::saveDotgraph(
+    const std::string& filepath,
+    const std::map<boost::uuids::uuid, std::unique_ptr<TaskComposerNodeInfo>>& results_map) const
+{
+  try
+  {
+    // Save dot graph
+    std::ofstream os;
+    os.open(filepath);
+    dump(os, nullptr, results_map);
+    os.close();
+    return true;
+  }
+  catch (const std::exception& e)
+  {
+    CONSOLE_BRIDGE_logError("Failed to save DOT Graph: '%s'!", e.what());
+  }
+
+  return false;
+}
+
 void TaskComposerNode::renameInputKeys(const std::map<std::string, std::string>& input_keys)
 {
   input_keys_.rename(input_keys);

diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp
@@ -2553,7 +2553,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests)  /
       data->setData("input_data", context->data_storage->getData("output_data"));
       EXPECT_GE(context->data_storage->getData("output_data").as<CompositeInstruction>().size(), 10);
     }
-    tesseract_planning::TaskComposerLog log;
+    tesseract_planning::TaskComposerLog log("TaskComposerMotionPlannerNodeInfoTests");
     auto profiles = std::make_shared<ProfileDictionary>();
     data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
     data->setData("profiles", profiles);
@@ -2563,6 +2563,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests)  /
     MotionPlannerTask<TrajOptMotionPlanner> task(
         "abc", "input_data", "environment", "profiles", "output_data", false, true);
     EXPECT_EQ(task.run(*log.context), 1);
+    log.dotgraph = task.getDotgraph(log.context->task_infos.getInfoMap());
     auto node_info = log.context->task_infos.getInfo(task.getUUID());
     EXPECT_EQ(node_info->color, "green");
     EXPECT_EQ(node_info->return_value, 1);