diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index 56c632cc5f..2627f3491f 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -41,6 +41,7 @@ add_library( ${PROJECT_NAME}_nodes src/nodes/done_task.cpp src/nodes/error_task.cpp + src/nodes/has_data_storage_entry_task.cpp src/nodes/remap_task.cpp src/nodes/start_task.cpp src/nodes/sync_task.cpp diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/has_data_storage_entry_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/has_data_storage_entry_task.h new file mode 100644 index 0000000000..674ee00d4d --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/has_data_storage_entry_task.h @@ -0,0 +1,49 @@ +#ifndef TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H +#define TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; +class HasDataStorageEntryTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + HasDataStorageEntryTask(); + explicit HasDataStorageEntryTask(std::string name, + const std::vector& input_keys, + bool is_conditional = true); + explicit HasDataStorageEntryTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory); + ~HasDataStorageEntryTask() override = default; + + bool operator==(const HasDataStorageEntryTask& rhs) const; + bool operator!=(const HasDataStorageEntryTask& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT + + std::unique_ptr + runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; +}; + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::HasDataStorageEntryTask, "HasDataStorageEntryTask") + +#endif // TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H diff --git a/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp b/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp new file mode 100644 index 0000000000..404a36be5d --- /dev/null +++ b/tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp @@ -0,0 +1,76 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace tesseract_planning +{ +HasDataStorageEntryTask::HasDataStorageEntryTask() : TaskComposerTask("HasDataStorageEntryTask", true) {} +HasDataStorageEntryTask::HasDataStorageEntryTask(std::string name, + const std::vector& input_keys, + bool is_conditional) + : TaskComposerTask(std::move(name), is_conditional) +{ + input_keys_ = input_keys; + if (input_keys_.empty()) + throw std::runtime_error("HasDataStorageEntryTask, input_keys should not be empty!"); +} +HasDataStorageEntryTask::HasDataStorageEntryTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ + if (input_keys_.empty()) + throw std::runtime_error("HasDataStorageEntryTask, input_keys should not be empty!"); + + if (!output_keys_.empty()) + throw std::runtime_error("HasDataStorageEntryTask, output_keys should be empty!"); +} + +std::unique_ptr HasDataStorageEntryTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const +{ + auto info = std::make_unique(*this); + for (const auto& input_key : input_keys_) + { + if (!context.data_storage->hasKey(input_key)) + { + info->color = "red"; + info->return_value = 0; + info->status_code = 0; + info->status_message = "Missing input key"; + return info; + } + } + + info->color = "green"; + info->return_value = 1; + info->status_code = 1; + info->status_message = "Successful"; + return info; +} + +bool HasDataStorageEntryTask::operator==(const HasDataStorageEntryTask& rhs) const +{ + return (TaskComposerNode::operator==(rhs)); +} +bool HasDataStorageEntryTask::operator!=(const HasDataStorageEntryTask& rhs) const { return !operator==(rhs); } + +template +void HasDataStorageEntryTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::HasDataStorageEntryTask) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::HasDataStorageEntryTask) diff --git a/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp b/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp index 9967b53b2e..4b39010160 100644 --- a/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp +++ b/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -40,6 +41,7 @@ namespace tesseract_planning { using DoneTaskFactory = TaskComposerTaskFactory; using ErrorTaskFactory = TaskComposerTaskFactory; +using HasDataStorageEntryTaskFactory = TaskComposerTaskFactory; using RemapTaskFactory = TaskComposerTaskFactory; using StartTaskFactory = TaskComposerTaskFactory; using SyncTaskFactory = TaskComposerTaskFactory; @@ -57,11 +59,14 @@ namespace tesseract_planning::test_suite using TestTaskFactory = TaskComposerTaskFactory; } +// clang-format off // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::DoneTaskFactory, DoneTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ErrorTaskFactory, ErrorTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::HasDataStorageEntryTaskFactory, HasDataStorageEntryTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::RemapTaskFactory, RemapTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::StartTaskFactory, StartTaskFactory) @@ -73,3 +78,4 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::GraphTaskFactory, Gr TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::PipelineTaskFactory, PipelineTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::test_suite::TestTaskFactory, TestTaskFactory) +// clang-format on diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index ef1b2beba7..5e8675ff18 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -40,6 +40,7 @@ set(LIB_SOURCE_FILES src/nodes/fix_state_bounds_task.cpp src/nodes/fix_state_collision_task.cpp src/nodes/format_as_input_task.cpp + src/nodes/format_as_result_task.cpp src/nodes/profile_switch_task.cpp src/nodes/min_length_task.cpp src/nodes/motion_planner_task_info.cpp diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_result_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_result_task.h new file mode 100644 index 0000000000..2ccf40c267 --- /dev/null +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_result_task.h @@ -0,0 +1,50 @@ +#ifndef TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H +#define TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; +class FormatAsResultTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + FormatAsResultTask(); + explicit FormatAsResultTask(std::string name, + const std::vector& input_keys, + const std::vector& output_keys, + bool is_conditional = true); + explicit FormatAsResultTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory); + ~FormatAsResultTask() override = default; + + bool operator==(const FormatAsResultTask& rhs) const; + bool operator!=(const FormatAsResultTask& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT + + std::unique_ptr + runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; +}; + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::FormatAsResultTask, "FormatAsResultTask") + +#endif // TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H diff --git a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp index f9734326fa..d3455f990c 100644 --- a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp +++ b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ using DiscreteContactCheckTaskFactory = TaskComposerTaskFactory; using FixStateCollisionTaskFactory = TaskComposerTaskFactory; using FormatAsInputTaskFactory = TaskComposerTaskFactory; +using FormatAsResultTaskFactory = TaskComposerTaskFactory; using MinLengthTaskFactory = TaskComposerTaskFactory; using ProfileSwitchTaskFactory = TaskComposerTaskFactory; using UpsampleTrajectoryTaskFactory = TaskComposerTaskFactory; @@ -79,6 +81,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FixStateCollisionTas // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsInputTaskFactory, FormatAsInputTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsResultTaskFactory, FormatAsResultTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::MinLengthTaskFactory, MinLengthTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ProfileSwitchTaskFactory, ProfileSwitchTaskFactory) diff --git a/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp b/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp new file mode 100644 index 0000000000..e8fa6e1321 --- /dev/null +++ b/tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp @@ -0,0 +1,104 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +FormatAsResultTask::FormatAsResultTask() : TaskComposerTask("FormatAsResultTask", true) {} + +FormatAsResultTask::FormatAsResultTask(std::string name, + const std::vector& input_keys, + const std::vector& output_keys, + bool is_conditional) + : TaskComposerTask(std::move(name), is_conditional) +{ + input_keys_ = input_keys; + output_keys_ = output_keys; + + if (input_keys_.empty()) + throw std::runtime_error("FormatAsResultTask, input_keys should not be empty!"); + + if (output_keys_.empty()) + throw std::runtime_error("FormatAsResultTask, output_keys should not be empty!"); + + if (input_keys_.size() != output_keys_.size()) + throw std::runtime_error("FormatAsResultTask, input_keys and output_keys be the same size!"); +} + +FormatAsResultTask::FormatAsResultTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ + if (input_keys_.empty()) + throw std::runtime_error("FormatAsResultTask, input_keys should not be empty!"); + + if (output_keys_.empty()) + throw std::runtime_error("FormatAsResultTask, output_keys should not be empty!"); + + if (input_keys_.size() != output_keys_.size()) + throw std::runtime_error("FormatAsResultTask, input_keys and output_keys be the same size!"); +} + +std::unique_ptr FormatAsResultTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const +{ + for (std::size_t i = 0; i < input_keys_.size(); ++i) + { + auto input_data = context.data_storage->getData(input_keys_[i]); + auto& ci = input_data.as(); + std::vector> instructions = ci.flatten(&moveFilter); + for (auto& instruction : instructions) + { + auto& mi = instruction.get().as(); + if (mi.getWaypoint().isStateWaypoint()) + continue; + + if (!mi.getWaypoint().isJointWaypoint()) + throw std::runtime_error("FormatAsResultTask, unsupported waypoint type!"); + + auto& jwp = mi.getWaypoint().as(); + + // Convert to StateWaypoint + StateWaypointPoly swp = mi.createStateWaypoint(); + swp.setName(jwp.getName()); + swp.setNames(jwp.getNames()); + swp.setPosition(jwp.getPosition()); + mi.assignStateWaypoint(swp); + } + + context.data_storage->setData(output_keys_[i], ci); + } + + auto info = std::make_unique(*this); + info->color = "green"; + info->return_value = 1; + info->status_code = 1; + info->status_message = "Successful"; + return info; +} + +bool FormatAsResultTask::operator==(const FormatAsResultTask& rhs) const { return (TaskComposerNode::operator==(rhs)); } +bool FormatAsResultTask::operator!=(const FormatAsResultTask& rhs) const { return !operator==(rhs); } + +template +void FormatAsResultTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsResultTask) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FormatAsResultTask) diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp index 3d9c3515d7..d0cd30ca2f 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -23,6 +23,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -2000,6 +2001,126 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT } } +TEST(TesseractTaskComposerCoreUnit, TaskComposerHasDataStorageEntryTaskTests) // NOLINT +{ + { // Construction + HasDataStorageEntryTask task; + EXPECT_EQ(task.getName(), "HasDataStorageEntryTask"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + std::vector input_keys{ "input1", "input2" }; + HasDataStorageEntryTask task("abc", input_keys); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.getInputKeys(), input_keys); + EXPECT_TRUE(task.getOutputKeys().empty()); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + std::vector input_keys{ "input1", "input2" }; + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: [input1, input2])"; + YAML::Node config = YAML::Load(str); + HasDataStorageEntryTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.getInputKeys(), input_keys); + EXPECT_TRUE(task.getOutputKeys().empty()); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction Failure + EXPECT_ANY_THROW(std::make_unique("abc", std::vector{})); // NOLINT + } + + { // Construction Failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + outputs: [output1, output2])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction Failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Serialization + std::vector input_keys{ "input1", "input2" }; + auto task = std::make_unique("abc", input_keys, true); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerHasDataStorageEntryTaskTests"); + } + + { // Test run method + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + + std::vector input_keys{ "input1", "input2" }; + HasDataStorageEntryTask task("test_run", input_keys); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->status_code, 0); + EXPECT_EQ(node_info->status_message, "Missing input key"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } + + { // Test run method + auto data_storage = std::make_unique(); + data_storage->setData("input1", std::vector{}); + auto context = + std::make_shared(std::make_unique(), std::move(data_storage)); + + std::vector input_keys{ "input1", "input2" }; + HasDataStorageEntryTask task("test_run", input_keys); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->status_code, 0); + EXPECT_EQ(node_info->status_message, "Missing input key"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } + + { // Test run method + auto data_storage = std::make_unique(); + data_storage->setData("input1", std::vector{}); + data_storage->setData("input2", std::vector{}); + auto context = + std::make_shared(std::make_unique(), std::move(data_storage)); + + std::vector input_keys{ "input1", "input2" }; + HasDataStorageEntryTask task("test_run", input_keys); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "green"); + EXPECT_EQ(node_info->return_value, 1); + EXPECT_EQ(node_info->status_code, 1); + EXPECT_EQ(node_info->status_message, "Successful"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } +} + TEST(TesseractTaskComposerCoreUnit, TaskComposerServerTests) // NOLINT { std::string str = R"(task_composer_plugins: 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 f33a20d659..ecfefbc481 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -11,6 +11,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -766,6 +767,119 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / } } +TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsResultTaskTests) // NOLINT +{ + { // Construction + FormatAsResultTask task; + EXPECT_EQ(task.getName(), "FormatAsResultTask"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: [input_data] + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + FormatAsResultTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + EXPECT_EQ(task.getInputKeys().size(), 1); + EXPECT_EQ(task.getInputKeys().front(), "input_data"); + EXPECT_EQ(task.getOutputKeys().size(), 1); + EXPECT_EQ(task.getOutputKeys().front(), "output_data"); + EXPECT_EQ(task.getOutboundEdges().size(), 0); + EXPECT_EQ(task.getInboundEdges().size(), 0); + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: [input_data])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: [input_data] + outputs: [output_data, output_data2])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Serialization + auto task = std::make_unique(); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerFormatAsResultTaskTests"); + } + + { // Test run method + auto profiles = std::make_shared(); + auto data = std::make_unique(); + CompositeInstruction compare = test_suite::jointInterpolateExampleProgramABB(false); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); + std::vector input_keys{ "input_data" }; + std::vector output_keys{ "output_data" }; + FormatAsResultTask task("abc", input_keys, output_keys, true); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "green"); + EXPECT_EQ(node_info->return_value, 1); + EXPECT_EQ(node_info->status_code, 1); + EXPECT_EQ(node_info->status_message.empty(), false); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data"), compare); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } + + { // Failure missing input data [0] + auto profiles = std::make_shared(); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); + std::vector input_keys{ "input_data" }; + std::vector output_keys{ "output_data" }; + FormatAsResultTask task("abc", input_keys, output_keys, true); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->status_code, -1); // Indicates an exception was thrown + EXPECT_EQ(node_info->status_message.empty(), false); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } +} + TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NOLINT { { // Construction