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 index 376eef4ee7..ad9ae0ca3b 100644 --- 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 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 index 828519fd08..78d18cab27 100644 --- 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>& results_map = {}) const; + + /** @brief Generate the Dotgraph and save to file */ + bool saveDotgraph(const std::string& filepath, + const std::map>& results_map = {}) const; + /** @brief Rename input keys */ virtual void renameInputKeys(const std::map& input_keys); diff --git a/tesseract_task_composer/core/src/task_composer_log.cpp b/tesseract_task_composer/core/src/task_composer_log.cpp index efe8232fc8..c46b87c8be 100644 --- 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 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 index 17693ec026..649c359dc9 100644 --- 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 #include #include +#include #include #include 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>& 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>& 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& 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 index 3f8ddc091c..58aa4e7e10 100644 --- 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().size(), 10); } - tesseract_planning::TaskComposerLog log; + tesseract_planning::TaskComposerLog log("TaskComposerMotionPlannerNodeInfoTests"); auto profiles = std::make_shared(); data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); @@ -2563,6 +2563,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / MotionPlannerTask 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);