diff --git a/tesseract_task_composer/README.rst b/tesseract_task_composer/README.rst index ab3d3fb59c2..361fbe1af01 100644 --- a/tesseract_task_composer/README.rst +++ b/tesseract_task_composer/README.rst @@ -108,6 +108,22 @@ Yaml Config: Task Composer Task Plugins -------------------------- +Task +^^^^ + +All tasks have the following config entries available. + +.. code-block:: yaml + + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + trigger_abort: true # default for task is false + inputs: [input_data] + outputs: [output_data] + + Graph Task ^^^^^^^^^^ @@ -182,6 +198,8 @@ When using a perviously defined task it is referenced using `task:` instead of ` Also in most case the tasks inputs and sometimes the outputs must be renamed. This accomplished by leveraging the `input_remapping:` and `output_remapping:`. +Also you can indicate that it should abort if a terminal is reached by specifying the terminal index `abort_terminal:`. If set to anything less than zero it will set all terminal tasks trigger abort to `false`. + .. code-block:: yaml UsePreviouslyDefinedTaskPipeline: @@ -199,10 +217,11 @@ Also in most case the tasks inputs and sometimes the outputs must be renamed. Th CartesianPipelineTask: task: CartesianPipeline config: - conditional: false - input_remapping: + conditional: false # Optional + abort_terminal: 0 # Optional + input_remapping: # Optional input_data: output_data - output_remapping: + output_remapping: # Optional output_data: output_data edges: - source: MinLengthTask diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index ba40bb077ba..dabd5fc4f69 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml +++ b/tesseract_task_composer/config/task_composer_plugins.yaml @@ -66,26 +66,23 @@ task_composer_plugins: DescartesFPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesFTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesDTask: class: PipelineTaskFactory config: @@ -138,26 +135,23 @@ task_composer_plugins: DescartesDPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesDTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesFNPCTask: class: PipelineTaskFactory config: @@ -203,26 +197,23 @@ task_composer_plugins: DescartesFNPCPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesFNPCTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesDNPCTask: class: PipelineTaskFactory config: @@ -268,26 +259,23 @@ task_composer_plugins: DescartesDNPCPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesDNPCTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] OMPLTask: class: PipelineTaskFactory config: @@ -340,26 +328,23 @@ task_composer_plugins: OMPLPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: OMPLTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] TrajOptTask: class: PipelineTaskFactory config: @@ -412,26 +397,23 @@ task_composer_plugins: TrajOptPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: TrajOptTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] TrajOptIfoptTask: class: PipelineTaskFactory config: @@ -484,26 +466,23 @@ task_composer_plugins: TrajOptIfoptPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: TrajOptIfoptTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] CartesianTask: class: PipelineTaskFactory config: @@ -565,26 +544,23 @@ task_composer_plugins: CartesianPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: CartesianTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] FreespaceTask: class: PipelineTaskFactory config: @@ -646,26 +622,23 @@ task_composer_plugins: FreespacePipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: FreespaceTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtTask: class: PipelineTaskFactory config: @@ -695,22 +668,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -724,26 +700,23 @@ task_composer_plugins: RasterFtPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtTask: class: PipelineTaskFactory config: @@ -773,22 +746,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -802,26 +778,23 @@ task_composer_plugins: RasterCtPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtOnlyTask: class: PipelineTaskFactory config: @@ -851,15 +824,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -873,26 +848,23 @@ task_composer_plugins: RasterFtOnlyPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtOnlyTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtOnlyTask: class: PipelineTaskFactory config: @@ -922,15 +894,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -944,26 +918,23 @@ task_composer_plugins: RasterCtOnlyPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtOnlyTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtGlobalTask: class: PipelineTaskFactory config: @@ -1000,22 +971,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1031,26 +1005,23 @@ task_composer_plugins: RasterFtGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtGlobalTask: class: PipelineTaskFactory config: @@ -1087,22 +1058,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1118,26 +1092,23 @@ task_composer_plugins: RasterCtGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtOnlyGlobalTask: class: PipelineTaskFactory config: @@ -1174,15 +1145,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1198,26 +1171,23 @@ task_composer_plugins: RasterFtOnlyGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtOnlyGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtOnlyGlobalTask: class: PipelineTaskFactory config: @@ -1254,15 +1224,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1278,23 +1250,20 @@ task_composer_plugins: RasterCtOnlyGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtOnlyGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] diff --git a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml index 2a54e53b097..62b0bd9e8d6 100644 --- a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml +++ b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml @@ -66,26 +66,23 @@ task_composer_plugins: DescartesFPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesFTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesDTask: class: PipelineTaskFactory config: @@ -138,26 +135,23 @@ task_composer_plugins: DescartesDPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesDTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesFNPCTask: class: PipelineTaskFactory config: @@ -203,26 +197,23 @@ task_composer_plugins: DescartesFNPCPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesFNPCTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] DescartesDNPCTask: class: PipelineTaskFactory config: @@ -268,26 +259,23 @@ task_composer_plugins: DescartesDNPCPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: DescartesDNPCTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] OMPLTask: class: PipelineTaskFactory config: @@ -340,26 +328,23 @@ task_composer_plugins: OMPLPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: OMPLTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] TrajOptTask: class: PipelineTaskFactory config: @@ -412,26 +397,23 @@ task_composer_plugins: TrajOptPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: TrajOptTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] CartesianTask: class: PipelineTaskFactory config: @@ -493,26 +475,23 @@ task_composer_plugins: CartesianPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: CartesianTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] FreespaceTask: class: PipelineTaskFactory config: @@ -574,26 +553,23 @@ task_composer_plugins: FreespacePipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: FreespaceTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtTask: class: PipelineTaskFactory config: @@ -623,22 +599,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -652,26 +631,23 @@ task_composer_plugins: RasterFtPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtTask: class: PipelineTaskFactory config: @@ -701,22 +677,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -730,26 +709,23 @@ task_composer_plugins: RasterCtPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtOnlyTask: class: PipelineTaskFactory config: @@ -779,15 +755,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -801,26 +779,23 @@ task_composer_plugins: RasterFtOnlyPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtOnlyTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtOnlyTask: class: PipelineTaskFactory config: @@ -850,15 +825,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -872,26 +849,23 @@ task_composer_plugins: RasterCtOnlyPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtOnlyTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtGlobalTask: class: PipelineTaskFactory config: @@ -928,22 +902,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -959,26 +936,23 @@ task_composer_plugins: RasterFtGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtGlobalTask: class: PipelineTaskFactory config: @@ -1015,22 +989,25 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] freespace: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1046,26 +1023,23 @@ task_composer_plugins: RasterCtGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtOnlyGlobalTask: class: PipelineTaskFactory config: @@ -1102,15 +1076,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: FreespacePipeline + task: FreespaceTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1126,26 +1102,23 @@ task_composer_plugins: RasterFtOnlyGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterFtOnlyGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterCtOnlyGlobalTask: class: PipelineTaskFactory config: @@ -1182,15 +1155,17 @@ task_composer_plugins: inputs: [output_data] outputs: [output_data] raster: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] output_indexing: [output_data] transition: - task: CartesianPipeline + task: CartesianTask config: + abort_terminal: 0 input_remapping: input_data: output_data input_indexing: [output_data] @@ -1206,23 +1181,20 @@ task_composer_plugins: RasterCtOnlyGlobalPipeline: class: PipelineTaskFactory config: - conditional: true - inputs: [input_data] + conditional: false outputs: [output_data] nodes: - DoneTask: - class: DoneTaskFactory - config: - conditional: false - AbortTask: - class: AbortTaskFactory + ProcessInputTask: + class: ProcessPlanningInputTaskFactory config: conditional: false + outputs: [input_data] MotionPlanningTask: task: RasterCtOnlyGlobalTask config: - conditional: true + conditional: false + abort_terminal: 0 edges: - - source: MotionPlanningTask - destinations: [AbortTask, DoneTask] - terminals: [AbortTask, DoneTask] + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h index 8f5b76bc991..c0dedd70dc9 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h @@ -58,6 +58,7 @@ struct TaskComposerContext using ConstUPtr = std::unique_ptr; TaskComposerContext() = default; // Required for serialization + TaskComposerContext(TaskComposerProblem::Ptr problem); TaskComposerContext(TaskComposerProblem::Ptr problem, TaskComposerDataStorage::Ptr data_storage); TaskComposerContext(const TaskComposerContext&) = delete; TaskComposerContext(TaskComposerContext&&) noexcept = delete; 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 f2733ffce6a..906c3a425e6 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 @@ -63,7 +63,7 @@ class TaskComposerExecutor */ TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerProblem::Ptr problem, - TaskComposerDataStorage::Ptr data_storage); + TaskComposerDataStorage::Ptr data_storage = std::make_shared()); /** @brief Queries the number of workers (example: number of threads) */ virtual long getWorkerCount() const = 0; 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 9bb57e759aa..4a3b106f3e3 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 @@ -96,6 +96,20 @@ class TaskComposerGraph : public TaskComposerNode /** @brief Get the terminal nodes for the pipeline */ std::vector getTerminals() const; + /** + * @brief Set if the pipeline should call abort if terminal node is reached + * @details If null, it will not abort + * @param terminal The terminal uuid + */ + void setTerminalTriggerAbort(boost::uuids::uuid terminal); + + /** + * @brief Set if the pipeline should call abort if terminal node is reached + * @details If less than zero, it will not abort + * @param terminal The terminal index + */ + void setTerminalTriggerAbortByIndex(int terminal_index); + void renameInputKeys(const std::map& input_keys) override; void renameOutputKeys(const std::map& output_keys) override; 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 0070a31eab4..ad2aa6f09df 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 @@ -62,9 +62,18 @@ class TaskComposerTask : public TaskComposerNode bool operator==(const TaskComposerTask& rhs) const; bool operator!=(const TaskComposerTask& rhs) const; + /** + * @brief If true this node should call context.abort(uuid_) after run method returns + * @param enable True if task should call context.abort(uuid_) after run method returns + */ + void setTriggerAbort(bool enable); + int run(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; protected: + /** @brief Indicate if task triggers abort */ + bool trigger_abort_{ false }; + friend struct tesseract_common::Serialization; friend class boost::serialization::access; diff --git a/tesseract_task_composer/core/src/task_composer_context.cpp b/tesseract_task_composer/core/src/task_composer_context.cpp index 08af8df248e..50aca058fbe 100644 --- a/tesseract_task_composer/core/src/task_composer_context.cpp +++ b/tesseract_task_composer/core/src/task_composer_context.cpp @@ -39,6 +39,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +TaskComposerContext::TaskComposerContext(TaskComposerProblem::Ptr problem) + : TaskComposerContext(std::move(problem), std::make_shared()) +{ +} + TaskComposerContext::TaskComposerContext(tesseract_planning::TaskComposerProblem::Ptr problem, TaskComposerDataStorage::Ptr data_storage) : problem(std::move(problem)), data_storage(std::move(data_storage)) diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index d8784363737..38f8045f484 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -100,6 +100,15 @@ TaskComposerGraph::TaskComposerGraph(std::string name, if (YAML::Node n = tc["conditional"]) task_node->setConditional(n.as()); + if (YAML::Node n = tc["abort_terminal"]) + { + if (task_node->getType() != TaskComposerNodeType::GRAPH || + task_node->getType() == TaskComposerNodeType::PIPELINE) + static_cast(*task_node).setTerminalTriggerAbortByIndex(n.as()); + else + throw std::runtime_error("YAML entry 'abort_terminal' is only supported for GRAPH and PIPELINE types"); + } + if (YAML::Node n = tc["input_remapping"]) task_node->renameInputKeys(n.as>()); @@ -233,6 +242,48 @@ void TaskComposerGraph::setTerminals(std::vector terminals) std::vector TaskComposerGraph::getTerminals() const { return terminals_; } +void TaskComposerGraph::setTerminalTriggerAbort(boost::uuids::uuid terminal) +{ + if (!terminal.is_nil()) + { + auto& n = nodes_.at(terminal); + if (n->getType() == TaskComposerNodeType::TASK) + static_cast(*n).setTriggerAbort(true); + else + throw std::runtime_error("Tasks can only trigger abort!"); + } + else + { + for (const auto& terminal : terminals_) + { + auto& n = nodes_.at(terminal); + if (n->getType() == TaskComposerNodeType::TASK) + static_cast(*n).setTriggerAbort(false); + } + } +} + +void TaskComposerGraph::setTerminalTriggerAbortByIndex(int terminal_index) +{ + if (terminal_index >= 0) + { + auto& n = nodes_.at(terminals_.at(static_cast(terminal_index))); + if (n->getType() == TaskComposerNodeType::TASK) + static_cast(*n).setTriggerAbort(true); + else + throw std::runtime_error("Tasks can only trigger abort!"); + } + else + { + for (const auto& terminal : terminals_) + { + auto& n = nodes_.at(terminal); + if (n->getType() == TaskComposerNodeType::TASK) + static_cast(*n).setTriggerAbort(false); + } + } +} + void TaskComposerGraph::renameInputKeys(const std::map& input_keys) { for (const auto& key : input_keys) diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp index d919d5d0ddf..029b7774d22 100644 --- a/tesseract_task_composer/core/src/task_composer_task.cpp +++ b/tesseract_task_composer/core/src/task_composer_task.cpp @@ -44,8 +44,20 @@ TaskComposerTask::TaskComposerTask(std::string name, bool conditional) TaskComposerTask::TaskComposerTask(std::string name, const YAML::Node& config) : TaskComposerNode(std::move(name), TaskComposerNodeType::TASK, config) { + try + { + if (YAML::Node n = config["trigger_abort"]) + trigger_abort_ = n.as(); + } + catch (const std::exception& e) + { + throw std::runtime_error("TaskComposerTask: Failed to parse yaml config entry 'trigger_abort'! Details: " + + std::string(e.what())); + } } +void TaskComposerTask::setTriggerAbort(bool enable) { trigger_abort_ = enable; } + int TaskComposerTask::run(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { if (context.isAborted()) @@ -82,11 +94,25 @@ int TaskComposerTask::run(TaskComposerContext& context, OptionalTaskComposerExec int value = results->return_value; assert(value >= 0); + + // Call abort if required + if (trigger_abort_ && !context.isAborted()) + { + results->message += " (Abort Triggered)"; + context.abort(uuid_); + } + context.task_infos.addInfo(std::move(results)); return value; } -bool TaskComposerTask::operator==(const TaskComposerTask& rhs) const { return (TaskComposerNode::operator==(rhs)); } +bool TaskComposerTask::operator==(const TaskComposerTask& rhs) const +{ + bool equal{ true }; + equal &= trigger_abort_ == rhs.trigger_abort_; + equal &= (TaskComposerNode::operator==(rhs)); + return equal; +} // LCOV_EXCL_START bool TaskComposerTask::operator!=(const TaskComposerTask& rhs) const { return !operator==(rhs); } @@ -95,6 +121,7 @@ bool TaskComposerTask::operator!=(const TaskComposerTask& rhs) const { return !o template void TaskComposerTask::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("trigger_abort", trigger_abort_); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerNode); } diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 460b1516a0e..8b0f18b4639 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -43,6 +43,7 @@ set(LIB_SOURCE_FILES src/nodes/profile_switch_task.cpp src/nodes/min_length_task.cpp src/nodes/motion_planner_task_info.cpp + src/nodes/process_planning_input_task.cpp src/nodes/update_end_state_task.cpp src/nodes/update_start_and_end_state_task.cpp src/nodes/update_start_state_task.cpp diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/process_planning_input_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/process_planning_input_task.h new file mode 100644 index 00000000000..578860e7ab6 --- /dev/null +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/process_planning_input_task.h @@ -0,0 +1,79 @@ +/** + * @file process_planning_input_instruction.h + * + * @brief This cast the input instruction to a CompositeInstruciton and stores under the key in data_storage. + * + * @author Levi Armstrong + * @date September 17. 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Plectix Robotics + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_TASK_COMPOSER_PROCESS_PLANNING_INPUT_TASK_H +#define TESSERACT_TASK_COMPOSER_PROCESS_PLANNING_INPUT_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; +class ProcessPlanningInputTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + ProcessPlanningInputTask(); + explicit ProcessPlanningInputTask(std::string name, std::string output_key, bool conditional = false); + explicit ProcessPlanningInputTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory); + + ~ProcessPlanningInputTask() override = default; + ProcessPlanningInputTask(const ProcessPlanningInputTask&) = delete; + ProcessPlanningInputTask& operator=(const ProcessPlanningInputTask&) = delete; + ProcessPlanningInputTask(ProcessPlanningInputTask&&) = delete; + ProcessPlanningInputTask& operator=(ProcessPlanningInputTask&&) = delete; + + bool operator==(const ProcessPlanningInputTask& rhs) const; + bool operator!=(const ProcessPlanningInputTask& 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(TaskComposerContext& context, + OptionalTaskComposerExecutor executor = std::nullopt) const override final; +}; + +} // namespace tesseract_planning + +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::ProcessPlanningInputTask, "ProcessPlanningInputTask") + +#endif // TESSERACT_TASK_COMPOSER_PROCESS_PLANNING_INPUT_TASK_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h index 2d551429eef..3463fb94a65 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -85,6 +86,9 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem /** @brief The Profiles to use */ ProfileDictionary::ConstPtr profiles; + /** @brief The problem input instruction */ + InstructionPoly input_instruction; + /** * @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a * given motion planner. diff --git a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp index 7efa13bef1a..b8a805ee7d8 100644 --- a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp +++ b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include @@ -56,6 +57,7 @@ using UpsampleTrajectoryTaskFactory = TaskComposerTaskFactory; using RasterOnlyMotionTaskFactory = TaskComposerTaskFactory; using SimpleMotionPlannerTaskFactory = TaskComposerTaskFactory>; +using ProcessPlanningInputTaskFactory = TaskComposerTaskFactory; // LCOV_EXCL_START TESSERACT_PLUGIN_ANCHOR_IMPL(TaskComposerPlanningFactoriesAnchor) @@ -91,3 +93,6 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::RasterOnlyMotionTask // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::SimpleMotionPlannerTaskFactory, SimpleMotionPlannerTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ProcessPlanningInputTaskFactory, + ProcessPlanningInputTaskFactory) diff --git a/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp new file mode 100644 index 00000000000..bfb37b4be7d --- /dev/null +++ b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp @@ -0,0 +1,104 @@ +/** + * @file check_input_task.cpp + * @brief Task for checking input data structure + * + * @author Levi Armstrong + * @date November 2. 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @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 +{ +ProcessPlanningInputTask::ProcessPlanningInputTask() : TaskComposerTask("ProcessPlanningInputInstructionTask", false) {} + +ProcessPlanningInputTask::ProcessPlanningInputTask(std::string name, std::string output_key, bool is_conditional) + : TaskComposerTask(std::move(name), is_conditional) +{ + output_keys_ = { std::move(output_key) }; +} + +ProcessPlanningInputTask::ProcessPlanningInputTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ + if (!input_keys_.empty()) + throw std::runtime_error("ProcessPlanningInputTask, config should not have 'inputs' entry"); + + if (output_keys_.empty()) + throw std::runtime_error("ProcessPlanningInputTask, config missing 'inputs' entry"); + + if (output_keys_.size() != 1) + throw std::runtime_error("ProcessPlanningInputTask, 'outputs' should only have one key"); +} + +TaskComposerNodeInfo::UPtr ProcessPlanningInputTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const +{ + // Get the problem + auto& problem = dynamic_cast(*context.problem); + + auto info = std::make_unique(*this); + + if (!problem.input_instruction.isCompositeInstruction()) + { + info->color = "red"; + info->message = "Input instructon is not a Composite Instruction, aborting..."; + info->return_value = 0; + + // Abort + context.abort(uuid_); + return info; + } + + context.data_storage->setData(output_keys_[0], problem.input_instruction.as()); + + info->color = "green"; + info->message = "Successful"; + info->return_value = 1; + return info; +} + +bool ProcessPlanningInputTask::operator==(const ProcessPlanningInputTask& rhs) const +{ + return (TaskComposerTask::operator==(rhs)); +} +bool ProcessPlanningInputTask::operator!=(const ProcessPlanningInputTask& rhs) const { return !operator==(rhs); } + +template +void ProcessPlanningInputTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProcessPlanningInputTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProcessPlanningInputTask) 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 e73314e9766..ad390c9201d 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -53,7 +53,6 @@ createTask(const std::string& name, tesseract_planning::RasterMotionTask::TaskFactoryResults tf_results; tf_results.node = plugin_factory.createTaskComposerNode(task_name); tf_results.node->setName(name); - if (!input_remapping.empty()) tf_results.node->renameInputKeys(input_remapping); @@ -136,6 +135,8 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node freespace_config = config["freespace"]) { std::string task_name; + bool has_abort_terminal_entry{ false }; + int abort_terminal_index{ -1 }; std::vector input_indexing; std::vector output_indexing; std::map input_remapping; @@ -148,6 +149,12 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node task_config = freespace_config["config"]) { + if (YAML::Node n = task_config["abort_terminal"]) + { + has_abort_terminal_entry = true; + abort_terminal_index = n.as(); + } + if (YAML::Node n = task_config["input_remapping"]) input_remapping = n.as>(); @@ -169,15 +176,33 @@ RasterMotionTask::RasterMotionTask(std::string name, throw std::runtime_error("RasterMotionTask, entry 'freespace' missing 'config' entry"); } - freespace_task_factory_ = [task_name, - input_remapping, - output_remapping, - input_indexing, - output_indexing, - &plugin_factory](const std::string& name, std::size_t index) { - return createTask( - name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); - }; + if (has_abort_terminal_entry) + { + freespace_task_factory_ = [task_name, + abort_terminal_index, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + auto tr = createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + static_cast(*tr.node).setTerminalTriggerAbortByIndex(abort_terminal_index); + return tr; + }; + } + else + { + freespace_task_factory_ = [task_name, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + return createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + }; + } } else { @@ -187,6 +212,8 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node raster_config = config["raster"]) { std::string task_name; + bool has_abort_terminal_entry{ false }; + int abort_terminal_index{ -1 }; std::vector input_indexing; std::vector output_indexing; std::map input_remapping; @@ -199,6 +226,12 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node task_config = raster_config["config"]) { + if (YAML::Node n = task_config["abort_terminal"]) + { + has_abort_terminal_entry = true; + abort_terminal_index = n.as(); + } + if (YAML::Node n = task_config["input_remapping"]) input_remapping = n.as>(); @@ -220,15 +253,33 @@ RasterMotionTask::RasterMotionTask(std::string name, throw std::runtime_error("RasterMotionTask, entry 'raster' missing 'config' entry"); } - raster_task_factory_ = [task_name, - input_remapping, - output_remapping, - input_indexing, - output_indexing, - &plugin_factory](const std::string& name, std::size_t index) { - return createTask( - name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); - }; + if (has_abort_terminal_entry) + { + raster_task_factory_ = [task_name, + abort_terminal_index, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + auto tr = createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + static_cast(*tr.node).setTerminalTriggerAbortByIndex(abort_terminal_index); + return tr; + }; + } + else + { + raster_task_factory_ = [task_name, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + return createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + }; + } } else { @@ -238,6 +289,8 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node transition_config = config["transition"]) { std::string task_name; + bool has_abort_terminal_entry{ false }; + int abort_terminal_index{ -1 }; std::vector input_indexing; std::vector output_indexing; std::map input_remapping; @@ -250,6 +303,12 @@ RasterMotionTask::RasterMotionTask(std::string name, if (YAML::Node task_config = transition_config["config"]) { + if (YAML::Node n = task_config["abort_terminal"]) + { + has_abort_terminal_entry = true; + abort_terminal_index = n.as(); + } + if (YAML::Node n = task_config["input_remapping"]) input_remapping = n.as>(); @@ -271,15 +330,33 @@ RasterMotionTask::RasterMotionTask(std::string name, throw std::runtime_error("RasterMotionTask, entry 'transition' missing 'config' entry"); } - transition_task_factory_ = [task_name, - input_remapping, - output_remapping, - input_indexing, - output_indexing, - &plugin_factory](const std::string& name, std::size_t index) { - return createTask( - name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); - }; + if (has_abort_terminal_entry) + { + transition_task_factory_ = [task_name, + abort_terminal_index, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + auto tr = createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + static_cast(*tr.node).setTerminalTriggerAbortByIndex(abort_terminal_index); + return tr; + }; + } + else + { + transition_task_factory_ = [task_name, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + return createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + }; + } } else { 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 5f9e2fabad2..8360eb47402 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 @@ -134,6 +134,8 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, if (YAML::Node raster_config = config["raster"]) { std::string task_name; + bool has_abort_terminal_entry{ false }; + int abort_terminal_index{ -1 }; std::vector input_indexing; std::vector output_indexing; std::map input_remapping; @@ -146,6 +148,12 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, if (YAML::Node task_config = raster_config["config"]) { + if (YAML::Node n = task_config["abort_terminal"]) + { + has_abort_terminal_entry = true; + abort_terminal_index = n.as(); + } + if (YAML::Node n = task_config["input_remapping"]) input_remapping = n.as>(); @@ -167,15 +175,35 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, throw std::runtime_error("RasterOnlyMotionTask, entry 'raster' missing 'config' entry"); } - raster_task_factory_ = [task_name, - input_remapping, - output_remapping, - input_indexing, - output_indexing, - &plugin_factory](const std::string& name, std::size_t index) { - return createTask( - name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); - }; + if (has_abort_terminal_entry) + { + raster_task_factory_ = [task_name, + abort_terminal_index, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + auto tr = createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + + static_cast(*tr.node).setTerminalTriggerAbortByIndex(abort_terminal_index); + + return tr; + }; + } + else + { + raster_task_factory_ = [task_name, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + return createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + }; + } } else { @@ -185,6 +213,8 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, if (YAML::Node transition_config = config["transition"]) { std::string task_name; + bool has_abort_terminal_entry{ false }; + int abort_terminal_index{ -1 }; std::vector input_indexing; std::vector output_indexing; std::map input_remapping; @@ -197,6 +227,12 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, if (YAML::Node task_config = transition_config["config"]) { + if (YAML::Node n = task_config["abort_terminal"]) + { + has_abort_terminal_entry = true; + abort_terminal_index = n.as(); + } + if (YAML::Node n = task_config["input_remapping"]) input_remapping = n.as>(); @@ -218,15 +254,35 @@ RasterOnlyMotionTask::RasterOnlyMotionTask(std::string name, throw std::runtime_error("RasterOnlyMotionTask, entry 'transition' missing 'config' entry"); } - transition_task_factory_ = [task_name, - input_remapping, - output_remapping, - input_indexing, - output_indexing, - &plugin_factory](const std::string& name, std::size_t index) { - return createTask( - name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); - }; + if (has_abort_terminal_entry) + { + transition_task_factory_ = [task_name, + abort_terminal_index, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + auto tr = createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + + static_cast(*tr.node).setTerminalTriggerAbortByIndex(abort_terminal_index); + + return tr; + }; + } + else + { + transition_task_factory_ = [task_name, + input_remapping, + output_remapping, + input_indexing, + output_indexing, + &plugin_factory](const std::string& name, std::size_t index) { + return createTask( + name, task_name, input_remapping, output_remapping, input_indexing, output_indexing, plugin_factory, index); + }; + } } else { diff --git a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp index c453aafc862..9af8313b4ea 100644 --- a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp +++ b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp @@ -102,6 +102,7 @@ bool PlanningTaskComposerProblem::operator==(const PlanningTaskComposerProblem& equal &= tesseract_common::pointersEqual(env, rhs.env); equal &= manip_info == rhs.manip_info; // equal &= tesseract_common::pointersEqual(profiles, rhs.profiles); + equal &= input_instruction == rhs.input_instruction; equal &= move_profile_remapping == rhs.move_profile_remapping; equal &= composite_profile_remapping == rhs.composite_profile_remapping; return equal; @@ -117,6 +118,7 @@ void PlanningTaskComposerProblem::serialize(Archive& ar, const unsigned int /*ve ar& boost::serialization::make_nvp("manip_info", manip_info); /** @todo Fix after profiles are serializable */ // ar& boost::serialization::make_nvp("profiles", profiles); + ar& boost::serialization::make_nvp("input_instruction", input_instruction); ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping); ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping); } diff --git a/tesseract_task_composer/test/CMakeLists.txt b/tesseract_task_composer/test/CMakeLists.txt index ca4eafe05a4..ac78c3a9838 100644 --- a/tesseract_task_composer/test/CMakeLists.txt +++ b/tesseract_task_composer/test/CMakeLists.txt @@ -131,14 +131,9 @@ if(TESSERACT_BUILD_TASK_COMPOSER_TASKFLOW) endif(TESSERACT_BUILD_TASK_COMPOSER_TASKFLOW) # Planning Tests -if(TESSERACT_BUILD_TASK_COMPOSER_PLANNING - AND tesseract_motion_planners_trajopt_FOUND - AND tesseract_motion_planners_trajopt_ifopt_FOUND - AND tesseract_motion_planners_descartes_FOUND - AND tesseract_motion_planners_ompl_FOUND - AND tesseract_motion_planners_isp_FOUND - AND tesseract_motion_planners_totg_FOUND - AND tesseract_motion_planners_ruckig_FOUND) +if(TESSERACT_BUILD_TASK_COMPOSER_PLANNING) + find_package(tesseract_motion_planners REQUIRED COMPONENTS core simple OPTIONAL_COMPONENTS trajopt) + add_executable(${PROJECT_NAME}_planning_unit ${PROJECT_NAME}_planning_unit.cpp) target_link_libraries( ${PROJECT_NAME}_planning_unit @@ -146,7 +141,8 @@ if(TESSERACT_BUILD_TASK_COMPOSER_PLANNING GTest::Main ${PROJECT_NAME} ${PROJECT_NAME}_nodes - ${PROJECT_NAME}_planning_nodes) + ${PROJECT_NAME}_planning_nodes + tesseract::tesseract_motion_planners_trajopt) target_include_directories(${PROJECT_NAME}_planning_unit PUBLIC "$") target_compile_options(${PROJECT_NAME}_planning_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 51e8040b8ea..81935c99392 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -85,16 +85,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // { // Construction auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); - PlanningTaskComposerProblem problem(data, profiles, "abc"); + PlanningTaskComposerProblem problem(profiles, "abc"); EXPECT_EQ(problem.name, "abc"); EXPECT_TRUE(problem.env == nullptr); EXPECT_TRUE(problem.manip_info.empty()); EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_data.hasKey("input_data")); + EXPECT_TRUE(problem.input_instruction.isNull()); + problem.input_instruction = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input_instruction.isNull()); // Test clone auto clone = problem.clone(); @@ -104,16 +104,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // { // Construction auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); - PlanningTaskComposerProblem problem(env_, manip_, data, profiles, "abc"); + PlanningTaskComposerProblem problem(env_, manip_, profiles, "abc"); EXPECT_EQ(problem.name, "abc"); EXPECT_TRUE(problem.env != nullptr); EXPECT_FALSE(problem.manip_info.empty()); EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_data.hasKey("input_data")); + EXPECT_TRUE(problem.input_instruction.isNull()); + problem.input_instruction = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input_instruction.isNull()); // Test clone auto clone = problem.clone(); @@ -123,16 +123,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // { // Construction auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); - PlanningTaskComposerProblem problem(env_, manip_, ProfileRemapping(), ProfileRemapping(), data, profiles, "abc"); + PlanningTaskComposerProblem problem(env_, manip_, ProfileRemapping(), ProfileRemapping(), profiles, "abc"); EXPECT_EQ(problem.name, "abc"); EXPECT_TRUE(problem.env != nullptr); EXPECT_FALSE(problem.manip_info.empty()); EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_data.hasKey("input_data")); + EXPECT_TRUE(problem.input_instruction.isNull()); + problem.input_instruction = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input_instruction.isNull()); // Test clone auto clone = problem.clone(); @@ -142,16 +142,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // { // Construction auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); - PlanningTaskComposerProblem problem(env_, ProfileRemapping(), ProfileRemapping(), data, profiles, "abc"); + PlanningTaskComposerProblem problem(env_, ProfileRemapping(), ProfileRemapping(), profiles, "abc"); EXPECT_EQ(problem.name, "abc"); EXPECT_TRUE(problem.env != nullptr); EXPECT_TRUE(problem.manip_info.empty()); EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_data.hasKey("input_data")); + EXPECT_TRUE(problem.input_instruction.isNull()); + problem.input_instruction = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input_instruction.isNull()); // Test clone auto clone = problem.clone(); @@ -217,34 +217,34 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::freespaceExampleProgramABB()); auto problem = - std::make_unique(env_, manip_, data, profiles, "TaskComposerCheckInputTaskTests"); - auto input = std::make_unique(std::move(problem)); + std::make_unique(env_, manip_, profiles, "TaskComposerCheckInputTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->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()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::freespaceExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::freespaceExampleProgramABB()); auto problem = - std::make_unique(env_, manip_, data, profiles, "TaskComposerCheckInputTaskTests"); + std::make_unique(env_, manip_, profiles, "TaskComposerCheckInputTaskTests"); problem->env = nullptr; - auto context = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem), std::move(data)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); @@ -256,20 +256,20 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble { // Failure auto profiles = std::make_shared(); - TaskComposerDataStorage data; + auto data = std::make_unique(); auto problem = - std::make_unique(env_, manip_, data, profiles, "TaskComposerCheckInputTaskTests"); - auto context = std::make_unique(std::move(problem)); + std::make_unique(env_, manip_, profiles, "TaskComposerCheckInputTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -323,21 +323,21 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); - auto context = std::make_unique(std::move(problem)); + env_, manip_, profiles, "TaskComposerContinuousContactCheckTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); ContinuousContactCheckTask task("TaskComposerContinuousContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization test_suite::runSerializationTest(dynamic_cast(*node_info), @@ -346,20 +346,19 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); + auto problem = + std::make_unique(env_, profiles, "TaskComposerContinuousContactCheckTaskTests"); auto context = std::make_unique(std::move(problem)); ContinuousContactCheckTask task("TaskComposerContinuousContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure collision @@ -371,22 +370,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask profiles->addProfile( "TaskComposerContinuousContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); - auto context = std::make_unique(std::move(problem)); + env_, manip_, profiles, "TaskComposerContinuousContactCheckTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); ContinuousContactCheckTask task("TaskComposerContinuousContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(dynamic_cast(*node_info).contact_results.empty(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -440,21 +439,21 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto context = std::make_unique(std::move(problem)); + env_, manip_, profiles, "TaskComposerDiscreteContactCheckTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization test_suite::runSerializationTest(dynamic_cast(*node_info), @@ -463,20 +462,20 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; + auto data = std::make_unique(); auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto context = std::make_unique(std::move(problem)); + env_, manip_, profiles, "TaskComposerDiscreteContactCheckTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure collision @@ -488,22 +487,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe profiles->addProfile( "TaskComposerDiscreteContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( - env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto context = std::make_unique(std::move(problem)); + env_, manip_, profiles, "TaskComposerDiscreteContactCheckTaskTests"); + auto context = std::make_unique(std::move(problem), std::move(data)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(dynamic_cast(*node_info).contact_results.empty(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -589,28 +588,28 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + data->setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data2"), context->data_storage->getData("input_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method with cartesian in input auto profiles = std::make_shared(); - TaskComposerDataStorage data; + auto data = std::make_unique(); auto input_program = test_suite::jointInterpolateExampleProgramABB(true); // Modify to have cartesian waypoint @@ -623,27 +622,27 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / EXPECT_EQ(input_program.size(), 2); // Set input data - data.setData("input_data", input_program); - data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + data->setData("input_data", input_program); + data->setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data2"), context->data_storage->getData("input_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method with unconstraint joint waypoint auto profiles = std::make_shared(); - TaskComposerDataStorage data; + auto data = std::make_unique(); auto input_program = test_suite::jointInterpolateExampleProgramABB(true); // Modify to have unconstrained joint waypoint @@ -651,101 +650,101 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / jwp.setIsConstrained(false); // Set input data - data.setData("input_data", input_program); - data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + data->setData("input_data", input_program); + data->setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data2"), context->data_storage->getData("input_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method with input is state waypoints auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + data->setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data2"), context->data_storage->getData("input_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data [0] auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data [1] auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure inputs are not the same size auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data.setData("output_data", test_suite::freespaceExampleProgramABB()); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + data->setData("output_data", test_suite::freespaceExampleProgramABB()); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -820,38 +819,38 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); MinLengthTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_GE(input->data_storage->getData("output_data").as().size(), 10); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); MinLengthTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -926,59 +925,59 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); FixStateBoundsTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method empty composite auto profiles = std::make_shared(); - TaskComposerDataStorage data; + auto data = std::make_unique(); auto program = test_suite::jointInterpolateExampleProgramABB(true); program.clear(); - data.setData("input_data", program); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + data->setData("input_data", program); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); FixStateBoundsTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); FixStateBoundsTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1053,21 +1052,21 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); FixStateCollisionTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization test_suite::runSerializationTest(dynamic_cast(*node_info), @@ -1076,19 +1075,19 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); FixStateCollisionTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1152,37 +1151,37 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerProfileSwitchTaskTests) / { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); ProfileSwitchTask task("abc", "input_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); ProfileSwitchTask task("abc", "input_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1233,61 +1232,61 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateEndStateTaskTests) getJointPosition(next_program.front().as().getWaypoint())); auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", input_program); - data.setData("next_data", next_program); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", input_program); + data->setData("next_data", next_program); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateEndStateTask task("abc", "input_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - auto output_program = input->data_storage->getData("output_data").as(); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + auto output_program = context->data_storage->getData("output_data").as(); EXPECT_EQ(output_program.back().getUUID(), input_program.back().getUUID()); EXPECT_EQ(output_program.back().as().getWaypoint().isStateWaypoint(), true); EXPECT_EQ(getJointPosition(output_program.back().as().getWaypoint()), getJointPosition(next_program.front().as().getWaypoint())); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateEndStateTask task("abc", "input_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing next data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateEndStateTask task("abc", "input_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1338,61 +1337,61 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartStateTaskTests) getJointPosition(prev_program.back().as().getWaypoint())); auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", input_program); - data.setData("prev_data", prev_program); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", input_program); + data->setData("prev_data", prev_program); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartStateTask task("abc", "input_data", "prev_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - auto output_program = input->data_storage->getData("output_data").as(); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + auto output_program = context->data_storage->getData("output_data").as(); EXPECT_EQ(output_program.front().getUUID(), input_program.front().getUUID()); EXPECT_EQ(output_program.front().as().getWaypoint().isStateWaypoint(), true); EXPECT_EQ(getJointPosition(output_program.front().as().getWaypoint()), getJointPosition(prev_program.back().as().getWaypoint())); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartStateTask task("abc", "input_data", "prev_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing prev data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartStateTask task("abc", "input_data", "prev_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1451,22 +1450,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask getJointPosition(next_program.front().as().getWaypoint())); auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", input_program); - data.setData("prev_data", prev_program); - data.setData("next_data", next_program); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", input_program); + data->setData("prev_data", prev_program); + data->setData("next_data", next_program); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartAndEndStateTask task("abc", "input_data", "prev_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - auto output_program = input->data_storage->getData("output_data").as(); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + auto output_program = context->data_storage->getData("output_data").as(); EXPECT_EQ(output_program.front().getUUID(), input_program.front().getUUID()); EXPECT_EQ(output_program.front().as().getWaypoint().isStateWaypoint(), true); EXPECT_EQ(getJointPosition(output_program.front().as().getWaypoint()), @@ -1475,61 +1474,61 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask EXPECT_EQ(output_program.back().as().getWaypoint().isStateWaypoint(), true); EXPECT_EQ(getJointPosition(output_program.back().as().getWaypoint()), getJointPosition(next_program.front().as().getWaypoint())); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartAndEndStateTask task("abc", "input_data", "prev_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing prev data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartAndEndStateTask task("abc", "input_data", "prev_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing next data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data.setData("prev_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + data->setData("prev_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpdateStartAndEndStateTask task("abc", "input_data", "prev_data", "next_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1604,38 +1603,38 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest { // Test run method auto profiles = std::make_shared(); - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1728,69 +1727,70 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz { // Test run method // Create input data - TaskComposerDataStorage data; + auto data = std::make_unique(); { - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto data2 = std::make_unique(); + data2->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data2)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - data.setData("input_data", input->data_storage->getData("output_data")); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(task.run(*context), 1); + data->setData("input_data", context->data_storage->getData("output_data")); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); } auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method - TaskComposerDataStorage data; + auto data = std::make_unique(); auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); - data.setData("input_data", program); + data->setData("input_data", program); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1865,31 +1865,32 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio { // Test run method // Create input data - TaskComposerDataStorage data; + auto data = std::make_unique(); { - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto data2 = std::make_unique(); + data2->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data2)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - data.setData("input_data", input->data_storage->getData("output_data")); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(task.run(*context), 1); + data->setData("input_data", context->data_storage->getData("output_data")); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); } auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization test_suite::runSerializationTest(dynamic_cast(*node_info), @@ -1897,41 +1898,41 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio } { // Test run method - TaskComposerDataStorage data; + auto data = std::make_unique(); auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); - data.setData("input_data", program); + data->setData("input_data", program); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -2006,30 +2007,32 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT { // Test run method // Create input data - TaskComposerDataStorage data; + auto data = std::make_unique(); { - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto data2 = std::make_unique(); + data2->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data2)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - data.setData("input_data", input->data_storage->getData("output_data")); - EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(task.run(*context), 1); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); - auto problem2 = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context2 = std::make_unique(std::move(problem2)); + auto data3 = std::make_unique(); + data3->setData("input_data", context->data_storage->getData("output_data")); + auto problem2 = std::make_unique(env_, manip_, profiles, "abc"); + auto context2 = std::make_unique(std::move(problem2), std::move(data3)); TimeOptimalParameterizationTask task2("abc", "input_data", "output_data", true); EXPECT_EQ(task2.run(*context2), 1); - data.setData("input_data", context2->data_storage->getData("output_data")); + data->setData("input_data", context2->data_storage->getData("output_data")); EXPECT_EQ(context2->data_storage->getData("output_data").as().size(), 17); } auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); @@ -2041,41 +2044,41 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT } { // Test run method - TaskComposerDataStorage data; + auto data = std::make_unique(); auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); - data.setData("input_data", program); + data->setData("input_data", program); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey("output_data")); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->data_storage->hasKey("output_data")); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -2154,31 +2157,32 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / } { // Test run method - TaskComposerDataStorage data; + auto data = std::make_unique(); { - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); + auto data2 = std::make_unique(); + data2->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data2)); MinLengthTask task("abc", "input_data", "output_data", true); - EXPECT_EQ(task.run(*input), 1); - data.setData("input_data", input->data_storage->getData("output_data")); - EXPECT_GE(input->data_storage->getData("output_data").as().size(), 10); + EXPECT_EQ(task.run(*context), 1); + data->setData("input_data", context->data_storage->getData("output_data")); + EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); } auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); MotionPlannerTask task("abc", "input_data", "output_data", false, true); - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "green"); EXPECT_EQ(node_info->return_value, 1); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_GE(input->data_storage->getData("output_data").as().size(), 10); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization test_suite::runSerializationTest(dynamic_cast(*node_info), @@ -2187,19 +2191,19 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / { // Failure missing input data auto profiles = std::make_shared(); - TaskComposerDataStorage data; - auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto context = std::make_unique(std::move(problem)); + auto data = std::make_unique(); + auto problem = std::make_unique(env_, manip_, profiles, "abc"); + auto context = std::make_unique(std::move(problem), std::move(data)); MotionPlannerTask task("abc", "input_data", "output_data", false, true); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -2543,39 +2547,32 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // { // Test run method // Create raster task TaskComposerNode::UPtr task = factory.createTaskComposerNode("RasterFtPipeline"); - const std::string input_key = task->getInputKeys().front(); const std::string output_key = task->getOutputKeys().front(); // Define profiles auto profiles = std::make_shared(); - // Create data storage - TaskComposerDataStorage data; - data.setData(input_key, test_suite::rasterExampleProgram()); - // Create problem - auto problem = std::make_unique(env_, data, profiles); - - // Create task input - auto context = std::make_unique(std::move(problem)); - input->dotgraph = true; + auto problem = std::make_unique(env_, profiles); + problem->dotgraph = true; + problem->input_instruction = test_suite::rasterExampleProgram(); // Solve raster plan auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = executor->run(*task, *input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); future->wait(); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "TaskComposerRasterMotionTaskTests.dot"); - EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os1, nullptr, future->context->task_infos.getInfoMap())); // NOLINT os1.close(); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey(output_key)); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); - auto info_map = input->task_infos.getInfoMap(); - EXPECT_EQ(info_map.size(), 94); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_TRUE(future->context->data_storage->hasKey(output_key)); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); + auto info_map = future->context->task_infos.getInfoMap(); + EXPECT_EQ(info_map.size(), 76); // Make sure keys are unique std::vector raster_input_keys; @@ -2653,22 +2650,19 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // YAML::Node config = YAML::Load(str); RasterMotionTask task("abc", config["config"], factory); - // Create data storage - TaskComposerDataStorage data; - // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); + auto problem = std::make_unique(env_, profiles); auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure null input data @@ -2695,22 +2689,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // RasterMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", tesseract_common::AnyPoly()); + auto data = std::make_unique(); + data->setData("input_data", tesseract_common::AnyPoly()); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure input data is not composite @@ -2737,22 +2731,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // RasterMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); + auto data = std::make_unique(); + data->setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure input data is not composite @@ -2779,22 +2773,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // RasterMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -3024,39 +3018,32 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) { // Test run method // Create raster task TaskComposerNode::UPtr task = factory.createTaskComposerNode("RasterFtOnlyPipeline"); - const std::string input_key = task->getInputKeys().front(); const std::string output_key = task->getOutputKeys().front(); // Define profiles auto profiles = std::make_shared(); - // Create data storage - TaskComposerDataStorage data; - data.setData(input_key, test_suite::rasterOnlyExampleProgram()); - // Create problem - auto problem = std::make_unique(env_, data, profiles); - - // Create task input - auto context = std::make_unique(std::move(problem)); - input->dotgraph = true; + auto problem = std::make_unique(env_, profiles); + problem->dotgraph = true; + problem->input_instruction = test_suite::rasterOnlyExampleProgram(); // Solve raster plan auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = executor->run(*task, *input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); future->wait(); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "TaskComposerRasterOnlyMotionTaskTests.dot"); - EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os1, nullptr, future->context->task_infos.getInfoMap())); // NOLINT os1.close(); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage->hasKey(output_key)); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); - auto info_map = input->task_infos.getInfoMap(); - EXPECT_EQ(info_map.size(), 74); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_TRUE(future->context->data_storage->hasKey(output_key)); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); + auto info_map = future->context->task_infos.getInfoMap(); + EXPECT_EQ(info_map.size(), 60); // Make sure keys are unique std::vector raster_input_keys; @@ -3128,22 +3115,19 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) YAML::Node config = YAML::Load(str); RasterOnlyMotionTask task("abc", config["config"], factory); - // Create data storage - TaskComposerDataStorage data; - // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); + auto problem = std::make_unique(env_, profiles); auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure null input data @@ -3165,22 +3149,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) RasterOnlyMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", tesseract_common::AnyPoly()); + auto data = std::make_unique(); + data->setData("input_data", tesseract_common::AnyPoly()); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure input data is not composite @@ -3202,22 +3186,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) RasterOnlyMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); + auto data = std::make_unique(); + data->setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure input data is not composite @@ -3239,22 +3223,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) RasterOnlyMotionTask task("abc", config["config"], factory); // Create data storage - TaskComposerDataStorage data; - data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); // Create problem auto profiles = std::make_shared(); - auto problem = std::make_unique(env_, data, profiles); - auto context = std::make_unique(std::move(problem)); - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + auto problem = std::make_unique(env_, profiles); + auto context = std::make_unique(std::move(problem), std::move(data)); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_EQ(node_info->message.empty(), false); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } }