From 5a1bcf3953676ce8d1f4a3a4dd97f81033688015 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 23 Jun 2023 16:51:43 -0500 Subject: [PATCH] Add TaskComposerPipeline and improve task composer code coverage (#337) --- tesseract_task_composer/README.rst | 33 +- .../config/task_composer_plugins.yaml | 68 +- tesseract_task_composer/core/CMakeLists.txt | 8 +- .../core/nodes/abort_task.h | 69 + .../core/task_composer_executor.h | 9 + .../core/task_composer_graph.h | 26 +- .../core/task_composer_input.h | 3 +- .../core/task_composer_node.h | 18 +- .../core/task_composer_node_info.h | 18 +- .../core/task_composer_pipeline.h | 90 + .../core/task_composer_task.h | 12 +- .../task_composer_node_info_unit.hpp | 75 + .../task_composer_serialization_utils.hpp | 36 + .../core/src/nodes/abort_task.cpp | 67 + .../core/src/nodes/done_task.cpp | 15 +- .../core/src/nodes/error_task.cpp | 12 +- .../core/src/nodes/start_task.cpp | 17 +- .../core/src/task_composer_executor.cpp | 3 + .../core/src/task_composer_graph.cpp | 142 +- .../core/src/task_composer_input.cpp | 20 +- .../core/src/task_composer_node.cpp | 85 +- .../core/src/task_composer_node_info.cpp | 25 +- .../core/src/task_composer_pipeline.cpp | 179 ++ .../core/src/task_composer_task.cpp | 115 +- .../src/task_composer_task_plugin_factory.cpp | 8 + .../examples/task_composer_example.cpp | 10 +- .../nodes/continuous_contact_check_task.h | 2 +- .../nodes/discrete_contact_check_task.h | 2 +- .../planning/nodes/fix_state_collision_task.h | 2 +- .../planning/nodes/motion_planner_task.hpp | 4 +- .../planning/nodes/motion_planner_task_info.h | 2 +- .../time_optimal_parameterization_task.h | 2 +- .../planning/src/nodes/check_input_task.cpp | 4 +- .../nodes/continuous_contact_check_task.cpp | 9 +- .../src/nodes/discrete_contact_check_task.cpp | 9 +- .../src/nodes/fix_state_bounds_task.cpp | 4 +- .../src/nodes/fix_state_collision_task.cpp | 9 +- .../src/nodes/format_as_input_task.cpp | 4 +- ...iterative_spline_parameterization_task.cpp | 4 +- .../planning/src/nodes/min_length_task.cpp | 4 +- .../src/nodes/motion_planner_task_info.cpp | 5 +- .../src/nodes/profile_switch_task.cpp | 4 +- .../planning/src/nodes/raster_motion_task.cpp | 4 +- .../src/nodes/raster_only_motion_task.cpp | 4 +- .../ruckig_trajectory_smoothing_task.cpp | 4 +- .../time_optimal_parameterization_task.cpp | 9 +- .../src/nodes/update_end_state_task.cpp | 5 +- .../nodes/update_start_and_end_state_task.cpp | 5 +- .../src/nodes/update_start_state_task.cpp | 5 +- .../src/nodes/upsample_trajectory_task.cpp | 4 +- .../taskflow_task_composer_executor.h | 10 +- .../src/taskflow_task_composer_executor.cpp | 43 +- tesseract_task_composer/test/CMakeLists.txt | 23 + .../tesseract_task_composer_core_unit.cpp | 1722 +++++++++++++++++ ...ct_task_composer_plugin_factories_unit.cpp | 49 +- 55 files changed, 2795 insertions(+), 325 deletions(-) create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_serialization_utils.hpp create mode 100644 tesseract_task_composer/core/src/nodes/abort_task.cpp create mode 100644 tesseract_task_composer/core/src/task_composer_pipeline.cpp create mode 100644 tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp diff --git a/tesseract_task_composer/README.rst b/tesseract_task_composer/README.rst index 786c702619..029497d686 100644 --- a/tesseract_task_composer/README.rst +++ b/tesseract_task_composer/README.rst @@ -83,6 +83,7 @@ This file allows you define Excutors and Tasks (aka Nodes). destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] Task Composer Executors Plugins ------------------------------- @@ -117,7 +118,7 @@ Define the graph nodes and edges as shown in the config below. .. code-block:: yaml CartesianPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: inputs: [input_data] outputs: [output_data] @@ -172,6 +173,7 @@ Define the graph nodes and edges as shown in the config below. destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] Leveraging a perviously defined task. @@ -182,7 +184,7 @@ Also in most case the tasks inputs and sometimes the outputs must be renamed. Th .. code-block:: yaml UsePreviouslyDefinedTaskPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: inputs: [input_data] outputs: [output_data] @@ -194,14 +196,17 @@ Also in most case the tasks inputs and sometimes the outputs must be renamed. Th inputs: [input_data] outputs: [output_data] CartesianPipelineTask: - task: CartesianPipeline - input_remapping: - input_data: output_data - output_remapping: - output_data: output_data + task: + name: CartesianPipeline + conditional: false + input_remapping: + input_data: output_data + output_remapping: + output_data: output_data edges: - source: MinLengthTask destinations: [CartesianPipelineTask] + terminals: [CartesianPipelineTask] Descartes Motion Planner Task ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -454,7 +459,19 @@ The final task that is called in a task graph if error occurs class: ErrorTaskFactory config: conditional: false - + +Error Task +^^^^^^^^^^ + +The task that is called if you want to abort everything + +.. code-block:: yaml + + AbortTask: + class: AbortTaskFactory + config: + conditional: false + Fix State Bounds Task ^^^^^^^^^^^^^^^^^^^^^ diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index 25cb2c456f..af901e63c2 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml +++ b/tesseract_task_composer/config/task_composer_plugins.yaml @@ -15,8 +15,9 @@ task_composer_plugins: tasks: plugins: DescartesFPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -61,9 +62,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] DescartesDPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -108,9 +111,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] DescartesFNPCPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -148,9 +153,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] DescartesDNPCPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -188,9 +195,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] OMPLPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -235,9 +244,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] TrajOptPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -282,9 +293,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] TrajOptIfoptPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -329,9 +342,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] CartesianPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -385,9 +400,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] FreespacePipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -441,9 +458,11 @@ task_composer_plugins: destinations: [ErrorTask, IterativeSplineParameterizationTask] - source: IterativeSplineParameterizationTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterFtPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -491,9 +510,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterCtPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -541,9 +562,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterFtOnlyPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -585,9 +608,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterCtOnlyPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -629,9 +654,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterFtGlobalPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -688,9 +715,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterCtGlobalPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -747,9 +776,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterFtOnlyGlobalPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -800,9 +831,11 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] RasterCtOnlyGlobalPipeline: - class: GraphTaskFactory + class: PipelineTaskFactory config: + conditional: true inputs: [input_data] outputs: [output_data] nodes: @@ -853,3 +886,4 @@ task_composer_plugins: destinations: [ErrorTask, RasterMotionTask] - source: RasterMotionTask destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index 981b11ab9e..f9840b39f3 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( src/task_composer_input.cpp src/task_composer_node.cpp src/task_composer_node_info.cpp + src/task_composer_pipeline.cpp src/task_composer_plugin_factory.cpp src/task_composer_problem.cpp src/task_composer_server.cpp @@ -36,7 +37,12 @@ target_code_coverage( target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -add_library(${PROJECT_NAME}_nodes src/nodes/done_task.cpp src/nodes/error_task.cpp src/nodes/start_task.cpp) +add_library( + ${PROJECT_NAME}_nodes + src/nodes/abort_task.cpp + src/nodes/done_task.cpp + src/nodes/error_task.cpp + src/nodes/start_task.cpp) target_link_libraries( ${PROJECT_NAME}_nodes PUBLIC ${PROJECT_NAME} diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h new file mode 100644 index 0000000000..c45cc35707 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h @@ -0,0 +1,69 @@ +/** + * @file abort_task.h + * + * @author Levi Armstrong + * @date June 22, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Levi Armstrong + * + * @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_ABORT_TASK_H +#define TESSERACT_TASK_COMPOSER_ABORT_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; +class AbortTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + AbortTask(); + explicit AbortTask(std::string name, bool conditional); + explicit AbortTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory); + ~AbortTask() override = default; + + bool operator==(const AbortTask& rhs) const; + bool operator!=(const AbortTask& 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::AbortTask, "AbortTask") + +#endif // TESSERACT_TASK_COMPOSER_ABORT_TASK_H diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h index 97646f8e12..2dd1a4459f 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include #include @@ -69,6 +70,14 @@ class TaskComposerExecutor */ virtual TaskComposerFuture::UPtr run(const TaskComposerGraph& task_graph, TaskComposerInput& task_input) = 0; + /** + * @brief Execute the provided task pipeline + * @param task_pipeline The task pipeline to execute + * @param task_input The task input provided to every task + * @return The future associated with execution + */ + virtual TaskComposerFuture::UPtr run(const TaskComposerPipeline& task_pipeline, TaskComposerInput& task_input) = 0; + /** * @brief Execute the provided task * @param task_graph The task to execute diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h index e0cdc7c1e0..9bb57e759a 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_graph.h @@ -77,9 +77,25 @@ class TaskComposerGraph : public TaskComposerNode */ void addEdges(boost::uuids::uuid source, std::vector destinations); - /** @brief Get the nodes associated with the pipeline */ + /** @brief Get the nodes associated with the pipeline mapped by uuid */ std::map getNodes() const; + /** + * @brief Get a node by name + * @param name The name of the node to search for + * @return The node with the name, otherwise nullptr + */ + TaskComposerNode::ConstPtr getNodeByName(const std::string& name) const; + + /** + * @brief Set the terminals nodes + * @details must be called after all nodes and edges are setup. + */ + void setTerminals(std::vector terminals); + + /** @brief Get the terminal nodes for the pipeline */ + std::vector getTerminals() const; + void renameInputKeys(const std::map& input_keys) override; void renameOutputKeys(const std::map& output_keys) override; @@ -95,10 +111,18 @@ class TaskComposerGraph : public TaskComposerNode friend struct tesseract_common::Serialization; friend class boost::serialization::access; + // These are protected and used by PIPELINE + TaskComposerGraph(std::string name, TaskComposerNodeType type, bool conditional); + TaskComposerGraph(std::string name, + TaskComposerNodeType type, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory); + template void serialize(Archive& ar, const unsigned int version); // NOLINT std::map nodes_; + std::vector terminals_; }; } // namespace tesseract_planning diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h index 52837c3be0..0e4036d4e8 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h @@ -52,6 +52,7 @@ struct TaskComposerInput TaskComposerInput(TaskComposerProblem::UPtr problem); TaskComposerInput(const TaskComposerInput&) = delete; + TaskComposerInput(TaskComposerInput&&) noexcept = delete; TaskComposerInput& operator=(const TaskComposerInput&) = delete; TaskComposerInput& operator=(TaskComposerInput&&) = delete; virtual ~TaskComposerInput() = default; @@ -109,7 +110,7 @@ struct TaskComposerInput friend class boost::serialization::access; TaskComposerInput() = default; // Required for serialization - TaskComposerInput(TaskComposerInput&&) noexcept; + // TaskComposerInput(TaskComposerInput&&) noexcept; template void serialize(Archive& ar, const unsigned int version); // NOLINT 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 161c28a6ed..93f43ee851 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 @@ -41,6 +41,7 @@ namespace tesseract_planning enum class TaskComposerNodeType { TASK, + PIPELINE, GRAPH }; @@ -53,7 +54,10 @@ class TaskComposerNode using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - TaskComposerNode(std::string name = "TaskComposerNode", TaskComposerNodeType type = TaskComposerNodeType::TASK); + TaskComposerNode(std::string name = "TaskComposerNode", + TaskComposerNodeType type = TaskComposerNodeType::TASK, + bool conditional = false); + explicit TaskComposerNode(std::string name, TaskComposerNodeType type, const YAML::Node& config); virtual ~TaskComposerNode() = default; TaskComposerNode(const TaskComposerNode&) = delete; TaskComposerNode& operator=(const TaskComposerNode&) = delete; @@ -81,6 +85,12 @@ class TaskComposerNode */ const boost::uuids::uuid& getParentUUID() const; + /** + * @brief Check if node is conditional + * @return + */ + bool isConditional() const; + /** @brief IDs of nodes (i.e. node) that should run after this node */ const std::vector& getOutboundEdges() const; @@ -105,6 +115,9 @@ class TaskComposerNode /** @brief Rename output keys */ virtual void renameOutputKeys(const std::map& output_keys); + /** @brief Set if conditional */ + virtual void setConditional(bool enable); + /** * @brief dump the task to dot * @brief Return additional subgraphs which should get appended if needed @@ -155,6 +168,9 @@ class TaskComposerNode /** @brief The nodes output keys */ std::vector output_keys_; + /** @brief Indicate if node is conditional */ + bool conditional_{ false }; + /** @brief This will create a UUID string with no hyphens used when creating dot graph */ static std::string toString(const boost::uuids::uuid& u, const std::string& prefix = ""); }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h index 4578ae27dd..d681821d51 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h @@ -39,7 +39,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { class TaskComposerNode; -struct TaskComposerInput; /** Stores information about a node */ class TaskComposerNodeInfo @@ -51,7 +50,7 @@ class TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; TaskComposerNodeInfo() = default; // Required for serialization - TaskComposerNodeInfo(const TaskComposerNode& node, const TaskComposerInput& input); + TaskComposerNodeInfo(const TaskComposerNode& node); virtual ~TaskComposerNodeInfo() = default; TaskComposerNodeInfo(const TaskComposerNodeInfo&) = default; TaskComposerNodeInfo& operator=(const TaskComposerNodeInfo&) = default; @@ -121,6 +120,8 @@ class TaskComposerNodeInfo private: friend struct tesseract_common::Serialization; friend class boost::serialization::access; + friend class TaskComposerTask; + friend class TaskComposerPipeline; /** @brief Indicate if task was not ran because input abort flag was enabled */ bool aborted_{ false }; @@ -150,6 +151,13 @@ struct TaskComposerNodeInfoContainer */ void addInfo(TaskComposerNodeInfo::UPtr info); + /** + * @brief Get info for the provided key + * @param key The key to retrieve info for + * @return If key does not exist nullptr, otherwise a clone of the info + */ + TaskComposerNodeInfo::UPtr getInfo(const boost::uuids::uuid& key) const; + /** @brief Get a copy of the task_info_map_ in case it gets resized*/ std::map getInfoMap() const; @@ -159,6 +167,12 @@ struct TaskComposerNodeInfoContainer */ void setAborted(const boost::uuids::uuid& node_uuid); + /** + * @brief Get the aborting node + * @return Null if not set, otherwise nodes uuid + */ + boost::uuids::uuid getAbortingNode() const; + /** @brief Clear the contents */ void clear(); diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h new file mode 100644 index 0000000000..64a8abd747 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h @@ -0,0 +1,90 @@ +/** + * @file task_composer_pipeline.h + * @brief A node in the pipeline + * + * @author Levi Armstrong + * @date July 29. 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @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_TASK_COMPOSER_PIPELINE_H +#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_PIPELINE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerExecutor; +class TaskComposerPluginFactory; +/** + * @brief This class facilitates the composition of an arbitrary taskflow pipeline. + * Tasks are nodes in the graph connected to each other in a configurable order by directed edges + */ +class TaskComposerPipeline : public TaskComposerGraph +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + /** @brief Most task will not require a executor so making it optional */ + using OptionalTaskComposerExecutor = std::optional>; + + TaskComposerPipeline(std::string name = "TaskComposerPipeline"); + TaskComposerPipeline(std::string name, bool conditional); + TaskComposerPipeline(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory); + ~TaskComposerPipeline() override = default; + TaskComposerPipeline(const TaskComposerPipeline&) = delete; + TaskComposerPipeline& operator=(const TaskComposerPipeline&) = delete; + TaskComposerPipeline(TaskComposerPipeline&&) = delete; + TaskComposerPipeline& operator=(TaskComposerPipeline&&) = delete; + + int run(TaskComposerInput& input, OptionalTaskComposerExecutor executor = std::nullopt) const; + + bool operator==(const TaskComposerPipeline& rhs) const; + bool operator!=(const TaskComposerPipeline& 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; + + void runRecursive(const TaskComposerNode& node, + TaskComposerInput& input, + OptionalTaskComposerExecutor executor = std::nullopt) const; +}; + +} // namespace tesseract_planning + +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::TaskComposerPipeline, "TaskComposerPipeline") + +#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_PIPELINE_H diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_task.h index 7ae25fc718..dd8b33e98f 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_task.h @@ -50,8 +50,8 @@ class TaskComposerTask : public TaskComposerNode /** @brief Most task will not require a executor so making it optional */ using OptionalTaskComposerExecutor = std::optional>; - TaskComposerTask(); - explicit TaskComposerTask(std::string name, bool is_conditional); + explicit TaskComposerTask(std::string name = "TaskComposerTask"); + explicit TaskComposerTask(std::string name, bool conditional); explicit TaskComposerTask(std::string name, const YAML::Node& config); ~TaskComposerTask() override = default; TaskComposerTask(const TaskComposerTask&) = delete; @@ -64,12 +64,6 @@ class TaskComposerTask : public TaskComposerNode int run(TaskComposerInput& input, OptionalTaskComposerExecutor executor = std::nullopt) const; - bool isConditional() const; - - std::string dump(std::ostream& os, - const TaskComposerNode* parent = nullptr, - const std::map& results_map = {}) const override; - protected: friend struct tesseract_common::Serialization; friend class boost::serialization::access; @@ -77,8 +71,6 @@ class TaskComposerTask : public TaskComposerNode template void serialize(Archive& ar, const unsigned int version); // NOLINT - bool is_conditional_{ true }; - virtual TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor executor = std::nullopt) const = 0; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp new file mode 100644 index 0000000000..f73187b4b1 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp @@ -0,0 +1,75 @@ +/** + * @file task_composer_node_info_unit.hpp + * @brief Collection of unit tests for TaskComposerNodeInfo + * + * @author Levi Armstrong + * @date June 12, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Levi Armstrong + * + * @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_TASK_COMPOSER_NODE_INFO_UNIT_HPP +#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_NODE_INFO_UNIT_HPP + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace tesseract_planning::test_suite +{ +template +void runTaskComposerNodeInfoTest() +{ + { // Default + T node_info; + EXPECT_EQ(node_info.return_value, -1); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(node_info.elapsed_time, 0)); + EXPECT_TRUE(node_info.uuid.is_nil()); + EXPECT_TRUE(node_info.parent_uuid.is_nil()); + EXPECT_EQ(node_info.color, "red"); + EXPECT_FALSE(node_info.isAborted()); + EXPECT_EQ(node_info, *(node_info.clone())); + + // Serialization + test_suite::runSerializationTest(node_info, "TaskComposerNodeInfoTests"); + } + + { // Constructor + TaskComposerNode node; + T node_info(node); + EXPECT_EQ(node_info.return_value, -1); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(node_info.elapsed_time, 0)); + EXPECT_FALSE(node_info.uuid.is_nil()); + EXPECT_EQ(node_info.uuid, node.getUUID()); + EXPECT_EQ(node_info.parent_uuid, node.getParentUUID()); + EXPECT_EQ(node_info.color, "red"); + EXPECT_FALSE(node_info.isAborted()); + EXPECT_EQ(node_info, *(node_info.clone())); + + // Serialization + test_suite::runSerializationTest(node_info, "TaskComposerNodeInfoTests"); + } +} +} // namespace tesseract_planning::test_suite +#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_NODE_INFO_UNIT_HPP diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_serialization_utils.hpp b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_serialization_utils.hpp new file mode 100644 index 0000000000..69862522ce --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_serialization_utils.hpp @@ -0,0 +1,36 @@ +#ifndef TESSERACT_TASK_COMPOSER_SERIALIZATION_UTILS_HPP +#define TESSERACT_TASK_COMPOSER_SERIALIZATION_UTILS_HPP + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning::test_suite +{ +template +inline void runSerializationTest(const T& input, const std::string& file_name) +{ + const std::string filepath = tesseract_common::getTempPath() + file_name + ".xml"; + tesseract_common::Serialization::toArchiveFileXML(input, filepath); + auto ninput = tesseract_common::Serialization::fromArchiveFileXML(filepath); + EXPECT_FALSE(input != ninput); +} + +template +inline void runSerializationPointerTest(const T& input, const std::string& file_name) +{ + const std::string filepath = tesseract_common::getTempPath() + file_name + ".xml"; + tesseract_common::Serialization::toArchiveFileXML(input, filepath); + auto ninput = tesseract_common::Serialization::fromArchiveFileXML(filepath); + EXPECT_FALSE(*input != *ninput); +} +} // namespace tesseract_planning::test_suite +#endif // TESSERACT_TASK_COMPOSER_SERIALIZATION_UTILS_HPP diff --git a/tesseract_task_composer/core/src/nodes/abort_task.cpp b/tesseract_task_composer/core/src/nodes/abort_task.cpp new file mode 100644 index 0000000000..2f6b384eea --- /dev/null +++ b/tesseract_task_composer/core/src/nodes/abort_task.cpp @@ -0,0 +1,67 @@ +/** + * @file abort_task.cpp + * + * @author Levi Armstrong + * @date June 22, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Levi Armstrong + * + * @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 + +#include + +namespace tesseract_planning +{ +AbortTask::AbortTask() : TaskComposerTask("AbortTask", false) {} +AbortTask::AbortTask(std::string name, bool is_conditional) : TaskComposerTask(std::move(name), is_conditional) {} +AbortTask::AbortTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ +} + +TaskComposerNodeInfo::UPtr AbortTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +{ + auto info = std::make_unique(*this); + info->color = "red"; + info->return_value = 0; + info->message = "Aborted"; + input.abort(uuid_); + CONSOLE_BRIDGE_logDebug("%s", info->message.c_str()); + return info; +} + +bool AbortTask::operator==(const AbortTask& rhs) const { return (TaskComposerTask::operator==(rhs)); } +bool AbortTask::operator!=(const AbortTask& rhs) const { return !operator==(rhs); } + +template +void AbortTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::AbortTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::AbortTask) diff --git a/tesseract_task_composer/core/src/nodes/done_task.cpp b/tesseract_task_composer/core/src/nodes/done_task.cpp index 31e0960bc5..48c22fc914 100644 --- a/tesseract_task_composer/core/src/nodes/done_task.cpp +++ b/tesseract_task_composer/core/src/nodes/done_task.cpp @@ -41,12 +41,10 @@ DoneTask::DoneTask(std::string name, const YAML::Node& config, const TaskCompose { } -TaskComposerNodeInfo::UPtr DoneTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +TaskComposerNodeInfo::UPtr DoneTask::runImpl(TaskComposerInput& /*input*/, + OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->color = "green"; info->return_value = 1; info->message = "Successful"; @@ -54,12 +52,7 @@ TaskComposerNodeInfo::UPtr DoneTask::runImpl(TaskComposerInput& input, OptionalT return info; } -bool DoneTask::operator==(const DoneTask& rhs) const -{ - bool equal = true; - equal &= TaskComposerTask::operator==(rhs); - return equal; -} +bool DoneTask::operator==(const DoneTask& rhs) const { return TaskComposerTask::operator==(rhs); } bool DoneTask::operator!=(const DoneTask& rhs) const { return !operator==(rhs); } template diff --git a/tesseract_task_composer/core/src/nodes/error_task.cpp b/tesseract_task_composer/core/src/nodes/error_task.cpp index 9309279cb1..dddf1d8744 100644 --- a/tesseract_task_composer/core/src/nodes/error_task.cpp +++ b/tesseract_task_composer/core/src/nodes/error_task.cpp @@ -43,10 +43,7 @@ ErrorTask::ErrorTask(std::string name, const YAML::Node& config, const TaskCompo TaskComposerNodeInfo::UPtr ErrorTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->color = "red"; info->return_value = 0; input.abort(uuid_); @@ -55,12 +52,7 @@ TaskComposerNodeInfo::UPtr ErrorTask::runImpl(TaskComposerInput& input, Optional return info; } -bool ErrorTask::operator==(const ErrorTask& rhs) const -{ - bool equal = true; - equal &= TaskComposerTask::operator==(rhs); - return equal; -} +bool ErrorTask::operator==(const ErrorTask& rhs) const { return (TaskComposerTask::operator==(rhs)); } bool ErrorTask::operator!=(const ErrorTask& rhs) const { return !operator==(rhs); } template diff --git a/tesseract_task_composer/core/src/nodes/start_task.cpp b/tesseract_task_composer/core/src/nodes/start_task.cpp index 163bf3b2b1..f3d5ff51b7 100644 --- a/tesseract_task_composer/core/src/nodes/start_task.cpp +++ b/tesseract_task_composer/core/src/nodes/start_task.cpp @@ -38,7 +38,7 @@ StartTask::StartTask(std::string name) : TaskComposerTask(std::move(name), false StartTask::StartTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) : TaskComposerTask(std::move(name), config) { - if (is_conditional_) + if (conditional_) throw std::runtime_error("StartTask, config is_conditional should not be true"); if (!input_keys_.empty()) @@ -47,24 +47,17 @@ StartTask::StartTask(std::string name, const YAML::Node& config, const TaskCompo if (!output_keys_.empty()) throw std::runtime_error("StartTask, config does not support 'outputs' entry"); } -TaskComposerNodeInfo::UPtr StartTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +TaskComposerNodeInfo::UPtr StartTask::runImpl(TaskComposerInput& /*input*/, + OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->color = "green"; info->message = "Successful"; info->return_value = 1; return info; } -bool StartTask::operator==(const StartTask& rhs) const -{ - bool equal = true; - equal &= TaskComposerTask::operator==(rhs); - return equal; -} +bool StartTask::operator==(const StartTask& rhs) const { return (TaskComposerTask::operator==(rhs)); } bool StartTask::operator!=(const StartTask& rhs) const { return !operator==(rhs); } template diff --git a/tesseract_task_composer/core/src/task_composer_executor.cpp b/tesseract_task_composer/core/src/task_composer_executor.cpp index a5b95a7bb4..574c8640b0 100644 --- a/tesseract_task_composer/core/src/task_composer_executor.cpp +++ b/tesseract_task_composer/core/src/task_composer_executor.cpp @@ -42,6 +42,9 @@ TaskComposerFuture::UPtr TaskComposerExecutor::run(const TaskComposerNode& node, if (node.getType() == TaskComposerNodeType::TASK) return run(static_cast(node), task_input); + if (node.getType() == TaskComposerNodeType::PIPELINE) + return run(static_cast(node), task_input); + if (node.getType() == TaskComposerNodeType::GRAPH) return run(static_cast(node), task_input); diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index 8b416daa5a..c0e48b35a6 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -28,6 +28,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include #include #include @@ -38,21 +39,31 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerGraph::TaskComposerGraph(std::string name) : TaskComposerNode(std::move(name), TaskComposerNodeType::GRAPH) +TaskComposerGraph::TaskComposerGraph(std::string name) + : TaskComposerGraph(std::move(name), TaskComposerNodeType::GRAPH, false) { } + +TaskComposerGraph::TaskComposerGraph(std::string name, TaskComposerNodeType type, bool conditional) + : TaskComposerNode(std::move(name), type, conditional) +{ +} + TaskComposerGraph::TaskComposerGraph(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory) - : TaskComposerGraph(std::move(name)) + : TaskComposerGraph(std::move(name), TaskComposerNodeType::GRAPH, config, plugin_factory) { - // Get input keys - if (YAML::Node n = config["inputs"]) - input_keys_ = n.as>(); - - if (YAML::Node n = config["outputs"]) - output_keys_ = n.as>(); + if (conditional_) + throw std::runtime_error("TaskComposerGraph, conditional should not be true"); +} +TaskComposerGraph::TaskComposerGraph(std::string name, + TaskComposerNodeType type, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory) + : TaskComposerNode(std::move(name), type, config) +{ std::unordered_map node_uuids; YAML::Node nodes = config["nodes"]; if (!nodes.IsMap()) @@ -76,15 +87,11 @@ TaskComposerGraph::TaskComposerGraph(std::string name, } else if (YAML::Node tn = node_it->second["task"]) { - std::map input_remapping; - std::map output_remapping; - - auto task_name = tn.as(); - if (YAML::Node n = tn["input_remapping"]) - input_remapping = n.as>(); - - if (YAML::Node n = tn["output_remapping"]) - output_remapping = n.as>(); + std::string task_name; + if (YAML::Node n = tn["name"]) + task_name = n.as(); + else + throw std::runtime_error("Task Composer Graph '" + name_ + "' missing 'name' entry!"); TaskComposerNode::UPtr task_node = plugin_factory.createTaskComposerNode(task_name); if (task_node == nullptr) @@ -92,11 +99,15 @@ TaskComposerGraph::TaskComposerGraph(std::string name, "' for node '" += node_name + "'"); task_node->setName(node_name); - if (!input_remapping.empty()) - task_node->renameInputKeys(input_remapping); - if (!output_remapping.empty()) - task_node->renameOutputKeys(output_remapping); + if (YAML::Node n = tn["conditional"]) + task_node->setConditional(n.as()); + + if (YAML::Node n = tn["input_remapping"]) + task_node->renameInputKeys(n.as>()); + + if (YAML::Node n = tn["output_remapping"]) + task_node->renameOutputKeys(n.as>()); node_uuids[node_name] = addNode(std::move(task_node)); } @@ -145,6 +156,25 @@ TaskComposerGraph::TaskComposerGraph(std::string name, addEdges(source_it->second, destination_uuids); } + + if (YAML::Node n = config["terminals"]) + { + auto terminals = n.as>(); + terminals_.clear(); + terminals_.reserve(terminals.size()); + for (const auto& terminal : terminals) + { + auto terminal_it = node_uuids.find(terminal); + if (terminal_it == node_uuids.end()) + throw std::runtime_error("Task Composer Graph '" + name_ + "' failed to find terminal '" + terminal + "'"); + + terminals_.push_back(terminal_it->second); + } + } + else + { + throw std::runtime_error("Task Composer Graph '" + name_ + "' is missing 'terminals' entry"); + } } boost::uuids::uuid TaskComposerGraph::addNode(TaskComposerNode::UPtr task_node) @@ -169,6 +199,34 @@ std::map TaskComposerGraph::getN return std::map{ nodes_.begin(), nodes_.end() }; } +TaskComposerNode::ConstPtr TaskComposerGraph::getNodeByName(const std::string& name) const +{ + for (const auto& pair : nodes_) + { + if (pair.second->getName() == name) + return pair.second; + } + + return nullptr; +} + +void TaskComposerGraph::setTerminals(std::vector terminals) +{ + for (const auto& terminal : terminals) + { + auto it = nodes_.find(terminal); + if (it == nodes_.end()) + throw std::runtime_error("TaskComposerGraph, terminal node does not exist!"); + + if (!it->second->getOutboundEdges().empty()) + throw std::runtime_error("TaskComposerGraph, terminal node has outbound edges!"); + } + + terminals_ = std::move(terminals); +} + +std::vector TaskComposerGraph::getTerminals() const { return terminals_; } + void TaskComposerGraph::renameInputKeys(const std::map& input_keys) { for (const auto& key : input_keys) @@ -204,10 +262,10 @@ std::string TaskComposerGraph::dump(std::ostream& os, { sub_graphs << node->dump(os, this, results_map); } - else if (node->getType() == TaskComposerNodeType::GRAPH) + else if (node->getType() == TaskComposerNodeType::GRAPH || node->getType() == TaskComposerNodeType::PIPELINE) { auto it = results_map.find(node->getUUID()); - std::string color = (it == results_map.end()) ? "blue" : it->second->color; + std::string color = (it != results_map.end() && it->second->color != "white") ? it->second->color : "blue"; const std::string tmp = toString(node->uuid_, "node_"); os << std::endl << tmp << " [shape=box3d, label=\"Subgraph: " << node->name_ << "\\n(" << node->uuid_str_ @@ -216,9 +274,39 @@ std::string TaskComposerGraph::dump(std::ostream& os, } } - for (const auto& edge : outbound_edges_) + if (type_ == TaskComposerNodeType::PIPELINE) + { + const auto& pipeline = static_cast(*this); + if (pipeline.isConditional()) + { + int return_value = -1; + + auto it = results_map.find(uuid_); + if (it != results_map.end()) + return_value = it->second->return_value; + + for (std::size_t i = 0; i < outbound_edges_.size(); ++i) + { + std::string line_type = (return_value == static_cast(i)) ? "bold" : "dashed"; + os << "node_" << tmp << " -> " << toString(outbound_edges_[i], "node_") << " [style=" << line_type + << ", label=\"[" << std::to_string(i) << "]\"" + << "];\n"; + } + } + else + { + for (const auto& edge : outbound_edges_) + { + os << "node_" << tmp << " -> " << toString(edge, "node_") << ";\n"; + } + } + } + else { - os << "node_" << tmp << " -> " << toString(edge, "node_") << ";\n"; + for (const auto& edge : outbound_edges_) + { + os << "node_" << tmp << " -> " << toString(edge, "node_") << ";\n"; + } } os << "}\n"; @@ -242,11 +330,12 @@ bool TaskComposerGraph::operator==(const TaskComposerGraph& rhs) const for (const auto& pair : nodes_) { auto it = rhs.nodes_.find(pair.first); - equal &= (it == rhs.nodes_.end()); + equal &= (it != rhs.nodes_.end()); if (equal) equal &= (*(pair.second) == *(it->second)); } } + equal &= (terminals_ == rhs.terminals_); equal &= TaskComposerNode::operator==(rhs); return equal; } @@ -256,6 +345,7 @@ template void TaskComposerGraph::serialize(Archive& ar, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("nodes", nodes_); + ar& boost::serialization::make_nvp("terminals", terminals_); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerNode); } diff --git a/tesseract_task_composer/core/src/task_composer_input.cpp b/tesseract_task_composer/core/src/task_composer_input.cpp index a6c20ff7f7..3dc27a3e17 100644 --- a/tesseract_task_composer/core/src/task_composer_input.cpp +++ b/tesseract_task_composer/core/src/task_composer_input.cpp @@ -66,7 +66,11 @@ void TaskComposerInput::reset() bool TaskComposerInput::operator==(const TaskComposerInput& rhs) const { bool equal = true; - equal &= problem == rhs.problem; + if (problem != nullptr && rhs.problem != nullptr) + equal &= (*problem == *rhs.problem); + else + equal &= (problem == nullptr && rhs.problem == nullptr); + equal &= data_storage == rhs.data_storage; equal &= task_infos == rhs.task_infos; equal &= aborted_ == rhs.aborted_; @@ -75,13 +79,13 @@ bool TaskComposerInput::operator==(const TaskComposerInput& rhs) const bool TaskComposerInput::operator!=(const TaskComposerInput& rhs) const { return !operator==(rhs); } -TaskComposerInput::TaskComposerInput(TaskComposerInput&& rhs) noexcept - : problem(std::move(rhs.problem)) - , data_storage(std::move(rhs.data_storage)) - , task_infos(std::move(rhs.task_infos)) - , aborted_(rhs.aborted_.load()) -{ -} +// TaskComposerInput::TaskComposerInput(TaskComposerInput&& rhs) noexcept +// : problem(std::move(rhs.problem)) +// , data_storage(std::move(rhs.data_storage)) +// , task_infos(std::move(rhs.task_infos)) +// , aborted_(rhs.aborted_.load()) +//{ +//} template void TaskComposerInput::serialize(Archive& ar, const unsigned int /*version*/) diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 5392062b90..b410767361 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -37,13 +37,45 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type) +TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, bool conditional) : name_(std::move(name)) , type_(type) , uuid_(boost::uuids::random_generator()()) , uuid_str_(boost::uuids::to_string(uuid_)) + , conditional_(conditional) { } + +TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, const YAML::Node& config) + : TaskComposerNode::TaskComposerNode(std::move(name), type) +{ + try + { + if (YAML::Node n = config["conditional"]) + conditional_ = n.as(); + + if (YAML::Node n = config["inputs"]) + { + if (n.IsSequence()) + input_keys_ = n.as>(); + else + input_keys_ = { n.as() }; + } + + if (YAML::Node n = config["outputs"]) + { + if (n.IsSequence()) + output_keys_ = n.as>(); + else + output_keys_ = { n.as() }; + } + } + catch (const std::exception& e) + { + throw std::runtime_error("TaskComposerNode: Failed to parse yaml config data! Details: " + std::string(e.what())); + } +} + void TaskComposerNode::setName(const std::string& name) { name_ = name; } const std::string& TaskComposerNode::getName() const { return name_; } @@ -55,6 +87,8 @@ const std::string& TaskComposerNode::getUUIDString() const { return uuid_str_; } const boost::uuids::uuid& TaskComposerNode::getParentUUID() const { return parent_uuid_; } +bool TaskComposerNode::isConditional() const { return conditional_; } + const std::vector& TaskComposerNode::getOutboundEdges() const { return outbound_edges_; } const std::vector& TaskComposerNode::getInboundEdges() const { return inbound_edges_; } @@ -79,6 +113,8 @@ void TaskComposerNode::renameOutputKeys(const std::map std::replace(output_keys_.begin(), output_keys_.end(), key.first, key.second); } +void TaskComposerNode::setConditional(bool enable) { conditional_ = enable; } + std::string TaskComposerNode::dump(std::ostream& os, const TaskComposerNode* /*parent*/, const std::map& results_map) const @@ -86,20 +122,47 @@ std::string TaskComposerNode::dump(std::ostream& os, const std::string tmp = toString(uuid_, "node_"); std::string color{ "white" }; - auto it = results_map.find(uuid_); - if (it != results_map.end() && !it->second->isAborted()) - color = it->second->color; + int return_value = -1; - os << std::endl << tmp << " [label=\"" << name_ << "\\n(" << uuid_str_ << ")"; + auto it = results_map.find(uuid_); if (it != results_map.end()) { - os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s" - << "\\n\"" << it->second->message << "\""; + return_value = it->second->return_value; + if (!it->second->isAborted()) + color = it->second->color; } - os << "\", fillcolor=" << color << ", style=filled];\n"; - for (const auto& edge : outbound_edges_) - os << tmp << " -> " << toString(edge, "node_") << ";\n"; + if (conditional_) + { + os << std::endl << tmp << " [shape=diamond, label=\"" << name_ << "\\n(" << uuid_str_ << ")"; + if (it != results_map.end()) + { + os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s" + << "\\n`" << it->second->message << "`"; + } + os << "\", color=black, fillcolor=" << color << ", style=filled];\n"; + + for (std::size_t i = 0; i < outbound_edges_.size(); ++i) + { + std::string line_type = (return_value == static_cast(i)) ? "bold" : "dashed"; + os << tmp << " -> " << toString(outbound_edges_[i], "node_") << " [style=" << line_type << ", label=\"[" + << std::to_string(i) << "]\"" + << "];\n"; + } + } + else + { + os << std::endl << tmp << " [label=\"" << name_ << "\\n(" << uuid_str_ << ")"; + if (it != results_map.end()) + { + os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s" + << "\\n'" << it->second->message << "'"; + } + os << "\", color=black, fillcolor=" << color << ", style=filled];\n"; + + for (const auto& edge : outbound_edges_) + os << tmp << " -> " << toString(edge, "node_") << ";\n"; + } if (it == results_map.end()) return {}; @@ -119,6 +182,7 @@ bool TaskComposerNode::operator==(const TaskComposerNode& rhs) const equal &= inbound_edges_ == rhs.inbound_edges_; equal &= input_keys_ == rhs.input_keys_; equal &= output_keys_ == rhs.output_keys_; + equal &= conditional_ == rhs.conditional_; return equal; } bool TaskComposerNode::operator!=(const TaskComposerNode& rhs) const { return !operator==(rhs); } @@ -135,6 +199,7 @@ void TaskComposerNode::serialize(Archive& ar, const unsigned int /*version*/) ar& boost::serialization::make_nvp("inbound_edges", inbound_edges_); ar& boost::serialization::make_nvp("input_keys", input_keys_); ar& boost::serialization::make_nvp("output_keys", output_keys_); + ar& boost::serialization::make_nvp("conditional", conditional_); } std::string TaskComposerNode::toString(const boost::uuids::uuid& u, const std::string& prefix) diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp index 5b96ea6493..7b1e0489d6 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -41,20 +41,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerNodeInfo::TaskComposerNodeInfo(const TaskComposerNode& node, const TaskComposerInput& input) +TaskComposerNodeInfo::TaskComposerNodeInfo(const TaskComposerNode& node) : name(node.name_) , uuid(node.uuid_) , parent_uuid(node.parent_uuid_) , inbound_edges(node.inbound_edges_) , outbound_edges(node.outbound_edges_) - , aborted_(input.isAborted()) { - if (aborted_) - { - return_value = 0; - color = "white"; - message = "Aborted"; - } } bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const @@ -149,6 +142,16 @@ void TaskComposerNodeInfoContainer::addInfo(TaskComposerNodeInfo::UPtr info) info_map_[info->uuid] = std::move(info); } +TaskComposerNodeInfo::UPtr TaskComposerNodeInfoContainer::getInfo(const boost::uuids::uuid& key) const +{ + std::unique_lock lock(mutex_); + auto it = info_map_.find(key); + if (it == info_map_.end()) + return nullptr; + + return it->second->clone(); +} + void TaskComposerNodeInfoContainer::setAborted(const boost::uuids::uuid& node_uuid) { assert(!node_uuid.is_nil()); @@ -156,6 +159,12 @@ void TaskComposerNodeInfoContainer::setAborted(const boost::uuids::uuid& node_uu aborting_node_ = node_uuid; } +boost::uuids::uuid TaskComposerNodeInfoContainer::getAbortingNode() const +{ + std::unique_lock lock(mutex_); + return aborting_node_; +} + void TaskComposerNodeInfoContainer::clear() { std::unique_lock lock(mutex_); diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp new file mode 100644 index 0000000000..eb744732fb --- /dev/null +++ b/tesseract_task_composer/core/src/task_composer_pipeline.cpp @@ -0,0 +1,179 @@ +/** + * @file task_composer_pipeline.cpp + * @brief A task pipeline + * + * @author Levi Armstrong + * @date July 29. 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @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 +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_planning +{ +TaskComposerPipeline::TaskComposerPipeline(std::string name) : TaskComposerPipeline(std::move(name), true) {} +TaskComposerPipeline::TaskComposerPipeline(std::string name, bool conditional) + : TaskComposerGraph(std::move(name), TaskComposerNodeType::PIPELINE, conditional) +{ +} +TaskComposerPipeline::TaskComposerPipeline(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory) + : TaskComposerGraph(std::move(name), TaskComposerNodeType::PIPELINE, config, plugin_factory) +{ +} + +int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const +{ + if (input.isAborted()) + { + auto info = std::make_unique(*this); + info->return_value = 0; + info->color = "white"; + info->message = "Aborted"; + info->aborted_ = true; + input.task_infos.addInfo(std::move(info)); + return 0; + } + + TaskComposerNodeInfo::UPtr results; + try + { + results = runImpl(input, executor); + } + catch (const std::exception& e) + { + results = std::make_unique(*this); + results->color = "red"; + results->message = "Exception thrown: " + std::string(e.what()); + results->return_value = 0; + } + + int value = results->return_value; + assert(value >= 0); + input.task_infos.addInfo(std::move(results)); + return value; +} + +TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& input, + OptionalTaskComposerExecutor executor) const +{ + if (terminals_.empty()) + throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + "' does not have terminals!"); + + boost::uuids::uuid root_node{}; + for (const auto& pair : nodes_) + { + if (pair.second->getInboundEdges().empty()) + { + root_node = pair.first; + break; + } + } + + if (root_node.is_nil()) + throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + "' does not have a root node!"); + + runRecursive(*(nodes_.at(root_node)), input, executor); + + for (std::size_t i = 0; i < terminals_.size(); ++i) + { + auto node_info = input.task_infos.getInfo(terminals_[i]); + if (node_info != nullptr) + { + auto info = std::make_unique(*this); + info->return_value = static_cast(i); + info->color = node_info->color; + info->message = node_info->message; + return info; + } + } + + throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + + "' has no node info for any of the leaf nodes!"); +} + +void TaskComposerPipeline::runRecursive(const TaskComposerNode& node, + TaskComposerInput& input, + OptionalTaskComposerExecutor executor) const +{ + if (node.getType() == TaskComposerNodeType::GRAPH) + throw std::runtime_error("TaskComposerPipeline, does not support GRAPH node types. Name: '" + name_ + "'"); + + if (node.getType() == TaskComposerNodeType::TASK) + { + const auto& task = static_cast(node); + int rv = task.run(input, executor); + if (task.isConditional()) + { + const auto& edge = node.getOutboundEdges().at(static_cast(rv)); + runRecursive(*nodes_.at(edge), input, executor); + } + else + { + if (node.getOutboundEdges().size() > 1) + throw std::runtime_error("TaskComposerPipeline, non conditional task can only have one out bound edge. Name: " + "'" + + name_ + "'"); + for (const auto& edge : node.getOutboundEdges()) + runRecursive(*(nodes_.at(edge)), input, executor); + } + } + else + { + const auto& pipeline = static_cast(node); + int rv = pipeline.run(input, executor); + if (pipeline.isConditional()) + { + const auto& edge = node.getOutboundEdges().at(static_cast(rv)); + runRecursive(*nodes_.at(edge), input, executor); + } + else + { + for (const auto& edge : node.getOutboundEdges()) + runRecursive(*nodes_.at(edge), input, executor); + } + } +} + +bool TaskComposerPipeline::operator==(const TaskComposerPipeline& rhs) const +{ + return (TaskComposerGraph::operator==(rhs)); +} +bool TaskComposerPipeline::operator!=(const TaskComposerPipeline& rhs) const { return !operator==(rhs); } + +template +void TaskComposerPipeline::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerGraph); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerPipeline) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerPipeline) diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp index f5a78f718c..e38a30e21d 100644 --- a/tesseract_task_composer/core/src/task_composer_task.cpp +++ b/tesseract_task_composer/core/src/task_composer_task.cpp @@ -34,49 +34,30 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerTask::TaskComposerTask() : TaskComposerNode("DoneTask", TaskComposerNodeType::TASK) {} - -TaskComposerTask::TaskComposerTask(std::string name, bool is_conditional) - : TaskComposerNode(std::move(name), TaskComposerNodeType::TASK), is_conditional_(is_conditional) +TaskComposerTask::TaskComposerTask(std::string name) : TaskComposerTask(std::move(name), true) {} +TaskComposerTask::TaskComposerTask(std::string name, bool conditional) + : TaskComposerNode(std::move(name), TaskComposerNodeType::TASK, conditional) { } TaskComposerTask::TaskComposerTask(std::string name, const YAML::Node& config) - : TaskComposerNode(std::move(name), TaskComposerNodeType::TASK) + : TaskComposerNode(std::move(name), TaskComposerNodeType::TASK, config) { - try - { - if (YAML::Node n = config["conditional"]) - is_conditional_ = n.as(); - else - throw std::runtime_error("TaskComposerTask, config missing 'conditional' entry"); - - if (YAML::Node n = config["inputs"]) - { - if (n.IsSequence()) - input_keys_ = n.as>(); - else - input_keys_ = { n.as() }; - } - - if (YAML::Node n = config["outputs"]) - { - if (n.IsSequence()) - output_keys_ = n.as>(); - else - output_keys_ = { n.as() }; - } - } - catch (const std::exception& e) - { - throw std::runtime_error("TaskComposerTask: Failed to parse yaml config data! Details: " + std::string(e.what())); - } } -bool TaskComposerTask::isConditional() const { return is_conditional_; } - int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const { + if (input.isAborted()) + { + auto info = std::make_unique(*this); + info->return_value = 0; + info->color = "white"; + info->message = "Aborted"; + info->aborted_ = true; + input.task_infos.addInfo(std::move(info)); + return 0; + } + TaskComposerNodeInfo::UPtr results; try { @@ -84,7 +65,7 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor } catch (const std::exception& e) { - results = std::make_unique(*this, input); + results = std::make_unique(*this); results->color = "red"; results->message = "Exception thrown: " + std::string(e.what()); results->return_value = 0; @@ -96,74 +77,12 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor return value; } -std::string TaskComposerTask::dump(std::ostream& os, - const TaskComposerNode* /*parent*/, - const std::map& results_map) const -{ - const std::string tmp = toString(uuid_, "node_"); - - std::string color{ "white" }; - int return_value = -1; - - auto it = results_map.find(uuid_); - if (it != results_map.end()) - { - return_value = it->second->return_value; - if (!it->second->isAborted()) - color = it->second->color; - } - - if (is_conditional_) - { - os << std::endl << tmp << " [shape=diamond, label=\"" << name_ << "\\n(" << uuid_str_ << ")"; - if (it != results_map.end()) - { - os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s" - << "\\n`" << it->second->message << "`"; - } - os << "\", color=black, fillcolor=" << color << ", style=filled];\n"; - - for (std::size_t i = 0; i < outbound_edges_.size(); ++i) - { - std::string line_type = (return_value == static_cast(i)) ? "bold" : "dashed"; - os << tmp << " -> " << toString(outbound_edges_[i], "node_") << " [style=" << line_type << ", label=\"[" - << std::to_string(i) << "]\"" - << "];\n"; - } - } - else - { - os << std::endl << tmp << " [label=\"" << name_ << "\\n(" << uuid_str_ << ")"; - if (it != results_map.end()) - { - os << "\\nTime: " << std::fixed << std::setprecision(3) << it->second->elapsed_time << "s" - << "\\n'" << it->second->message << "'"; - } - os << "\", color=black, fillcolor=" << color << ", style=filled];\n"; - - for (const auto& edge : outbound_edges_) - os << tmp << " -> " << toString(edge, "node_") << ";\n"; - } - - if (it == results_map.end()) - return {}; - - return it->second->dotgraph; -} - -bool TaskComposerTask::operator==(const TaskComposerTask& rhs) const -{ - bool equal = true; - equal &= (is_conditional_ == rhs.is_conditional_); - equal &= TaskComposerNode::operator==(rhs); - return equal; -} +bool TaskComposerTask::operator==(const TaskComposerTask& rhs) const { return (TaskComposerNode::operator==(rhs)); } bool TaskComposerTask::operator!=(const TaskComposerTask& rhs) const { return !operator==(rhs); } template void TaskComposerTask::serialize(Archive& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(is_conditional_); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerNode); } 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 1834f2e2c2..59cc86cf25 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 @@ -26,19 +26,25 @@ #include #include +#include +#include #include #include #include namespace tesseract_planning { +using AbortTaskFactory = TaskComposerTaskFactory; using DoneTaskFactory = TaskComposerTaskFactory; using ErrorTaskFactory = TaskComposerTaskFactory; using StartTaskFactory = TaskComposerTaskFactory; using GraphTaskFactory = TaskComposerTaskFactory; +using PipelineTaskFactory = TaskComposerTaskFactory; } // namespace tesseract_planning +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::AbortTaskFactory, AbortTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::DoneTaskFactory, DoneTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) @@ -47,3 +53,5 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ErrorTaskFactory, Er 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::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/examples/task_composer_example.cpp b/tesseract_task_composer/examples/task_composer_example.cpp index 2ccb123662..a8e235dbbf 100644 --- a/tesseract_task_composer/examples/task_composer_example.cpp +++ b/tesseract_task_composer/examples/task_composer_example.cpp @@ -23,10 +23,7 @@ class AddTaskComposerNode : public TaskComposerTask TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const override final { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->return_value = 0; std::cout << name_ << std::endl; double result = @@ -55,10 +52,7 @@ class MultiplyTaskComposerNode : public TaskComposerTask TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const override final { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->return_value = 0; std::cout << name_ << std::endl; double result = diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h index 9d7928c81b..d53711aef8 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h @@ -81,7 +81,7 @@ class ContinuousContactCheckTaskInfo : public TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; ContinuousContactCheckTaskInfo() = default; - ContinuousContactCheckTaskInfo(const ContinuousContactCheckTask& task, const TaskComposerInput& input); + ContinuousContactCheckTaskInfo(const ContinuousContactCheckTask& task); tesseract_environment::Environment::ConstPtr env; std::vector contact_results; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h index 5e7fdec9a1..dff19bc582 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h @@ -81,7 +81,7 @@ class DiscreteContactCheckTaskInfo : public TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; DiscreteContactCheckTaskInfo() = default; - DiscreteContactCheckTaskInfo(const DiscreteContactCheckTask& task, const TaskComposerInput& input); + DiscreteContactCheckTaskInfo(const DiscreteContactCheckTask& task); tesseract_environment::Environment::ConstPtr env; std::vector contact_results; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h index b196a1cce0..8471dbfcf5 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h @@ -94,7 +94,7 @@ class FixStateCollisionTaskInfo : public TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; FixStateCollisionTaskInfo() = default; - FixStateCollisionTaskInfo(const FixStateCollisionTask& task, const TaskComposerInput& input); + FixStateCollisionTaskInfo(const FixStateCollisionTask& task); TaskComposerNodeInfo::UPtr clone() const override; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index 88703d00b5..3a3f7ba315 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -116,9 +116,7 @@ class MotionPlannerTask : public TaskComposerTask TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task_info.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task_info.h index 09c34df7a5..1471d9a1d7 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task_info.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task_info.h @@ -47,7 +47,7 @@ class MotionPlannerTaskInfo : public TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; MotionPlannerTaskInfo() = default; - MotionPlannerTaskInfo(const TaskComposerTask& task, const TaskComposerInput& input); + MotionPlannerTaskInfo(const TaskComposerTask& task); tesseract_environment::Environment::ConstPtr env; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h index 1b70bc508e..39fdda8bc8 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h @@ -82,7 +82,7 @@ class TimeOptimalParameterizationTaskInfo : public TaskComposerNodeInfo using ConstUPtr = std::unique_ptr; TimeOptimalParameterizationTaskInfo() = default; - TimeOptimalParameterizationTaskInfo(const TimeOptimalParameterizationTask& task, const TaskComposerInput& input); + TimeOptimalParameterizationTaskInfo(const TimeOptimalParameterizationTask& task); TaskComposerNodeInfo::UPtr clone() const override; diff --git a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp index 26cfa82de4..147c35e6db 100644 --- a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp @@ -64,9 +64,7 @@ CheckInputTask::CheckInputTask(std::string name, TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index fb1163ac17..8160340202 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -70,9 +70,7 @@ ContinuousContactCheckTask::ContinuousContactCheckTask(std::string name, TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); @@ -150,9 +148,8 @@ void ContinuousContactCheckTask::serialize(Archive& ar, const unsigned int /*ver ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -ContinuousContactCheckTaskInfo::ContinuousContactCheckTaskInfo(const ContinuousContactCheckTask& task, - const TaskComposerInput& input) - : TaskComposerNodeInfo(task, input) +ContinuousContactCheckTaskInfo::ContinuousContactCheckTaskInfo(const ContinuousContactCheckTask& task) + : TaskComposerNodeInfo(task) { } diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index 3ee74e7280..d02182ec95 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -70,9 +70,7 @@ DiscreteContactCheckTask::DiscreteContactCheckTask(std::string name, TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); @@ -148,9 +146,8 @@ void DiscreteContactCheckTask::serialize(Archive& ar, const unsigned int /*versi ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -DiscreteContactCheckTaskInfo::DiscreteContactCheckTaskInfo(const DiscreteContactCheckTask& task, - const TaskComposerInput& input) - : TaskComposerNodeInfo(task, input) +DiscreteContactCheckTaskInfo::DiscreteContactCheckTaskInfo(const DiscreteContactCheckTask& task) + : TaskComposerNodeInfo(task) { } diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp index 2c80fbab30..0f69255988 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp @@ -73,9 +73,7 @@ FixStateBoundsTask::FixStateBoundsTask(std::string name, TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 8a9c11ba6d..f323394dd0 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -345,9 +345,7 @@ FixStateCollisionTask::FixStateCollisionTask(std::string name, TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); @@ -707,10 +705,7 @@ void FixStateCollisionTask::serialize(Archive& ar, const unsigned int /*version* ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -FixStateCollisionTaskInfo::FixStateCollisionTaskInfo(const FixStateCollisionTask& task, const TaskComposerInput& input) - : TaskComposerNodeInfo(task, input) -{ -} +FixStateCollisionTaskInfo::FixStateCollisionTaskInfo(const FixStateCollisionTask& task) : TaskComposerNodeInfo(task) {} TaskComposerNodeInfo::UPtr FixStateCollisionTaskInfo::clone() const { diff --git a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp index f89781fa1d..d932457560 100644 --- a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp @@ -72,9 +72,7 @@ FormatAsInputTask::FormatAsInputTask(std::string name, TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); info->return_value = 0; tesseract_common::Timer timer; diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index c709afa128..766a339839 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -92,9 +92,7 @@ IterativeSplineParameterizationTask::IterativeSplineParameterizationTask( TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index b32b51aff9..1dcbf1970c 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -73,9 +73,7 @@ MinLengthTask::MinLengthTask(std::string name, TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/motion_planner_task_info.cpp b/tesseract_task_composer/planning/src/nodes/motion_planner_task_info.cpp index dc1fd4454b..5d25bce257 100644 --- a/tesseract_task_composer/planning/src/nodes/motion_planner_task_info.cpp +++ b/tesseract_task_composer/planning/src/nodes/motion_planner_task_info.cpp @@ -34,10 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -MotionPlannerTaskInfo::MotionPlannerTaskInfo(const TaskComposerTask& task, const TaskComposerInput& input) - : TaskComposerNodeInfo(task, input) -{ -} +MotionPlannerTaskInfo::MotionPlannerTaskInfo(const TaskComposerTask& task) : TaskComposerNodeInfo(task) {} TaskComposerNodeInfo::UPtr MotionPlannerTaskInfo::clone() const { diff --git a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp index aa4389edee..4f0e9415ee 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -64,9 +64,7 @@ ProfileSwitchTask::ProfileSwitchTask(std::string name, TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); 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 7f7f52c1e6..c4fad1911e 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -286,9 +286,7 @@ void RasterMotionTask::serialize(Archive& ar, const unsigned int /*version*/) / TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); 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 0ed508dc32..3509ef31da 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 @@ -239,9 +239,7 @@ void RasterOnlyMotionTask::serialize(Archive& ar, const unsigned int /*version*/ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp index 0eed6d24e0..86e2a2cde3 100644 --- a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -77,9 +77,7 @@ RuckigTrajectorySmoothingTask::RuckigTrajectorySmoothingTask(std::string name, TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp index ca158c18f9..d424f040cf 100644 --- a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp @@ -83,9 +83,7 @@ TimeOptimalParameterizationTask::TimeOptimalParameterizationTask(std::string nam TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); @@ -189,9 +187,8 @@ void TimeOptimalParameterizationTask::serialize(Archive& ar, const unsigned int ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -TimeOptimalParameterizationTaskInfo::TimeOptimalParameterizationTaskInfo(const TimeOptimalParameterizationTask& task, - const TaskComposerInput& input) - : TaskComposerNodeInfo(task, input) +TimeOptimalParameterizationTaskInfo::TimeOptimalParameterizationTaskInfo(const TimeOptimalParameterizationTask& task) + : TaskComposerNodeInfo(task) { } diff --git a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp index 1cb455076b..4fcb716f14 100644 --- a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp @@ -61,10 +61,7 @@ UpdateEndStateTask::UpdateEndStateTask(std::string name, TaskComposerNodeInfo::UPtr UpdateEndStateTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->return_value = 0; tesseract_common::Timer timer; timer.start(); diff --git a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp index bce7566335..03f0d57577 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp @@ -65,10 +65,7 @@ UpdateStartAndEndStateTask::UpdateStartAndEndStateTask(std::string name, TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->return_value = 0; tesseract_common::Timer timer; timer.start(); diff --git a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp index 65927d74f5..018f5e95f7 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp @@ -61,10 +61,7 @@ UpdateStartStateTask::UpdateStartStateTask(std::string name, TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; - + auto info = std::make_unique(*this); info->return_value = 0; tesseract_common::Timer timer; timer.start(); diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp index 9651e7cb7d..4cc3501b0b 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -74,9 +74,7 @@ UpsampleTrajectoryTask::UpsampleTrajectoryTask(std::string name, TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const { - auto info = std::make_unique(*this, input); - if (info->isAborted()) - return info; + auto info = std::make_unique(*this); // Get the problem auto& problem = dynamic_cast(*input.problem); diff --git a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h index 5a0eeff1fa..fb2837055c 100644 --- a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h +++ b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h @@ -32,9 +32,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include -#include namespace tf { @@ -64,6 +61,8 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor TaskComposerFuture::UPtr run(const TaskComposerGraph& task_graph, TaskComposerInput& task_input) override final; + TaskComposerFuture::UPtr run(const TaskComposerPipeline& task_pipeline, TaskComposerInput& task_input) override final; + TaskComposerFuture::UPtr run(const TaskComposerTask& task, TaskComposerInput& task_input) override final; long getWorkerCount() const override final; @@ -94,6 +93,11 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor TaskComposerInput& task_input, TaskComposerExecutor& task_executor); + static std::shared_ptr>> + convertToTaskflow(const TaskComposerPipeline& task_pipeline, + TaskComposerInput& task_input, + TaskComposerExecutor& task_executor); + static std::shared_ptr>> convertToTaskflow(const TaskComposerTask& task, TaskComposerInput& task_input, TaskComposerExecutor& task_executor); }; 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 e405227bd1..ddf15251e3 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -82,6 +82,20 @@ TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerGra return std::make_unique(f, std::move(taskflow)); } +TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerPipeline& task_pipeline, + TaskComposerInput& task_input) +{ + auto taskflow = convertToTaskflow(task_pipeline, task_input, *this); + std::shared_future f = executor_->run(*(taskflow->front())); + + // std::ofstream out_data; + // out_data.open(tesseract_common::getTempPath() + "task_composer_example.dot"); + // taskflow.top->dump(out_data); // dump the graph including dynamic tasks + // out_data.close(); + + return std::make_unique(f, std::move(taskflow)); +} + TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerTask& task, TaskComposerInput& task_input) { auto taskflow = convertToTaskflow(task, task_input, *this); @@ -162,12 +176,26 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr ->emplace([task, &task_input, &task_executor] { task->run(task_input, task_executor); }) .name(pair.second->getName()); } + if (pair.second->getType() == TaskComposerNodeType::PIPELINE) + { + auto pipeline = std::static_pointer_cast(pair.second); + if (edges.size() > 1 && pipeline->isConditional()) + tasks[pair.first] = + tf_container->front() + ->emplace([pipeline, &task_input, &task_executor] { return pipeline->run(task_input, task_executor); }) + .name(pair.second->getName()); + else + tasks[pair.first] = + tf_container->front() + ->emplace([pipeline, &task_input, &task_executor] { pipeline->run(task_input, task_executor); }) + .name(pair.second->getName()); + } else if (pair.second->getType() == TaskComposerNodeType::GRAPH) { const auto& graph = static_cast(*pair.second); // Must add a Node Info object for the graph - auto info = std::make_unique(graph, task_input); + auto info = std::make_unique(graph); info->color = "green"; task_input.task_infos.addInfo(std::move(info)); @@ -192,6 +220,19 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr return tf_container; } +std::shared_ptr>> +TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerPipeline& task_pipeline, + TaskComposerInput& task_input, + TaskComposerExecutor& task_executor) +{ + auto tf_container = std::make_shared>>(); + tf_container->emplace_back(std::make_unique(task_pipeline.getName())); + tf_container->front() + ->emplace([&task_pipeline, &task_input, &task_executor] { return task_pipeline.run(task_input, task_executor); }) + .name(task_pipeline.getName()); + return tf_container; +} + std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerTask& task, TaskComposerInput& task_input, diff --git a/tesseract_task_composer/test/CMakeLists.txt b/tesseract_task_composer/test/CMakeLists.txt index 1892dce5a6..fe5a7bac6e 100644 --- a/tesseract_task_composer/test/CMakeLists.txt +++ b/tesseract_task_composer/test/CMakeLists.txt @@ -95,3 +95,26 @@ target_code_coverage( add_gtest_discover_tests(${PROJECT_NAME}_plugin_factories_unit) add_dependencies(run_tests ${PROJECT_NAME}_plugin_factories_unit) add_dependencies(${PROJECT_NAME}_plugin_factories_unit ${PROJECT_NAME}) + +# Core Tests +add_executable(${PROJECT_NAME}_core_unit ${PROJECT_NAME}_core_unit.cpp) +target_link_libraries( + ${PROJECT_NAME}_core_unit + PRIVATE GTest::GTest + GTest::Main + ${PROJECT_NAME} + ${PROJECT_NAME}_nodes) +target_include_directories(${PROJECT_NAME}_core_unit PUBLIC "$") +target_compile_options(${PROJECT_NAME}_core_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} + ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_clang_tidy(${PROJECT_NAME}_core_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_core_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) +target_code_coverage( + ${PROJECT_NAME}_core_unit + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) +add_gtest_discover_tests(${PROJECT_NAME}_core_unit) +add_dependencies(run_tests ${PROJECT_NAME}_core_unit) +add_dependencies(${PROJECT_NAME}_core_unit ${PROJECT_NAME}) diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp new file mode 100644 index 0000000000..40c52f8673 --- /dev/null +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -0,0 +1,1722 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +TESSERACT_ANY_EXPORT(tesseract_common, JointState) + +using namespace tesseract_planning; + +TEST(TesseractTaskComposerCoreUnit, TaskComposerDataStorageTests) // NOLINT +{ + std::string key{ "joint_state" }; + std::vector joint_names{ "joint_1", "joint_2" }; + Eigen::Vector2d joint_values(5, 10); + tesseract_common::JointState js(joint_names, joint_values); + TaskComposerDataStorage data; + EXPECT_FALSE(data.hasKey(key)); + EXPECT_TRUE(data.getData(key).isNull()); + + // Test Add + data.setData(key, js); + EXPECT_TRUE(data.hasKey(key)); + EXPECT_TRUE(data.getData().size() == 1); + EXPECT_TRUE(data.getData(key).as() == js); + + // Test Copy + TaskComposerDataStorage copy{ data }; + EXPECT_TRUE(copy.hasKey(key)); + EXPECT_TRUE(copy.getData().size() == 1); + EXPECT_TRUE(copy.getData(key).as() == js); + + // Test Assign + TaskComposerDataStorage assign; + assign = data; + EXPECT_TRUE(assign.hasKey(key)); + EXPECT_TRUE(assign.getData().size() == 1); + EXPECT_TRUE(assign.getData(key).as() == js); + + // Test Assign Move + TaskComposerDataStorage move_assign; + move_assign = std::move(data); + EXPECT_TRUE(move_assign.hasKey(key)); + EXPECT_TRUE(move_assign.getData().size() == 1); + EXPECT_TRUE(move_assign.getData(key).as() == js); + + // Serialization + test_suite::runSerializationTest(move_assign, "TaskComposerDataStorageTests"); + + // Test Remove + move_assign.removeData(key); + EXPECT_FALSE(move_assign.hasKey(key)); + EXPECT_TRUE(move_assign.getData().empty()); +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerInputTests) // NOLINT +{ + TaskComposerNode node; + auto input = std::make_unique(std::make_unique()); + EXPECT_FALSE(input->dotgraph); + EXPECT_FALSE(input->isAborted()); + EXPECT_TRUE(input->isSuccessful()); + EXPECT_TRUE(input->task_infos.getInfoMap().empty()); + input->task_infos.addInfo(std::make_unique(node)); + input->abort(node.getUUID()); + EXPECT_EQ(input->task_infos.getAbortingNode(), node.getUUID()); + EXPECT_TRUE(input->isAborted()); + EXPECT_FALSE(input->isSuccessful()); + EXPECT_EQ(input->task_infos.getInfoMap().size(), 1); + + // Serialization + test_suite::runSerializationPointerTest(input, "TaskComposerInputTests"); + + input->reset(); + EXPECT_TRUE(input->problem != nullptr); + EXPECT_FALSE(input->dotgraph); + EXPECT_FALSE(input->isAborted()); + EXPECT_TRUE(input->isSuccessful()); + EXPECT_TRUE(input->task_infos.getInfoMap().empty()); +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT +{ + auto problem = std::make_unique(); + EXPECT_EQ(problem->name, "unset"); + + // Serialization + test_suite::runSerializationPointerTest(problem, "TaskComposerProblemTests"); + + auto problem2 = std::make_unique("TaskComposerProblemTests"); + EXPECT_EQ(problem2->name, "TaskComposerProblemTests"); + + auto problem3 = std::make_unique(TaskComposerDataStorage(), "TaskComposerProblemTests"); + EXPECT_EQ(problem3->name, "TaskComposerProblemTests"); +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLINT +{ + TaskComposerNode node; + auto node_info = std::make_unique(node); + + auto node_info_container = std::make_unique(); + EXPECT_TRUE(node_info_container->getInfoMap().empty()); + node_info_container->addInfo(std::move(node_info)); + EXPECT_EQ(node_info_container->getInfoMap().size(), 1); + EXPECT_TRUE(node_info_container->getInfo(node.getUUID()) != nullptr); + + // Serialization + test_suite::runSerializationPointerTest(node_info_container, "TaskComposerNodeInfoContainerTests"); + + // Copy + auto copy_node_info_container = std::make_unique(*node_info_container); + EXPECT_EQ(copy_node_info_container->getInfoMap().size(), 1); + EXPECT_TRUE(copy_node_info_container->getInfo(node.getUUID()) != nullptr); + + // Move + auto move_node_info_container = std::make_unique(std::move(*node_info_container)); + EXPECT_EQ(move_node_info_container->getInfoMap().size(), 1); + EXPECT_TRUE(move_node_info_container->getInfo(node.getUUID()) != nullptr); + + move_node_info_container->clear(); + EXPECT_TRUE(move_node_info_container->getInfoMap().empty()); + EXPECT_TRUE(move_node_info_container->getInfo(node.getUUID()) == nullptr); +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeTests) // NOLINT +{ + std::stringstream os; + auto node = std::make_unique(); + // Default + EXPECT_EQ(node->getName(), "TaskComposerNode"); + EXPECT_EQ(node->getType(), TaskComposerNodeType::TASK); + EXPECT_FALSE(node->getUUID().is_nil()); + EXPECT_FALSE(node->getUUIDString().empty()); + EXPECT_TRUE(node->getParentUUID().is_nil()); + EXPECT_TRUE(node->getOutboundEdges().empty()); + EXPECT_TRUE(node->getInboundEdges().empty()); + EXPECT_TRUE(node->getInputKeys().empty()); + EXPECT_TRUE(node->getOutputKeys().empty()); + EXPECT_FALSE(node->isConditional()); + EXPECT_NO_THROW(node->dump(os)); // NOLINT + + // Setters + std::string name{ "TaskComposerNodeTests" }; + std::vector input_keys{ "I1", "I2" }; + std::vector output_keys{ "O1", "O2" }; + node->setName(name); + node->setInputKeys(input_keys); + node->setOutputKeys(output_keys); + node->setConditional(true); + EXPECT_EQ(node->getName(), name); + EXPECT_EQ(node->getInputKeys(), input_keys); + EXPECT_EQ(node->getOutputKeys(), output_keys); + EXPECT_EQ(node->isConditional(), true); + EXPECT_NO_THROW(node->dump(os)); // NOLINT + + // Utils + std::map rename_input_keys{ { "I1", "I3" }, { "I2", "I4" } }; + std::map rename_output_keys{ { "O1", "O3" }, { "O2", "O4" } }; + node->renameInputKeys(rename_input_keys); + node->renameOutputKeys(rename_output_keys); + EXPECT_EQ(node->getInputKeys(), std::vector({ "I3", "I4" })); + EXPECT_EQ(node->getOutputKeys(), std::vector({ "O3", "O4" })); + EXPECT_NO_THROW(node->dump(os)); // NOLINT + + // Serialization + test_suite::runSerializationPointerTest(node, "TaskComposerNodeTests"); + + { + std::string str = R"(config: + inputs: input_data + outputs: output_data)"; + YAML::Node config = YAML::Load(str); + auto task = std::make_unique(name, TaskComposerNodeType::TASK, config["config"]); + EXPECT_EQ(task->getName(), name); + EXPECT_EQ(task->getType(), TaskComposerNodeType::TASK); + EXPECT_EQ(task->getInputKeys().size(), 1); + EXPECT_EQ(task->getOutputKeys().size(), 1); + EXPECT_EQ(task->getInputKeys().front(), "input_data"); + EXPECT_EQ(task->getOutputKeys().front(), "output_data"); + EXPECT_EQ(task->isConditional(), false); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerNodeTests"); + } + + { + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data)"; + YAML::Node config = YAML::Load(str); + auto task = std::make_unique(name, TaskComposerNodeType::TASK, config["config"]); + EXPECT_EQ(task->getName(), name); + EXPECT_EQ(task->getType(), TaskComposerNodeType::TASK); + EXPECT_EQ(task->getInputKeys().size(), 1); + EXPECT_EQ(task->getOutputKeys().size(), 1); + EXPECT_EQ(task->getInputKeys().front(), "input_data"); + EXPECT_EQ(task->getOutputKeys().front(), "output_data"); + EXPECT_EQ(task->isConditional(), true); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerNodeTests"); + } +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoTests) // NOLINT +{ + test_suite::runTaskComposerNodeInfoTest(); +} + +class TestTask : public TaskComposerTask +{ +public: + using TaskComposerTask::TaskComposerTask; + + bool throw_exception{ false }; + bool set_abort{ false }; + int return_value{ 0 }; + + bool operator==(const TestTask& rhs) const + { + bool equal = true; + equal &= (throw_exception == rhs.throw_exception); + equal &= (set_abort == rhs.set_abort); + equal &= (return_value == rhs.return_value); + equal &= TaskComposerTask::operator==(rhs); + return equal; + } + bool operator!=(const TestTask& rhs) const { return !operator==(rhs); } + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + + template + void serialize(Archive& ar, const unsigned int /*version*/) + { + ar& BOOST_SERIALIZATION_NVP(throw_exception); + ar& BOOST_SERIALIZATION_NVP(set_abort); + ar& BOOST_SERIALIZATION_NVP(return_value); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); + } + + TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override final + { + if (throw_exception) + throw std::runtime_error("TestTask, failure"); + + auto node_info = std::make_unique(*this); + if (conditional_) + node_info->color = (return_value == 0) ? "red" : "green"; + else + node_info->color = "green"; + node_info->return_value = return_value; + + if (set_abort) + { + node_info->color = "red"; + input.abort(uuid_); + } + + return node_info; + } +}; + +#include +#include +BOOST_CLASS_EXPORT_KEY2(TestTask, "TestTask") +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(TestTask) +BOOST_CLASS_EXPORT_IMPLEMENT(TestTask) + +TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT +{ + std::string name = "TaskComposerTaskTests"; + { // Not Conditional + auto task = std::make_unique(name, false); + EXPECT_EQ(task->getName(), name); + EXPECT_FALSE(task->isConditional()); + EXPECT_TRUE(task->getInputKeys().empty()); + EXPECT_TRUE(task->getOutputKeys().empty()); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(task->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + + std::stringstream os; + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + } + + { // Conditional + auto task = std::make_unique(name, true); + task->return_value = 1; + EXPECT_EQ(task->getName(), name); + EXPECT_TRUE(task->isConditional()); + EXPECT_TRUE(task->getInputKeys().empty()); + EXPECT_TRUE(task->getOutputKeys().empty()); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(task->run(input), 1); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 1); + + std::stringstream os; + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + } + + { + std::string str = R"(config: + conditional: false + inputs: input_data + outputs: output_data)"; + YAML::Node config = YAML::Load(str); + auto task = std::make_unique(name, config["config"]); + EXPECT_EQ(task->getName(), name); + EXPECT_FALSE(task->isConditional()); + EXPECT_EQ(task->getInputKeys().size(), 1); + EXPECT_EQ(task->getOutputKeys().size(), 1); + EXPECT_EQ(task->getInputKeys().front(), "input_data"); + EXPECT_EQ(task->getOutputKeys().front(), "output_data"); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(task->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + + std::stringstream os; + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + } + + { + std::string str = R"(config: + conditional: true + inputs: [input_data] + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + auto task = std::make_unique(name, config["config"]); + task->return_value = 1; + EXPECT_EQ(task->getName(), name); + EXPECT_TRUE(task->isConditional()); + EXPECT_EQ(task->getInputKeys().size(), 1); + EXPECT_EQ(task->getOutputKeys().size(), 1); + EXPECT_EQ(task->getInputKeys().front(), "input_data"); + EXPECT_EQ(task->getOutputKeys().front(), "output_data"); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(task->run(input), 1); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 1); + + std::stringstream os; + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + } + + { // Failure due to exception during run + std::string str = R"(config: + conditional: true + inputs: [input_data] + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + auto task = std::make_unique(name, config["config"]); + TaskComposerInput input(std::make_unique()); + + task->throw_exception = true; + EXPECT_EQ(task->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + } +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT +{ + std::string name = "TaskComposerPipelineTests"; + std::string name1 = "TaskComposerPipelineTests1"; + std::string name2 = "TaskComposerPipelineTests2"; + std::string name3 = "TaskComposerPipelineTests3"; + std::string name4 = "TaskComposerPipelineTests4"; + std::vector input_keys{ "input_data" }; + std::vector output_keys{ "output_data" }; + std::map rename_input_keys{ { "input_data", "id" }, { "output_data", "od" } }; + std::map rename_output_keys{ { "output_data", "od" } }; + std::vector new_input_keys{ "id" }; + std::vector new_output_keys{ "od" }; + { // Not Conditional + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, false); + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task1->setInputKeys(input_keys); + task1->setOutputKeys(output_keys); + task2->setInputKeys(output_keys); + task2->setOutputKeys(output_keys); + task3->setInputKeys(output_keys); + task3->setOutputKeys(output_keys); + task4->setInputKeys(output_keys); + task4->setOutputKeys(output_keys); + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline->addNode(std::move(task4)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3 }); + pipeline->addEdges(uuid3, { uuid4 }); + pipeline->setTerminals({ uuid4 }); + auto nodes_map = pipeline->getNodes(); + EXPECT_EQ(pipeline->getName(), name); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ uuid4 })); + EXPECT_EQ(nodes_map.at(uuid1)->getInboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().front(), uuid1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().front(), uuid4); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid4)->getOutboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getInputKeys(), input_keys); + EXPECT_EQ(nodes_map.at(uuid1)->getOutputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid2)->getInputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid2)->getOutputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid3)->getInputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid3)->getOutputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid4)->getInputKeys(), output_keys); + EXPECT_EQ(nodes_map.at(uuid4)->getOutputKeys(), output_keys); + pipeline->renameInputKeys(rename_input_keys); + pipeline->renameOutputKeys(rename_output_keys); + EXPECT_EQ(nodes_map.at(uuid1)->getInputKeys(), new_input_keys); + EXPECT_EQ(nodes_map.at(uuid1)->getOutputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid2)->getInputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid2)->getOutputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid3)->getInputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid3)->getOutputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid4)->getInputKeys(), new_output_keys); + EXPECT_EQ(nodes_map.at(uuid4)->getOutputKeys(), new_output_keys); + + // Serialization + test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 5); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test1a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test1b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Conditional + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 1; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline->addNode(std::move(task4)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3, uuid4 }); + pipeline->setTerminals({ uuid3, uuid4 }); + auto nodes_map = pipeline->getNodes(); + EXPECT_EQ(pipeline->getName(), name); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ uuid3, uuid4 })); + EXPECT_EQ(nodes_map.at(uuid1)->getInboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().front(), uuid1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().size(), 2); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().back(), uuid4); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid4)->getOutboundEdges().size(), 0); + + // Serialization + test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 1); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 1); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test2a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test2b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Throw exception + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 0; + task2->throw_exception = true; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline->addNode(std::move(task4)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3, uuid4 }); + pipeline->setTerminals({ uuid3, uuid4 }); + auto nodes_map = pipeline->getNodes(); + EXPECT_EQ(pipeline->getName(), name); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ uuid3, uuid4 })); + EXPECT_EQ(nodes_map.at(uuid1)->getInboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().front(), uuid1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().size(), 2); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().back(), uuid4); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid4)->getOutboundEdges().size(), 0); + + // Serialization + test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test3a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test3b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Set Abort + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 0; + task2->set_abort = true; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline->addNode(std::move(task4)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3, uuid4 }); + pipeline->setTerminals({ uuid3, uuid4 }); + auto nodes_map = pipeline->getNodes(); + EXPECT_EQ(pipeline->getName(), name); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ uuid3, uuid4 })); + EXPECT_EQ(nodes_map.at(uuid1)->getInboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().front(), uuid1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().size(), 2); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().back(), uuid4); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid4)->getOutboundEdges().size(), 0); + + // Serialization + test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_FALSE(input.isSuccessful()); + EXPECT_TRUE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test4a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test4b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Nested Pipeline Not Conditional + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 1; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline1 = std::make_unique(name + "_1", false); + boost::uuids::uuid uuid1 = pipeline1->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline1->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline1->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline1->addNode(std::move(task4)); + pipeline1->addEdges(uuid1, { uuid2 }); + pipeline1->addEdges(uuid2, { uuid3, uuid4 }); + pipeline1->setTerminals({ uuid3, uuid4 }); + + auto task5 = std::make_unique(name1, false); + auto task6 = std::make_unique(name2, true); + task6->return_value = 1; + auto task7 = std::make_unique(name3, false); + auto task8 = std::make_unique(name4, false); + task8->return_value = 1; + auto pipeline2 = std::make_unique(name + "_2", false); + boost::uuids::uuid uuid5 = pipeline2->addNode(std::move(task5)); + boost::uuids::uuid uuid6 = pipeline2->addNode(std::move(task6)); + boost::uuids::uuid uuid7 = pipeline2->addNode(std::move(task7)); + boost::uuids::uuid uuid8 = pipeline2->addNode(std::move(task8)); + pipeline2->addEdges(uuid5, { uuid6 }); + pipeline2->addEdges(uuid6, { uuid7, uuid8 }); + pipeline2->setTerminals({ uuid7, uuid8 }); + + auto pipeline3 = std::make_unique(name + "_3"); + boost::uuids::uuid uuid9 = pipeline3->addNode(std::move(pipeline1)); + boost::uuids::uuid uuid10 = pipeline3->addNode(std::move(pipeline2)); + pipeline3->addEdges(uuid9, { uuid10 }); + pipeline3->setTerminals({ uuid10 }); + + // Serialization + test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline3->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 9); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test5a.dot"); + EXPECT_NO_THROW(pipeline3->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test5b.dot"); + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Nested Pipeline Conditional + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 1; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline1 = std::make_unique(name + "_1", true); + boost::uuids::uuid uuid1 = pipeline1->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline1->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline1->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline1->addNode(std::move(task4)); + pipeline1->addEdges(uuid1, { uuid2 }); + pipeline1->addEdges(uuid2, { uuid3, uuid4 }); + pipeline1->setTerminals({ uuid3, uuid4 }); + + auto task5 = std::make_unique(name1, false); + auto task6 = std::make_unique(name2, true); + task6->return_value = 1; + auto task7 = std::make_unique(name3, false); + auto task8 = std::make_unique(name4, false); + task8->return_value = 1; + auto pipeline2 = std::make_unique(name + "_2", false); + boost::uuids::uuid uuid5 = pipeline2->addNode(std::move(task5)); + boost::uuids::uuid uuid6 = pipeline2->addNode(std::move(task6)); + boost::uuids::uuid uuid7 = pipeline2->addNode(std::move(task7)); + boost::uuids::uuid uuid8 = pipeline2->addNode(std::move(task8)); + pipeline2->addEdges(uuid5, { uuid6 }); + pipeline2->addEdges(uuid6, { uuid7, uuid8 }); + pipeline2->setTerminals({ uuid7, uuid8 }); + + auto pipeline3 = std::make_unique(name + "_3"); + auto task11 = std::make_unique(name1, false); + boost::uuids::uuid uuid9 = pipeline3->addNode(std::move(pipeline1)); + boost::uuids::uuid uuid10 = pipeline3->addNode(std::move(pipeline2)); + boost::uuids::uuid uuid11 = pipeline3->addNode(std::move(task11)); + pipeline3->addEdges(uuid9, { uuid11, uuid10 }); + pipeline3->setTerminals({ uuid11, uuid10 }); + + // Serialization + test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline3->run(input), 1); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 9); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test6a.dot"); + EXPECT_NO_THROW(pipeline3->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test6b.dot"); + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Nested Pipeline Abort + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 1; + task2->set_abort = true; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline1 = std::make_unique(name + "_1", true); + boost::uuids::uuid uuid1 = pipeline1->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline1->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline1->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline1->addNode(std::move(task4)); + pipeline1->addEdges(uuid1, { uuid2 }); + pipeline1->addEdges(uuid2, { uuid3, uuid4 }); + pipeline1->setTerminals({ uuid3, uuid4 }); + + auto task5 = std::make_unique(name1, false); + auto task6 = std::make_unique(name2, true); + task6->return_value = 1; + auto task7 = std::make_unique(name3, false); + auto task8 = std::make_unique(name4, false); + task8->return_value = 1; + auto pipeline2 = std::make_unique(name + "_2", false); + boost::uuids::uuid uuid5 = pipeline2->addNode(std::move(task5)); + boost::uuids::uuid uuid6 = pipeline2->addNode(std::move(task6)); + boost::uuids::uuid uuid7 = pipeline2->addNode(std::move(task7)); + boost::uuids::uuid uuid8 = pipeline2->addNode(std::move(task8)); + pipeline2->addEdges(uuid5, { uuid6 }); + pipeline2->addEdges(uuid6, { uuid7, uuid8 }); + pipeline2->setTerminals({ uuid7, uuid8 }); + + auto pipeline3 = std::make_unique(name + "_3"); + auto task11 = std::make_unique(name1, false); + boost::uuids::uuid uuid9 = pipeline3->addNode(std::move(pipeline1)); + boost::uuids::uuid uuid10 = pipeline3->addNode(std::move(pipeline2)); + boost::uuids::uuid uuid11 = pipeline3->addNode(std::move(task11)); + pipeline3->addEdges(uuid9, { uuid11, uuid10 }); + pipeline3->setTerminals({ uuid11, uuid10 }); + + // Serialization + test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline3->run(input), 1); + EXPECT_FALSE(input.isSuccessful()); + EXPECT_TRUE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 6); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test7a.dot"); + EXPECT_NO_THROW(pipeline3->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test7b.dot"); + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + // This section test yaml parsing + + { + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + auto pipeline = std::make_unique(name, config["config"], factory); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals().size(), 1); + auto task1 = pipeline->getNodeByName("StartTask"); + auto task2 = pipeline->getNodeByName("DoneTask"); + EXPECT_EQ(pipeline->getNodeByName("DoestNotExist"), nullptr); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ task2->getUUID() })); + EXPECT_EQ(task1->getInboundEdges().size(), 0); + EXPECT_EQ(task1->getOutboundEdges().size(), 1); + EXPECT_EQ(task1->getOutboundEdges().front(), task2->getUUID()); + EXPECT_EQ(task2->getInboundEdges().size(), 1); + EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); + EXPECT_EQ(task2->getOutboundEdges().size(), 0); + } + + { + std::string str = R"(task_composer_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_task_composer_factories + tasks: + plugins: + TestPipeline: + class: PipelineTaskFactory + config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + + TaskComposerPluginFactory factory(str); + + std::string str2 = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + task: + name: TestPipeline + conditional: false + input_remapping: + input_data: output_data + output_remapping: + output_data: input_data + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask])"; + YAML::Node config = YAML::Load(str2); + auto pipeline = std::make_unique(name, config["config"], factory); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals().size(), 2); + auto task1 = pipeline->getNodeByName("StartTask"); + auto task2 = pipeline->getNodeByName("ErrorTask"); + auto task3 = pipeline->getNodeByName("DoneTask"); + EXPECT_EQ(pipeline->getNodeByName("DoestNotExist"), nullptr); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ task2->getUUID(), task3->getUUID() })); + EXPECT_EQ(task1->getInboundEdges().size(), 0); + EXPECT_EQ(task1->getOutboundEdges().size(), 2); + EXPECT_EQ(task1->getOutboundEdges().front(), task2->getUUID()); + EXPECT_EQ(task1->getOutboundEdges().back(), task3->getUUID()); + EXPECT_EQ(task2->getInboundEdges().size(), 1); + EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); + EXPECT_EQ(task2->getOutboundEdges().size(), 0); + EXPECT_EQ(task3->getInboundEdges().size(), 1); + EXPECT_EQ(task3->getInboundEdges().front(), task1->getUUID()); + EXPECT_EQ(task3->getOutboundEdges().size(), 0); + } + + // This section tests failures + + { // Missing terminals + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, true); + task2->return_value = 1; + auto task3 = std::make_unique(name3, false); + auto task4 = std::make_unique(name4, false); + task4->return_value = 1; + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + boost::uuids::uuid uuid4 = pipeline->addNode(std::move(task4)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3, uuid4 }); + auto nodes_map = pipeline->getNodes(); + EXPECT_EQ(pipeline->getName(), name); + EXPECT_TRUE(pipeline->isConditional()); + EXPECT_NE(pipeline->getTerminals(), std::vector({ uuid3, uuid4 })); + EXPECT_EQ(nodes_map.at(uuid1)->getInboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid1)->getOutboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid2)->getInboundEdges().front(), uuid1); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().size(), 2); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().front(), uuid3); + EXPECT_EQ(nodes_map.at(uuid2)->getOutboundEdges().back(), uuid4); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid3)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid3)->getOutboundEdges().size(), 0); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().size(), 1); + EXPECT_EQ(nodes_map.at(uuid4)->getInboundEdges().front(), uuid2); + EXPECT_EQ(nodes_map.at(uuid4)->getOutboundEdges().size(), 0); + + // Serialization + test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test8a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test8b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // No root + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, false); + auto task3 = std::make_unique(name3, false); + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + pipeline->setTerminals({ uuid3 }); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3 }); + pipeline->addEdges(uuid3, { uuid1 }); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test9a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test9b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Non conditional with multiple out edgets + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, false); + auto task3 = std::make_unique(name3, false); + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid1, { uuid3 }); + pipeline->setTerminals({ uuid2, uuid3 }); + + TaskComposerInput input(std::make_unique()); + EXPECT_EQ(pipeline->run(input), 0); + EXPECT_TRUE(input.isSuccessful()); + EXPECT_FALSE(input.isAborted()); + EXPECT_EQ(input.task_infos.getInfoMap().size(), 2); + EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test10a.dot"); + EXPECT_NO_THROW(pipeline->dump(os1)); // NOLINT + os1.close(); + + std::ofstream os2; + os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test10b.dot"); + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + os2.close(); + } + + { // Set invalid terminal + auto task1 = std::make_unique(name1, false); + auto task2 = std::make_unique(name2, false); + auto task3 = std::make_unique(name3, false); + auto pipeline = std::make_unique(name); + boost::uuids::uuid uuid1 = pipeline->addNode(std::move(task1)); + boost::uuids::uuid uuid2 = pipeline->addNode(std::move(task2)); + boost::uuids::uuid uuid3 = pipeline->addNode(std::move(task3)); + pipeline->addEdges(uuid1, { uuid2 }); + pipeline->addEdges(uuid2, { uuid3 }); + pipeline->addEdges(uuid3, { uuid1 }); + EXPECT_ANY_THROW(pipeline->setTerminals({ uuid3 })); // NOLINT + EXPECT_ANY_THROW(pipeline->setTerminals({ boost::uuids::uuid{} })); // NOLINT + } + + { // Edges is not a sequence failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Edges source is missing + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Edges destination is missing + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Edges source node name invalid + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: DoesNotExist + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Edges destination node name invalid + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoesNotExist] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // terminals is missing + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // terminals invalid entry + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoesNotExist])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Node is not a map + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + - StartTask: + class: DoesNotExist + config: + conditional: false + - DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Node missing class or task entry + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Node class does not exist + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: DoesNotExist + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } +} + +// Graph is mostly tested through the Pipeline tests becasue they can be run +TEST(TesseractTaskComposerCoreUnit, TaskComposerGraphTests) // NOLINT +{ + std::string name{ "TaskComposerGraphTests" }; + auto graph = std::make_unique(name); + EXPECT_EQ(graph->getName(), name); + EXPECT_EQ(graph->getType(), TaskComposerNodeType::GRAPH); + EXPECT_EQ(graph->isConditional(), false); + + { + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + auto pipeline = std::make_unique(name, config["config"], factory); + EXPECT_FALSE(pipeline->isConditional()); + EXPECT_EQ(pipeline->getTerminals().size(), 1); + auto task1 = pipeline->getNodeByName("StartTask"); + auto task2 = pipeline->getNodeByName("DoneTask"); + EXPECT_EQ(pipeline->getNodeByName("DoestNotExist"), nullptr); + EXPECT_EQ(pipeline->getTerminals(), std::vector({ task2->getUUID() })); + EXPECT_EQ(task1->getInboundEdges().size(), 0); + EXPECT_EQ(task1->getOutboundEdges().size(), 1); + EXPECT_EQ(task1->getOutboundEdges().front(), task2->getUUID()); + EXPECT_EQ(task2->getInboundEdges().size(), 1); + EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); + EXPECT_EQ(task2->getOutboundEdges().size(), 0); + } + + { // Failure conditional graph is currently not supported + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Task missing name entry + std::string str = R"(task_composer_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_task_composer_factories + tasks: + plugins: + TestPipeline: + class: PipelineTaskFactory + config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + + TaskComposerPluginFactory factory(str); + + std::string str2 = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + task: + conditional: false + input_remapping: + input_data: output_data + output_remapping: + output_data: input_data + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask])"; + YAML::Node config = YAML::Load(str2); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } + + { // Task name does not exist + std::string str = R"(task_composer_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_task_composer_factories + tasks: + plugins: + TestPipeline: + class: PipelineTaskFactory + config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + class: StartTaskFactory + config: + conditional: false + DoneTask: + class: DoneTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [DoneTask] + terminals: [DoneTask])"; + + TaskComposerPluginFactory factory(str); + + std::string str2 = R"(config: + conditional: true + inputs: input_data + outputs: output_data + nodes: + StartTask: + task: + name: DoesNotExist + conditional: false + input_remapping: + input_data: output_data + output_remapping: + output_data: input_data + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + edges: + - source: StartTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask])"; + YAML::Node config = YAML::Load(str2); + EXPECT_ANY_THROW(std::make_unique(name, config["config"], factory)); // NOLINT + } +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerAbortTaskTests) // NOLINT +{ + { // Construction + AbortTask task; + EXPECT_EQ(task.getName(), "AbortTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + AbortTask task("abc", true); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + AbortTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Serialization + auto task = std::make_unique("abc", true); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerAbortTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + AbortTask task; + EXPECT_EQ(task.run(*input), 0); + auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->message, "Aborted"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(input->isAborted(), true); + EXPECT_EQ(input->isSuccessful(), false); + EXPECT_EQ(input->task_infos.getAbortingNode(), task.getUUID()); + } +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerErrorTaskTests) // NOLINT +{ + { // Construction + ErrorTask task; + EXPECT_EQ(task.getName(), "ErrorTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + ErrorTask task("abc", true); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + ErrorTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Serialization + auto task = std::make_unique("abc", true); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerErrorTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + ErrorTask task; + EXPECT_EQ(task.run(*input), 0); + auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->message, "Error"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(input->isAborted(), true); + EXPECT_EQ(input->isSuccessful(), false); + EXPECT_EQ(input->task_infos.getAbortingNode(), task.getUUID()); + } +} + +TEST(TesseractTaskComposerCoreUnit, TaskComposerDoneTaskTests) // NOLINT +{ + { // Construction + DoneTask task; + EXPECT_EQ(task.getName(), "DoneTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + DoneTask task("abc", true); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + DoneTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + } + + { // Serialization + auto task = std::make_unique("abc", true); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerDoneTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + DoneTask 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, TaskComposerStartTaskTests) // NOLINT +{ + { // Construction + StartTask task; + EXPECT_EQ(task.getName(), "StartTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false)"; + YAML::Node config = YAML::Load(str); + StartTask 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, "TaskComposerDoneTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + StartTask 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()); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp index 920fe36f0d..cf56421714 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp @@ -34,11 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_planning; -void runTaskComposerFactoryTest(const tesseract_common::fs::path& config_path) +void runTaskComposerFactoryTest(TaskComposerPluginFactory& factory, YAML::Node plugin_config) { - TaskComposerPluginFactory factory(config_path); - YAML::Node plugin_config = YAML::LoadFile(config_path.string()); - const YAML::Node& plugin_info = plugin_config[tesseract_common::TaskComposerPluginInfo::CONFIG_KEY]; const YAML::Node& search_paths = plugin_info["search_paths"]; const YAML::Node& search_libraries = plugin_info["search_libraries"]; @@ -110,14 +107,46 @@ void runTaskComposerFactoryTest(const tesseract_common::fs::path& config_path) TEST(TesseractTaskComposerFactoryUnit, LoadAndExportPluginTest) // NOLINT { - tesseract_common::fs::path config_path(std::string(TESSERACT_TASK_COMPOSER_DIR) + "/config/" - "task_composer_plugins.yaml"); - runTaskComposerFactoryTest(config_path); + { // File Path Construction + tesseract_common::fs::path config_path(std::string(TESSERACT_TASK_COMPOSER_DIR) + "/config/" + "task_composer_plugins.yaml"); + TaskComposerPluginFactory factory(config_path); + YAML::Node plugin_config = YAML::LoadFile(config_path.string()); + runTaskComposerFactoryTest(factory, plugin_config); + + auto export_config_path = tesseract_common::fs::path(tesseract_common::getTempPath()) / "task_composer_plugins_" + "export.yaml"; + TaskComposerPluginFactory check_factory(export_config_path); + runTaskComposerFactoryTest(check_factory, plugin_config); + } + + { // String Constructor + tesseract_common::fs::path config_path(std::string(TESSERACT_TASK_COMPOSER_DIR) + "/config/" + "task_composer_plugins.yaml"); + + TaskComposerPluginFactory factory(tesseract_common::fileToString(config_path)); + YAML::Node plugin_config = YAML::LoadFile(config_path.string()); + runTaskComposerFactoryTest(factory, plugin_config); - auto export_config_path = tesseract_common::fs::path(tesseract_common::getTempPath()) / "task_composer_plugins_" - "export.yaml"; + auto export_config_path = tesseract_common::fs::path(tesseract_common::getTempPath()) / "task_composer_plugins_" + "export.yaml"; + TaskComposerPluginFactory check_factory(export_config_path); + runTaskComposerFactoryTest(check_factory, plugin_config); + } + + { // YAML Node Constructor + tesseract_common::fs::path config_path(std::string(TESSERACT_TASK_COMPOSER_DIR) + "/config/" + "task_composer_plugins.yaml"); - runTaskComposerFactoryTest(export_config_path); + YAML::Node plugin_config = YAML::LoadFile(config_path.string()); + TaskComposerPluginFactory factory(plugin_config); + runTaskComposerFactoryTest(factory, plugin_config); + + auto export_config_path = tesseract_common::fs::path(tesseract_common::getTempPath()) / "task_composer_plugins_" + "export.yaml"; + TaskComposerPluginFactory check_factory(export_config_path); + runTaskComposerFactoryTest(check_factory, plugin_config); + } } TEST(TesseractTaskComposerFactoryUnit, PluginFactorAPIUnit) // NOLINT