From 3d73290ebfda5a2af39bdfcc8d782eb85ad39764 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 13 Aug 2023 19:38:15 -0500 Subject: [PATCH] Fix Raster and RasterOnly Tasks --- tesseract_task_composer/README.rst | 12 ++ tesseract_task_composer/core/CMakeLists.txt | 1 + .../core/nodes/sync_task.h | 73 +++++++++++ .../core/src/nodes/sync_task.cpp | 72 +++++++++++ .../core/src/task_composer_pipeline.cpp | 6 + .../core/src/task_composer_task.cpp | 4 + .../src/task_composer_task_plugin_factory.cpp | 4 + .../planning/src/nodes/raster_motion_task.cpp | 34 ++--- .../src/nodes/raster_only_motion_task.cpp | 14 ++- .../src/taskflow_task_composer_executor.cpp | 2 + .../tesseract_task_composer_core_unit.cpp | 70 ++++++++++- .../tesseract_task_composer_planning_unit.cpp | 118 ++++++++++++++++++ 12 files changed, 391 insertions(+), 19 deletions(-) create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h create mode 100644 tesseract_task_composer/core/src/nodes/sync_task.cpp diff --git a/tesseract_task_composer/README.rst b/tesseract_task_composer/README.rst index a56ca6bc09..ab3d3fb59c 100644 --- a/tesseract_task_composer/README.rst +++ b/tesseract_task_composer/README.rst @@ -478,6 +478,18 @@ The task that is called if you want to abort everything config: conditional: false +Sync Task +^^^^^^^^^ + +The task is used to create a syncronization point within a task graph + +.. code-block:: yaml + + SyncTask: + class: SyncTaskFactory + config: + conditional: false + Remap Task ^^^^^^^^^^ diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index 9fc107488c..7fbb9d1a4a 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -43,6 +43,7 @@ add_library( src/nodes/error_task.cpp src/nodes/remap_task.cpp src/nodes/start_task.cpp + src/nodes/sync_task.cpp src/test_suite/test_task.cpp) target_link_libraries( ${PROJECT_NAME}_nodes diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h new file mode 100644 index 0000000000..e0ae1ba606 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h @@ -0,0 +1,73 @@ +/** + * @file sync_task.h + * + * @author Levi Armstrong + * @date August 13, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Plectix Robotics + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_TASK_COMPOSER_SYNC_TASK_H +#define TESSERACT_TASK_COMPOSER_SYNC_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; + +/** + * @brief This is used as a syncronization point in your task graph + * This task has no inputs or output and is not conditional + */ +class SyncTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + explicit SyncTask(std::string name = "SyncTask"); + explicit SyncTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory); + ~SyncTask() override = default; + + bool operator==(const SyncTask& rhs) const; + bool operator!=(const SyncTask& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT + + TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + OptionalTaskComposerExecutor executor = std::nullopt) const override final; +}; + +} // namespace tesseract_planning + +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::SyncTask, "SyncTask") + +#endif // TESSERACT_TASK_COMPOSER_SYNC_TASK_H diff --git a/tesseract_task_composer/core/src/nodes/sync_task.cpp b/tesseract_task_composer/core/src/nodes/sync_task.cpp new file mode 100644 index 0000000000..5f51030411 --- /dev/null +++ b/tesseract_task_composer/core/src/nodes/sync_task.cpp @@ -0,0 +1,72 @@ +/** + * @file sync_task.cpp + * + * @author Levi Armstrong + * @date August 13, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Plectix Robotics + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +SyncTask::SyncTask(std::string name) : TaskComposerTask(std::move(name), false) {} +SyncTask::SyncTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ + if (conditional_) + throw std::runtime_error("SyncTask, config is_conditional should not be true"); + + if (!input_keys_.empty()) + throw std::runtime_error("SyncTask, config does not support 'inputs' entry"); + + if (!output_keys_.empty()) + throw std::runtime_error("SyncTask, config does not support 'outputs' entry"); +} +TaskComposerNodeInfo::UPtr SyncTask::runImpl(TaskComposerInput& /*input*/, + OptionalTaskComposerExecutor /*executor*/) const +{ + auto info = std::make_unique(*this); + info->color = "green"; + info->message = "Successful"; + info->return_value = 1; + return info; +} + +bool SyncTask::operator==(const SyncTask& rhs) const { return (TaskComposerTask::operator==(rhs)); } +bool SyncTask::operator!=(const SyncTask& rhs) const { return !operator==(rhs); } + +template +void SyncTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SyncTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SyncTask) diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp index 25bf6e2f56..9828644528 100644 --- a/tesseract_task_composer/core/src/task_composer_pipeline.cpp +++ b/tesseract_task_composer/core/src/task_composer_pipeline.cpp @@ -53,6 +53,8 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec if (input.isAborted()) { auto info = std::make_unique(*this); + info->input_keys = input_keys_; + info->output_keys = output_keys_; info->return_value = 0; info->color = "white"; info->message = "Aborted"; @@ -76,6 +78,8 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec results->return_value = 0; } timer.stop(); + results->input_keys = input_keys_; + results->output_keys = output_keys_; results->elapsed_time = timer.elapsedSeconds(); int value = results->return_value; @@ -114,6 +118,8 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu { timer.stop(); auto info = std::make_unique(*this); + info->input_keys = input_keys_; + info->output_keys = output_keys_; info->return_value = static_cast(i); info->color = node_info->color; info->message = node_info->message; diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp index d5d8db317f..fb124da425 100644 --- a/tesseract_task_composer/core/src/task_composer_task.cpp +++ b/tesseract_task_composer/core/src/task_composer_task.cpp @@ -51,6 +51,8 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor if (input.isAborted()) { auto info = std::make_unique(*this); + info->input_keys = input_keys_; + info->output_keys = output_keys_; info->return_value = 0; info->color = "white"; info->message = "Aborted"; @@ -74,6 +76,8 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor results->return_value = 0; } timer.stop(); + results->input_keys = input_keys_; + results->output_keys = output_keys_; results->elapsed_time = timer.elapsedSeconds(); int value = results->return_value; 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 8ad9b83927..aa597d347f 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 @@ -34,6 +34,7 @@ #include #include #include +#include #include namespace tesseract_planning @@ -43,6 +44,7 @@ using DoneTaskFactory = TaskComposerTaskFactory; using ErrorTaskFactory = TaskComposerTaskFactory; using RemapTaskFactory = TaskComposerTaskFactory; using StartTaskFactory = TaskComposerTaskFactory; +using SyncTaskFactory = TaskComposerTaskFactory; using GraphTaskFactory = TaskComposerTaskFactory; using PipelineTaskFactory = TaskComposerTaskFactory; @@ -68,6 +70,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::RemapTaskFactory, Re // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::StartTaskFactory, StartTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::SyncTaskFactory, SyncTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::GraphTaskFactory, GraphTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::PipelineTaskFactory, PipelineTaskFactory) diff --git a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp index 34d351f9d0..5750c630e6 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -326,6 +326,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, tesseract_common::ManipulatorInfo program_manip_info = program.getManipulatorInfo().getCombined(problem.manip_info); + // Start Task auto start_task = std::make_unique(); auto start_uuid = task_graph.addNode(std::move(start_task)); @@ -352,6 +353,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, const std::string task_name = "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription(); auto raster_results = raster_task_factory_(task_name, raster_idx + 1); + raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); input.data_storage.setData(raster_results.input_key, raster_input); @@ -384,6 +386,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, const std::string task_name = "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription(); auto transition_results = transition_task_factory_(task_name, transition_idx + 1); + transition_results.node->setConditional(false); auto transition_uuid = task_graph.addNode(std::move(transition_results.node)); transition_keys.emplace_back(std::make_pair(transition_results.input_key, transition_results.output_key)); @@ -391,12 +394,15 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, const auto& next = raster_tasks[transition_idx + 1]; const auto& prev_output = prev.second.second; const auto& next_output = next.second.second; - auto transition_mux_task = std::make_unique( - "UpdateStartAndEndStateTask", prev_output, next_output, transition_results.input_key, false); - std::string transition_mux_key = transition_mux_task->getUUIDString(); + auto transition_mux_task = std::make_unique("UpdateStartAndEndStateTask", + transition_results.input_key, + prev_output, + next_output, + transition_results.output_key, + false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - input.data_storage.setData(transition_mux_key, transition_input); + input.data_storage.setData(transition_results.input_key, transition_input); task_graph.addEdges(transition_mux_uuid, { transition_uuid }); task_graph.addEdges(prev.first, { transition_mux_uuid }); @@ -409,23 +415,24 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, auto from_start_input = program[0].template as(); from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info)); - auto from_start_results = freespace_task_factory_("From Start: " + from_start_input.getDescription(), 1); + auto from_start_results = freespace_task_factory_("From Start: " + from_start_input.getDescription(), 0); auto from_start_pipeline_uuid = task_graph.addNode(std::move(from_start_results.node)); const auto& first_raster_output_key = raster_tasks[0].second.second; - auto update_end_state_task = std::make_unique( - "UpdateEndStateTask", first_raster_output_key, from_start_results.input_key, false); - std::string update_end_state_key = update_end_state_task->getUUIDString(); + auto update_end_state_task = std::make_unique("UpdateEndStateTask", + from_start_results.input_key, + first_raster_output_key, + from_start_results.output_key, + false); auto update_end_state_uuid = task_graph.addNode(std::move(update_end_state_task)); - input.data_storage.setData(update_end_state_key, from_start_input); + input.data_storage.setData(from_start_results.input_key, from_start_input); task_graph.addEdges(update_end_state_uuid, { from_start_pipeline_uuid }); task_graph.addEdges(raster_tasks[0].first, { update_end_state_uuid }); // Plan to_end - preceded by the last raster auto to_end_input = program.back().template as(); - to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info)); // Get Start Plan Instruction @@ -436,16 +443,15 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, assert(li != nullptr); to_end_input.insertMoveInstruction(to_end_input.begin(), *li); - auto to_end_results = freespace_task_factory_("To End: " + to_end_input.getDescription(), 2); + auto to_end_results = freespace_task_factory_("To End: " + to_end_input.getDescription(), program.size()); auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_results.node)); const auto& last_raster_output_key = raster_tasks.back().second.second; auto update_start_state_task = std::make_unique( - "UpdateStartStateTask", last_raster_output_key, to_end_results.input_key, false); - std::string update_start_state_key = update_start_state_task->getUUIDString(); + "UpdateStartStateTask", to_end_results.input_key, last_raster_output_key, to_end_results.output_key, false); auto update_start_state_uuid = task_graph.addNode(std::move(update_start_state_task)); - input.data_storage.setData(update_start_state_key, to_end_input); + input.data_storage.setData(to_end_results.input_key, to_end_input); task_graph.addEdges(update_start_state_uuid, { to_end_pipeline_uuid }); task_graph.addEdges(raster_tasks.back().first, { update_start_state_uuid }); diff --git a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp index cab076a89d..672e222a79 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp @@ -276,6 +276,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu tesseract_common::ManipulatorInfo program_manip_info = program.getManipulatorInfo().getCombined(problem.manip_info); + // Start Task auto start_task = std::make_unique(); auto start_uuid = task_graph.addNode(std::move(start_task)); @@ -305,6 +306,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu const std::string task_name = "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription(); auto raster_results = raster_task_factory_(task_name, raster_idx + 1); + raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); input.data_storage.setData(raster_results.input_key, raster_input); @@ -336,6 +338,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu const std::string task_name = "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription(); auto transition_results = transition_task_factory_(task_name, transition_idx + 1); + transition_results.node->setConditional(false); auto transition_uuid = task_graph.addNode(std::move(transition_results.node)); transition_keys.emplace_back(std::make_pair(transition_results.input_key, transition_results.output_key)); @@ -343,12 +346,15 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu const auto& next = raster_tasks[transition_idx + 1]; const auto& prev_output = prev.second.second; const auto& next_output = next.second.second; - auto transition_mux_task = std::make_unique( - "UpdateStartAndEndStateTask", prev_output, next_output, transition_results.input_key, false); - std::string transition_mux_key = transition_mux_task->getUUIDString(); + auto transition_mux_task = std::make_unique("UpdateStartAndEndStateTask", + transition_results.input_key, + prev_output, + next_output, + transition_results.output_key, + false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - input.data_storage.setData(transition_mux_key, transition_input); + input.data_storage.setData(transition_results.input_key, transition_input); task_graph.addEdges(transition_mux_uuid, { transition_uuid }); task_graph.addEdges(prev.first, { transition_mux_uuid }); diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp index 86fd158d8b..7cf4383a35 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -139,6 +139,8 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr // Must add a Node Info object for the graph auto info = std::make_unique(task_graph); info->color = "green"; + info->input_keys = task_graph.getInputKeys(); + info->output_keys = task_graph.getOutputKeys(); task_input.task_infos.addInfo(std::move(info)); // Generate process tasks for each node 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 c7cd62b0af..33e53841d4 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 TESSERACT_ANY_EXPORT(tesseract_common, JointState) @@ -1900,7 +1901,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT auto task = std::make_unique("abc"); // Serialization - test_suite::runSerializationPointerTest(task, "TaskComposerDoneTaskTests"); + test_suite::runSerializationPointerTest(task, "TaskComposerStartTaskTests"); } { // Test run method @@ -1918,6 +1919,73 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT } } +TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT +{ + { // Construction + SyncTask task; + EXPECT_EQ(task.getName(), "SyncTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false)"; + YAML::Node config = YAML::Load(str); + SyncTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), false); + } + + { // 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: false + inputs: [input_data] + ouputs: [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: false + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Serialization + auto task = std::make_unique("abc"); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerSyncTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + SyncTask task; + EXPECT_EQ(task.run(*input), 1); + auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "green"); + EXPECT_EQ(node_info->return_value, 1); + EXPECT_EQ(node_info->message, "Successful"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(input->isAborted(), false); + EXPECT_EQ(input->isSuccessful(), true); + EXPECT_TRUE(input->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 871128146a..0338bdd1c4 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -2564,10 +2565,69 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // TaskComposerFuture::UPtr future = executor->run(*task, *input); future->wait(); + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "TaskComposerRasterMotionTaskTests.dot"); + EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + os1.close(); + EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); EXPECT_TRUE(input->data_storage.hasKey(output_key)); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + auto info_map = input->task_infos.getInfoMap(); + EXPECT_EQ(info_map.size(), 94); + + // Make sure keys are unique + std::vector raster_input_keys; + std::vector raster_output_keys; + std::vector update_input_keys; + std::vector update_output_keys; + std::vector transition_input_keys; + std::vector transition_output_keys; + for (const auto& info : info_map) + { + std::cout << info.second->name << std::endl; + if (boost::algorithm::starts_with(info.second->name, "Raster #")) + { + auto it1 = std::find(raster_input_keys.begin(), raster_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == raster_input_keys.end()); + raster_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = std::find(raster_output_keys.begin(), raster_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == raster_output_keys.end()); + raster_output_keys.push_back(info.second->output_keys.at(0)); + } + else if (info.second->name == "UpdateStartAndEndStateTask" || info.second->name == "UpdateStartStateTask" || + info.second->name == "UpdateEndStateTask") + { + auto it1 = std::find(update_input_keys.begin(), update_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == update_input_keys.end()); + update_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = std::find(update_output_keys.begin(), update_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == update_output_keys.end()); + update_output_keys.push_back(info.second->output_keys.at(0)); + } + else if (boost::algorithm::starts_with(info.second->name, "Transition #") || + boost::algorithm::starts_with(info.second->name, "From Start") || + boost::algorithm::starts_with(info.second->name, "To End")) + { + auto it1 = std::find(transition_input_keys.begin(), transition_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == transition_input_keys.end()); + transition_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = + std::find(transition_output_keys.begin(), transition_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == transition_output_keys.end()); + transition_output_keys.push_back(info.second->output_keys.at(0)); + } + } + EXPECT_FALSE(raster_input_keys.empty()); + EXPECT_FALSE(raster_output_keys.empty()); + EXPECT_FALSE(update_input_keys.empty()); + EXPECT_FALSE(update_output_keys.empty()); + EXPECT_FALSE(transition_input_keys.empty()); + EXPECT_FALSE(transition_output_keys.empty()); } { // Failure missing input data @@ -2986,10 +3046,68 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) TaskComposerFuture::UPtr future = executor->run(*task, *input); future->wait(); + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "TaskComposerRasterOnlyMotionTaskTests.dot"); + EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + os1.close(); + EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); EXPECT_TRUE(input->data_storage.hasKey(output_key)); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + auto info_map = input->task_infos.getInfoMap(); + EXPECT_EQ(info_map.size(), 74); + + // Make sure keys are unique + std::vector raster_input_keys; + std::vector raster_output_keys; + std::vector update_input_keys; + std::vector update_output_keys; + std::vector transition_input_keys; + std::vector transition_output_keys; + for (const auto& info : info_map) + { + if (boost::algorithm::starts_with(info.second->name, "Raster #")) + { + auto it1 = std::find(raster_input_keys.begin(), raster_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == raster_input_keys.end()); + raster_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = std::find(raster_output_keys.begin(), raster_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == raster_output_keys.end()); + raster_output_keys.push_back(info.second->output_keys.at(0)); + } + else if (info.second->name == "UpdateStartAndEndStateTask" || info.second->name == "UpdateStartStateTask" || + info.second->name == "UpdateEndStateTask") + { + auto it1 = std::find(update_input_keys.begin(), update_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == update_input_keys.end()); + update_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = std::find(update_output_keys.begin(), update_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == update_output_keys.end()); + update_output_keys.push_back(info.second->output_keys.at(0)); + } + else if (boost::algorithm::starts_with(info.second->name, "Transition #") || + boost::algorithm::starts_with(info.second->name, "From Start") || + boost::algorithm::starts_with(info.second->name, "To End")) + { + auto it1 = std::find(transition_input_keys.begin(), transition_input_keys.end(), info.second->input_keys.at(0)); + ASSERT_TRUE(it1 == transition_input_keys.end()); + transition_input_keys.push_back(info.second->input_keys.at(0)); + + auto it2 = + std::find(transition_output_keys.begin(), transition_output_keys.end(), info.second->output_keys.at(0)); + ASSERT_TRUE(it2 == transition_output_keys.end()); + transition_output_keys.push_back(info.second->output_keys.at(0)); + } + } + EXPECT_FALSE(raster_input_keys.empty()); + EXPECT_FALSE(raster_output_keys.empty()); + EXPECT_FALSE(update_input_keys.empty()); + EXPECT_FALSE(update_output_keys.empty()); + EXPECT_FALSE(transition_input_keys.empty()); + EXPECT_FALSE(transition_output_keys.empty()); } { // Failure missing input data