diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 9900785aba8..06b055087e5 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -49,7 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include @@ -244,11 +244,11 @@ bool BasicCartesianExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); @@ -256,8 +256,7 @@ bool BasicCartesianExample::run() // Solve task tesseract_common::Timer stopwatch; stopwatch.start(); - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -267,7 +266,7 @@ bool BasicCartesianExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -276,7 +275,7 @@ bool BasicCartesianExample::run() } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index a00539a4d62..584665d6e10 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -302,24 +302,23 @@ bool CarSeatExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); - if (!input.isSuccessful()) + if (!future->context->isSuccessful()) return false; // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -391,24 +390,23 @@ bool CarSeatExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); - if (!input.isSuccessful()) + if (!future->context->isSuccessful()) return false; // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index b341c6e4433..ef3f261e3a3 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -180,22 +180,21 @@ bool FreespaceHybridExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -204,6 +203,6 @@ bool FreespaceHybridExample::run() } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 5f58f191b06..524c9664fd5 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -42,7 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -180,22 +180,21 @@ bool FreespaceOMPLExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -204,6 +203,6 @@ bool FreespaceOMPLExample::run() } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index de738379e29..308731f0152 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -39,7 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -237,11 +237,11 @@ bool GlassUprightExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); @@ -249,8 +249,7 @@ bool GlassUprightExample::run() // Solve process plan tesseract_common::Timer stopwatch; stopwatch.start(); - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -260,7 +259,7 @@ bool GlassUprightExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -270,6 +269,6 @@ bool GlassUprightExample::run() } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 4e79dff657f..f622eb53365 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -227,25 +227,24 @@ bool PickAndPlaceExample::run() const std::string pick_output_key = pick_task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage pick_input_data; - pick_input_data.setData(pick_input_key, pick_program); + auto pick_input_data = std::make_unique(); + pick_input_data->setData(pick_input_key, pick_program); // Create Task Composer Problem - auto pick_problem = std::make_unique(env_, pick_input_data, profiles); + auto pick_problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput pick_input(std::move(pick_problem)); - TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, pick_input); + TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem), std::move(pick_input_data)); pick_future->wait(); - if (!pick_input.isSuccessful()) + if (!pick_future->context->isSuccessful()) return false; // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = pick_input.data_storage.getData(pick_output_key).as(); + auto ci = pick_future->context->data_storage->getData(pick_output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -278,7 +277,8 @@ bool PickAndPlaceExample::run() env_->applyCommands(cmds); // Get the last move instruction - CompositeInstruction pick_composite = pick_input.data_storage.getData(pick_output_key).as(); + CompositeInstruction pick_composite = + pick_future->context->data_storage->getData(pick_output_key).as(); const MoveInstructionPoly* pick_final_state = pick_composite.getLastMoveInstruction(); // Retreat to the approach pose @@ -348,25 +348,25 @@ bool PickAndPlaceExample::run() const std::string place_output_key = pick_task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage place_input_data; - place_input_data.setData(place_input_key, place_program); + auto place_input_data = std::make_unique(); + place_input_data->setData(place_input_key, place_program); // Create Task Composer Problem - auto place_problem = std::make_unique(env_, place_input_data, profiles); + auto place_problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput place_input(std::move(place_problem)); - TaskComposerFuture::UPtr place_future = executor->run(*place_task, place_input); + TaskComposerFuture::UPtr place_future = + executor->run(*place_task, std::move(place_problem), std::move(place_input_data)); place_future->wait(); - if (!place_input.isSuccessful()) + if (!place_future->context->isSuccessful()) return false; // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = place_input.data_storage.getData(place_output_key).as(); + auto ci = place_future->context->data_storage->getData(place_output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index ae9a854e10c..4452358818f 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -230,31 +230,30 @@ bool PuzzlePieceAuxillaryAxesExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); if (plotter_ != nullptr) plotter_->waitForInput(); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); plotter_->plotTrajectory(trajectory, *state_solver); } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index f8f06f1d612..eaa2cda9662 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -44,7 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -222,20 +222,19 @@ bool PuzzlePieceExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - TaskComposerDataStorage input_data; - input_data.setData(input_key, program); + auto input_data = std::make_unique(); + input_data->setData(input_key, program); if (plotter_ != nullptr) plotter_->waitForInput(); // Create Task Composer Problem - auto problem = std::make_unique(env_, input_data, profiles); + auto problem = std::make_unique(env_, profiles); // Solve task tesseract_common::Timer stopwatch; stopwatch.start(); - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -245,13 +244,13 @@ bool PuzzlePieceExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = input.data_storage.getData(output_key).as(); + auto ci = future->context->data_storage->getData(output_key).as(); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); plotter_->plotTrajectory(trajectory, *state_solver); } CONSOLE_BRIDGE_logInform("Final trajectory is collision free"); - return input.isSuccessful(); + return future->context->isSuccessful(); } } // namespace tesseract_examples diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index eb68db73a43..65a4f8fd781 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -1,9 +1,9 @@ add_library( ${PROJECT_NAME} src/task_composer_data_storage.cpp + src/task_composer_context.cpp src/task_composer_executor.cpp src/task_composer_graph.cpp - src/task_composer_input.cpp src/task_composer_node.cpp src/task_composer_node_info.cpp src/task_composer_pipeline.cpp diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h index c45cc35707d..76587618106 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/abort_task.h @@ -57,7 +57,7 @@ class AbortTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/done_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/done_task.h index 94b559229aa..6f6fe309b07 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/done_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/done_task.h @@ -57,7 +57,7 @@ class DoneTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/error_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/error_task.h index d1bfd38f7ec..b5a4ce5197d 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/error_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/error_task.h @@ -57,7 +57,7 @@ class ErrorTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h index 4e75de72ce5..3800c68af4f 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h @@ -65,7 +65,7 @@ class RemapTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/start_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/start_task.h index d34d7567e0e..963814ec311 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/start_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/start_task.h @@ -56,7 +56,7 @@ class StartTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h index 3b1c6ee9640..a1bf0f51c85 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h @@ -66,7 +66,7 @@ class SyncTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h similarity index 56% rename from tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h rename to tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h index 4f6f9eb78d1..8f5b76bc991 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_input.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h @@ -1,6 +1,6 @@ /** - * @file task_composer_input.h - * @brief The input data structure to the pipeline + * @file task_composer_context.h + * @brief The context data structure to the pipeline * * @author Levi Armstrong * @date July 29. 2022 @@ -23,12 +23,13 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_TASK_COMPOSER_TASK_COMPOSER_INPUT_H -#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_INPUT_H +#ifndef TESSERACT_TASK_COMPOSER_TASK_COMPOSER_CONTEXT_H +#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_CONTEXT_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -40,38 +41,42 @@ namespace tesseract_planning /** * @brief This struct is passed as an input to each process in the decision tree * - * Note that it does not have ownership of any of its members (except the pointer). This means that if a TaskInput - * spawns a child that is a subset, it does not have to remain in scope as the references will still be valid + * Container for use internally by tasks and task executors to facilitate the execution of tasks + * + * This object contains: + * - `TaskComposerProblem`: represents the mutable input data needed to initiate the task and additional meta-data. + * - `TaskComposerDataStorage`: container in which tasks are expected to find their required input data and where tasks + * should save their results at run-time. This class member is originally set equal to the `TaskComposerDataStorage` + * member of the `TaskComposerProblem`, which represents the immutable input data for the task + * - `TaskComposerNodeInfoContainer`: container for meta-data generated by task(s) during execution. */ -struct TaskComposerInput +struct TaskComposerContext { - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - using UPtr = std::unique_ptr; - using ConstUPtr = std::unique_ptr; - - TaskComposerInput(TaskComposerProblem::UPtr problem); - TaskComposerInput(const TaskComposerInput&) = delete; - TaskComposerInput(TaskComposerInput&&) noexcept = delete; - TaskComposerInput& operator=(const TaskComposerInput&) = delete; - TaskComposerInput& operator=(TaskComposerInput&&) = delete; - virtual ~TaskComposerInput() = default; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + TaskComposerContext() = default; // Required for serialization + TaskComposerContext(TaskComposerProblem::Ptr problem, TaskComposerDataStorage::Ptr data_storage); + TaskComposerContext(const TaskComposerContext&) = delete; + TaskComposerContext(TaskComposerContext&&) noexcept = delete; + TaskComposerContext& operator=(const TaskComposerContext&) = delete; + TaskComposerContext& operator=(TaskComposerContext&&) = delete; + virtual ~TaskComposerContext() = default; /** @brief The problem */ - TaskComposerProblem::UPtr problem; + TaskComposerProblem::Ptr problem; /** * @brief The location data is stored and retrieved during execution * @details The problem input data is copied into this structure when constructed */ - TaskComposerDataStorage data_storage; + TaskComposerDataStorage::Ptr data_storage; - /** @brief The location where task info is stored during execution */ + /** @brief Container for meta-data generated by task(s) during execution */ TaskComposerNodeInfoContainer task_infos; - /** @brief Indicate if dotgraph should be provided */ - bool dotgraph{ false }; - /** * @brief Check if process has been aborted * @details This accesses the internal process interface class @@ -99,18 +104,13 @@ struct TaskComposerInput */ void abort(const TaskComposerNode& caller); - /** @brief Reset abort and data storage to constructed state */ - void reset(); + bool operator==(const TaskComposerContext& rhs) const; + bool operator!=(const TaskComposerContext& rhs) const; - bool operator==(const TaskComposerInput& rhs) const; - bool operator!=(const TaskComposerInput& rhs) const; - -protected: +private: friend struct tesseract_common::Serialization; friend class boost::serialization::access; - TaskComposerInput() = default; // Required for serialization - template void serialize(Archive& ar, const unsigned int version); // NOLINT @@ -119,6 +119,6 @@ struct TaskComposerInput } // namespace tesseract_planning #include -BOOST_CLASS_EXPORT_KEY2(tesseract_planning::TaskComposerInput, "TaskComposerInput") +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::TaskComposerContext, "TaskComposerContext") -#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_INPUT_H +#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_CONTEXT_H diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_executor.h index 8bde1f8d380..f2733ffce6a 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 @@ -34,7 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include #include namespace tesseract_planning @@ -56,10 +57,13 @@ class TaskComposerExecutor /** * @brief Execute the provided node * @param node The node to execute - * @param task_input The task input provided to every task + * @param problem The problem + * @param data_storage The data storage object to leverage * @return The future associated with execution */ - virtual TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerInput& task_input) = 0; + TaskComposerFuture::UPtr run(const TaskComposerNode& node, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage); /** @brief Queries the number of workers (example: number of threads) */ virtual long getWorkerCount() const = 0; @@ -78,6 +82,15 @@ class TaskComposerExecutor void serialize(Archive& ar, const unsigned int version); // NOLINT std::string name_; + + /** + * @brief Execute provided node provide the cotext + * @details This should only be used for dynamic tasking + * @param node The node to execute + * @param context The context + * @return The future associated with execution + */ + virtual TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerContext::Ptr context) = 0; }; } // namespace tesseract_planning diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_future.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_future.h index 0ad2df1424d..1f96224bbc9 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_future.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_future.h @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { /** @@ -50,8 +52,12 @@ class TaskComposerFuture using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + TaskComposerFuture() = default; + TaskComposerFuture(TaskComposerContext::Ptr context) : context(std::move(context)) {} virtual ~TaskComposerFuture() = default; + TaskComposerContext::Ptr context; + /** @brief Clear all content */ virtual void clear() = 0; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h index 7451e479ed2..23b0905e319 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include namespace tesseract_planning diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h index 554a2188cc9..0ff4c0f1cb4 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h @@ -126,7 +126,7 @@ class TaskComposerNodeInfo friend class TaskComposerTask; friend class TaskComposerPipeline; - /** @brief Indicate if task was not ran because input abort flag was enabled */ + /** @brief Indicate if task was not ran because abort flag was enabled */ bool aborted_{ false }; template @@ -164,6 +164,12 @@ struct TaskComposerNodeInfoContainer /** @brief Get a copy of the task_info_map_ in case it gets resized*/ std::map getInfoMap() const; + /** @brief Insert the contents of another container's info map */ + void insertInfoMap(const TaskComposerNodeInfoContainer& container); + + /** @brief Merge the contents of another container's info map */ + void mergeInfoMap(TaskComposerNodeInfoContainer&& container); + /** * @brief Called if aborted * @details This is set if abort is called in input diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h index b44e90d60e8..5e4fec5b4d0 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h @@ -63,7 +63,7 @@ class TaskComposerPipeline : public TaskComposerGraph TaskComposerPipeline(TaskComposerPipeline&&) = delete; TaskComposerPipeline& operator=(TaskComposerPipeline&&) = delete; - int run(TaskComposerInput& input, OptionalTaskComposerExecutor executor = std::nullopt) const; + int run(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; bool operator==(const TaskComposerPipeline& rhs) const; bool operator!=(const TaskComposerPipeline& rhs) const; @@ -75,11 +75,11 @@ class TaskComposerPipeline : public TaskComposerGraph template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; void runRecursive(const TaskComposerNode& node, - TaskComposerInput& input, + TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h index b7f36de3e62..3bf43003371 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_problem.h @@ -44,8 +44,7 @@ struct TaskComposerProblem using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - TaskComposerProblem(std::string name = "unset"); - TaskComposerProblem(TaskComposerDataStorage input_data, std::string name = "unset"); + TaskComposerProblem(std::string name = "unset", bool dotgraph = false); TaskComposerProblem(const TaskComposerProblem&) = default; TaskComposerProblem& operator=(const TaskComposerProblem&) = default; @@ -56,8 +55,8 @@ struct TaskComposerProblem /** @brief The name of the task to be ran for this problem */ std::string name; - /** @brief The location data is stored and retrieved during execution */ - TaskComposerDataStorage input_data; + /** @brief Indicate if dotgraph should be provided */ + bool dotgraph{ false }; /** * @brief Clone the planning problem diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h index ecbe1692734..45cb24def85 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_server.h @@ -116,20 +116,25 @@ class TaskComposerServer /** * @brief Execute the provided task graph - * @param task_input The task input provided to every task + * @param problem The task problem * @param name The name of the executor to use * @return The future associated with execution */ - TaskComposerFuture::UPtr run(TaskComposerInput& task_input, const std::string& name); + TaskComposerFuture::UPtr run(TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, + const std::string& name); /** * @brief Execute the provided node * @details It will call one of the methods below based on the node type * @param node The node to execute - * @param task_input The task input provided to every task + * @param problem The problem * @return The future associated with execution */ - TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerInput& task_input, const std::string& name); + TaskComposerFuture::UPtr run(const TaskComposerNode& node, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, + const std::string& name); /** @brief Queries the number of workers (example: number of threads) */ long getWorkerCount(const std::string& name) const; 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 dd8b33e98f1..0070a31eab4 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,7 +62,7 @@ class TaskComposerTask : public TaskComposerNode bool operator==(const TaskComposerTask& rhs) const; bool operator!=(const TaskComposerTask& rhs) const; - int run(TaskComposerInput& input, OptionalTaskComposerExecutor executor = std::nullopt) const; + int run(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; protected: friend struct tesseract_common::Serialization; @@ -71,7 +71,7 @@ class TaskComposerTask : public TaskComposerNode template void serialize(Archive& ar, const unsigned int version); // NOLINT - virtual TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + virtual TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const = 0; }; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp index 06c3a46d08c..59778c7f232 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_executor_unit.hpp @@ -42,13 +42,14 @@ void runTaskComposerExecutorTest() { { // task auto task = std::make_unique("DoneTask"); - auto executor = std::make_unique("TaskComposerExecutorTests", 3); + tesseract_planning::TaskComposerExecutor::UPtr executor = std::make_unique("TaskComposerExecutorTests", 3); EXPECT_EQ(executor->getName(), "TaskComposerExecutorTests"); EXPECT_EQ(executor->getWorkerCount(), 3); EXPECT_EQ(executor->getTaskCount(), 0); - auto input = std::make_unique(std::make_unique()); - auto future = executor->run(*task, *input); + auto problem = std::make_unique(); + auto data_storage = std::make_unique(); + auto future = executor->run(*task, std::move(problem), std::move(data_storage)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -61,10 +62,10 @@ void runTaskComposerExecutorTest() EXPECT_TRUE(copy_future->ready()); EXPECT_FALSE(task->isConditional()); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 1); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 1); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -167,13 +168,14 @@ void runTaskComposerExecutorTest() terminals: [DoneTask])"; YAML::Node config = YAML::Load(str2); auto pipeline = std::make_unique("Pipeline", config["config"], factory); - auto executor = std::make_unique("TaskComposerExecutorTests", 3); + tesseract_planning::TaskComposerExecutor::UPtr executor = std::make_unique("TaskComposerExecutorTests", 3); EXPECT_EQ(executor->getName(), "TaskComposerExecutorTests"); EXPECT_EQ(executor->getWorkerCount(), 3); EXPECT_EQ(executor->getTaskCount(), 0); - auto input = std::make_unique(std::make_unique()); - auto future = executor->run(*pipeline, *input); + auto problem = std::make_unique(); + auto data_storage = std::make_unique(); + auto future = executor->run(*pipeline, std::move(problem), std::move(data_storage)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -196,10 +198,10 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getInboundEdges().size(), 1); EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); EXPECT_EQ(task2->getOutboundEdges().size(), 0); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 6); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -232,13 +234,14 @@ void runTaskComposerExecutorTest() terminals: [DoneTask])"; YAML::Node config = YAML::Load(str2); auto graph = std::make_unique("Graph", config["config"], factory); - auto executor = std::make_unique("TaskComposerExecutorTests", 3); + tesseract_planning::TaskComposerExecutor::UPtr executor = std::make_unique("TaskComposerExecutorTests", 3); EXPECT_EQ(executor->getName(), "TaskComposerExecutorTests"); EXPECT_EQ(executor->getWorkerCount(), 3); EXPECT_EQ(executor->getTaskCount(), 0); - auto input = std::make_unique(std::make_unique()); - auto future = executor->run(*graph, *input); + auto problem = std::make_unique(); + auto data_storage = std::make_unique(); + auto future = executor->run(*graph, std::move(problem), std::move(data_storage)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -261,10 +264,10 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getInboundEdges().size(), 1); EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); EXPECT_EQ(task2->getOutboundEdges().size(), 0); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 6); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -301,13 +304,14 @@ void runTaskComposerExecutorTest() terminals: [AbortTask, DoneTask])"; YAML::Node config = YAML::Load(str2); auto graph = std::make_unique("Graph", config["config"], factory); - auto executor = std::make_unique(3); + tesseract_planning::TaskComposerExecutor::UPtr executor = std::make_unique(3); EXPECT_FALSE(executor->getName().empty()); EXPECT_EQ(executor->getWorkerCount(), 3); EXPECT_EQ(executor->getTaskCount(), 0); - auto input = std::make_unique(std::make_unique()); - auto future = executor->run(*graph, *input); + auto problem = std::make_unique(); + auto data_storage = std::make_unique(); + auto future = executor->run(*graph, std::move(problem), std::move(data_storage)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -335,10 +339,10 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task3->getInboundEdges().size(), 1); EXPECT_EQ(task3->getInboundEdges().front(), task1->getUUID()); EXPECT_EQ(task3->getOutboundEdges().size(), 0); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 6); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -371,13 +375,14 @@ void runTaskComposerExecutorTest() terminals: [DoneTask])"; YAML::Node config = YAML::Load(str2); auto graph = std::make_unique("Graph", config["config"], factory); - auto executor = std::make_unique("TaskComposerExecutorTests", 3); + tesseract_planning::TaskComposerExecutor::UPtr executor = std::make_unique("TaskComposerExecutorTests", 3); EXPECT_EQ(executor->getName(), "TaskComposerExecutorTests"); EXPECT_EQ(executor->getWorkerCount(), 3); EXPECT_EQ(executor->getTaskCount(), 0); - auto input = std::make_unique(std::make_unique()); - auto future = executor->run(*graph, *input); + auto problem = std::make_unique(); + auto data_storage = std::make_unique(); + auto future = executor->run(*graph, std::move(problem), std::move(data_storage)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -400,10 +405,10 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getInboundEdges().size(), 1); EXPECT_EQ(task2->getInboundEdges().front(), task1->getUUID()); EXPECT_EQ(task2->getOutboundEdges().size(), 0); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 6); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp index f73187b4b18..1a98ce6b88a 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include namespace tesseract_planning::test_suite diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_task.h index 2845f8e0bfd..6be5e65f75b 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_task.h @@ -61,7 +61,7 @@ class TestTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int /*version*/); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override final; }; } // namespace tesseract_planning::test_suite diff --git a/tesseract_task_composer/core/src/nodes/abort_task.cpp b/tesseract_task_composer/core/src/nodes/abort_task.cpp index fec5f0152fb..5769251cfbc 100644 --- a/tesseract_task_composer/core/src/nodes/abort_task.cpp +++ b/tesseract_task_composer/core/src/nodes/abort_task.cpp @@ -39,13 +39,14 @@ AbortTask::AbortTask(std::string name, const YAML::Node& config, const TaskCompo { } -TaskComposerNodeInfo::UPtr AbortTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +TaskComposerNodeInfo::UPtr AbortTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->color = "red"; info->return_value = 0; info->message = "Aborted"; - input.abort(uuid_); + context.abort(uuid_); CONSOLE_BRIDGE_logDebug("%s", info->message.c_str()); return info; } diff --git a/tesseract_task_composer/core/src/nodes/done_task.cpp b/tesseract_task_composer/core/src/nodes/done_task.cpp index 5a09313fd9d..21b7249b5ed 100644 --- a/tesseract_task_composer/core/src/nodes/done_task.cpp +++ b/tesseract_task_composer/core/src/nodes/done_task.cpp @@ -40,7 +40,7 @@ DoneTask::DoneTask(std::string name, const YAML::Node& config, const TaskCompose { } -TaskComposerNodeInfo::UPtr DoneTask::runImpl(TaskComposerInput& /*input*/, +TaskComposerNodeInfo::UPtr DoneTask::runImpl(TaskComposerContext& /*context*/, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); diff --git a/tesseract_task_composer/core/src/nodes/error_task.cpp b/tesseract_task_composer/core/src/nodes/error_task.cpp index 299bcf1dfa4..8203c26cecd 100644 --- a/tesseract_task_composer/core/src/nodes/error_task.cpp +++ b/tesseract_task_composer/core/src/nodes/error_task.cpp @@ -40,7 +40,7 @@ ErrorTask::ErrorTask(std::string name, const YAML::Node& config, const TaskCompo { } -TaskComposerNodeInfo::UPtr ErrorTask::runImpl(TaskComposerInput& /*input*/, +TaskComposerNodeInfo::UPtr ErrorTask::runImpl(TaskComposerContext& /*context*/, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); diff --git a/tesseract_task_composer/core/src/nodes/remap_task.cpp b/tesseract_task_composer/core/src/nodes/remap_task.cpp index cfbfd9a0e41..6717b9a182c 100644 --- a/tesseract_task_composer/core/src/nodes/remap_task.cpp +++ b/tesseract_task_composer/core/src/nodes/remap_task.cpp @@ -58,10 +58,11 @@ RemapTask::RemapTask(std::string name, const YAML::Node& config, const TaskCompo copy_ = n.as(); } -TaskComposerNodeInfo::UPtr RemapTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +TaskComposerNodeInfo::UPtr RemapTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); - if (input.data_storage.remapData(remap_, copy_)) + if (context.data_storage->remapData(remap_, copy_)) { info->color = "green"; info->return_value = 1; diff --git a/tesseract_task_composer/core/src/nodes/start_task.cpp b/tesseract_task_composer/core/src/nodes/start_task.cpp index 01fba45b49c..245497f8f68 100644 --- a/tesseract_task_composer/core/src/nodes/start_task.cpp +++ b/tesseract_task_composer/core/src/nodes/start_task.cpp @@ -46,7 +46,7 @@ StartTask::StartTask(std::string name, const YAML::Node& config, const TaskCompo if (!output_keys_.empty()) throw std::runtime_error("StartTask, config does not support 'outputs' entry"); } -TaskComposerNodeInfo::UPtr StartTask::runImpl(TaskComposerInput& /*input*/, +TaskComposerNodeInfo::UPtr StartTask::runImpl(TaskComposerContext& /*context*/, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); diff --git a/tesseract_task_composer/core/src/nodes/sync_task.cpp b/tesseract_task_composer/core/src/nodes/sync_task.cpp index 5f510304118..45bd482d2b0 100644 --- a/tesseract_task_composer/core/src/nodes/sync_task.cpp +++ b/tesseract_task_composer/core/src/nodes/sync_task.cpp @@ -46,7 +46,7 @@ SyncTask::SyncTask(std::string name, const YAML::Node& config, const TaskCompose if (!output_keys_.empty()) throw std::runtime_error("SyncTask, config does not support 'outputs' entry"); } -TaskComposerNodeInfo::UPtr SyncTask::runImpl(TaskComposerInput& /*input*/, +TaskComposerNodeInfo::UPtr SyncTask::runImpl(TaskComposerContext& /*context*/, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); diff --git a/tesseract_task_composer/core/src/task_composer_input.cpp b/tesseract_task_composer/core/src/task_composer_context.cpp similarity index 64% rename from tesseract_task_composer/core/src/task_composer_input.cpp rename to tesseract_task_composer/core/src/task_composer_context.cpp index 4ea467eab08..08af8df248e 100644 --- a/tesseract_task_composer/core/src/task_composer_input.cpp +++ b/tesseract_task_composer/core/src/task_composer_context.cpp @@ -1,6 +1,6 @@ /** - * @file task_composer_input.cpp - * @brief The input data structure to the pipeline + * @file task_composer_context.cpp + * @brief The context data structure to the pipeline * * @author Levi Armstrong * @date July 29. 2022 @@ -35,20 +35,21 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_planning { -TaskComposerInput::TaskComposerInput(TaskComposerProblem::UPtr problem) - : problem(std::move(problem)), data_storage(this->problem->input_data) +TaskComposerContext::TaskComposerContext(tesseract_planning::TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage) + : problem(std::move(problem)), data_storage(std::move(data_storage)) { } -bool TaskComposerInput::isAborted() const { return aborted_; } +bool TaskComposerContext::isAborted() const { return aborted_; } -bool TaskComposerInput::isSuccessful() const { return !aborted_; } +bool TaskComposerContext::isSuccessful() const { return !aborted_; } -void TaskComposerInput::abort(const boost::uuids::uuid& calling_node) +void TaskComposerContext::abort(const boost::uuids::uuid& calling_node) { if (!calling_node.is_nil()) task_infos.setAborted(calling_node); @@ -56,14 +57,7 @@ void TaskComposerInput::abort(const boost::uuids::uuid& calling_node) aborted_ = true; } -void TaskComposerInput::reset() -{ - aborted_ = false; - data_storage = problem->input_data; - task_infos.clear(); -} - -bool TaskComposerInput::operator==(const TaskComposerInput& rhs) const +bool TaskComposerContext::operator==(const TaskComposerContext& rhs) const { bool equal = true; if (problem != nullptr && rhs.problem != nullptr) @@ -71,16 +65,20 @@ bool TaskComposerInput::operator==(const TaskComposerInput& rhs) const else equal &= (problem == nullptr && rhs.problem == nullptr); - equal &= data_storage == rhs.data_storage; + if (data_storage != nullptr && rhs.data_storage != nullptr) + equal &= (*data_storage == *rhs.data_storage); + else + equal &= (data_storage == nullptr && rhs.data_storage == nullptr); + equal &= task_infos == rhs.task_infos; equal &= aborted_ == rhs.aborted_; return equal; } -bool TaskComposerInput::operator!=(const TaskComposerInput& rhs) const { return !operator==(rhs); } +bool TaskComposerContext::operator!=(const TaskComposerContext& rhs) const { return !operator==(rhs); } template -void TaskComposerInput::serialize(Archive& ar, const unsigned int /*version*/) +void TaskComposerContext::serialize(Archive& ar, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("problem", problem); ar& boost::serialization::make_nvp("data_storage", data_storage); @@ -91,5 +89,5 @@ void TaskComposerInput::serialize(Archive& ar, const unsigned int /*version*/) } // namespace tesseract_planning #include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerInput) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerInput) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerContext) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerContext) diff --git a/tesseract_task_composer/core/src/task_composer_data_storage.cpp b/tesseract_task_composer/core/src/task_composer_data_storage.cpp index 8d56dd39e4b..f26c9b536f9 100644 --- a/tesseract_task_composer/core/src/task_composer_data_storage.cpp +++ b/tesseract_task_composer/core/src/task_composer_data_storage.cpp @@ -38,10 +38,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include namespace tesseract_planning { -TaskComposerDataStorage::TaskComposerDataStorage(const TaskComposerDataStorage& other) { *this = other; } +TaskComposerDataStorage::TaskComposerDataStorage(const TaskComposerDataStorage& other) +{ + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + + data_ = other.data_; +} TaskComposerDataStorage& TaskComposerDataStorage::operator=(const TaskComposerDataStorage& other) { - std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); std::shared_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; @@ -50,16 +57,16 @@ TaskComposerDataStorage& TaskComposerDataStorage::operator=(const TaskComposerDa } TaskComposerDataStorage::TaskComposerDataStorage(TaskComposerDataStorage&& other) noexcept { - std::shared_lock lhs_lock(mutex_, std::defer_lock); - std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; data_ = std::move(other.data_); } TaskComposerDataStorage& TaskComposerDataStorage::operator=(TaskComposerDataStorage&& other) noexcept { - std::shared_lock lhs_lock(mutex_, std::defer_lock); - std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; data_ = std::move(other.data_); diff --git a/tesseract_task_composer/core/src/task_composer_executor.cpp b/tesseract_task_composer/core/src/task_composer_executor.cpp index 92b771e1e6f..99b507a7cc4 100644 --- a/tesseract_task_composer/core/src/task_composer_executor.cpp +++ b/tesseract_task_composer/core/src/task_composer_executor.cpp @@ -37,6 +37,13 @@ TaskComposerExecutor::TaskComposerExecutor(std::string name) : name_(std::move(n const std::string& TaskComposerExecutor::getName() const { return name_; } +TaskComposerFuture::UPtr TaskComposerExecutor::run(const TaskComposerNode& node, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage) +{ + return run(node, std::make_shared(std::move(problem), std::move(data_storage))); +} + bool TaskComposerExecutor::operator==(const TaskComposerExecutor& rhs) const { return (name_ == rhs.name_); } // LCOV_EXCL_START diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp index 7b1e0489d6e..fc2c853a896 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -99,18 +99,21 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(const TaskComposerNodeInfoContainer& other) { - std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); std::shared_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; - *this = other; + aborting_node_ = other.aborting_node_; + for (const auto& pair : other.info_map_) + info_map_[pair.first] = pair.second->clone(); } TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const TaskComposerNodeInfoContainer& other) { - std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); std::shared_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; + aborting_node_ = other.aborting_node_; for (const auto& pair : other.info_map_) info_map_[pair.first] = pair.second->clone(); @@ -119,18 +122,20 @@ TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const Ta TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(TaskComposerNodeInfoContainer&& other) noexcept { - std::shared_lock lhs_lock(mutex_, std::defer_lock); - std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; - *this = std::move(other); + aborting_node_ = other.aborting_node_; + info_map_ = std::move(other.info_map_); } TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(TaskComposerNodeInfoContainer&& other) noexcept { - std::shared_lock lhs_lock(mutex_, std::defer_lock); - std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock rhs_lock(other.mutex_, std::defer_lock); std::scoped_lock lock{ lhs_lock, rhs_lock }; + aborting_node_ = other.aborting_node_; info_map_ = std::move(other.info_map_); return *this; @@ -185,6 +190,26 @@ std::map TaskComposerNodeInfoCon return copy; } +void TaskComposerNodeInfoContainer::insertInfoMap(const TaskComposerNodeInfoContainer& container) +{ + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(container.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + for (const auto& pair : container.info_map_) + info_map_[pair.first] = pair.second->clone(); +} + +void TaskComposerNodeInfoContainer::mergeInfoMap(TaskComposerNodeInfoContainer&& container) +{ + std::unique_lock lhs_lock(mutex_, std::defer_lock); + std::unique_lock rhs_lock(container.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + info_map_.merge(std::move(container.info_map_)); + + // Should be empty, but if not then duplicates keys exist which should not be possible. + assert(container.info_map_.empty()); +} + void TaskComposerNodeInfoContainer::updateParents(std::map& info_map, const boost::uuids::uuid& uuid) const { @@ -232,6 +257,7 @@ template void TaskComposerNodeInfoContainer::serialize(Archive& ar, const unsigned int /*version*/) { std::unique_lock lock(mutex_); + ar& BOOST_SERIALIZATION_NVP(aborting_node_); ar& BOOST_SERIALIZATION_NVP(info_map_); } diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp index 9828644528e..36a33309e87 100644 --- a/tesseract_task_composer/core/src/task_composer_pipeline.cpp +++ b/tesseract_task_composer/core/src/task_composer_pipeline.cpp @@ -48,9 +48,9 @@ TaskComposerPipeline::TaskComposerPipeline(std::string name, { } -int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const +int TaskComposerPipeline::run(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { - if (input.isAborted()) + if (context.isAborted()) { auto info = std::make_unique(*this); info->input_keys = input_keys_; @@ -59,7 +59,7 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec info->color = "white"; info->message = "Aborted"; info->aborted_ = true; - input.task_infos.addInfo(std::move(info)); + context.task_infos.addInfo(std::move(info)); return 0; } @@ -68,7 +68,7 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec timer.start(); try { - results = runImpl(input, executor); + results = runImpl(context, executor); } catch (const std::exception& e) { @@ -84,11 +84,11 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec int value = results->return_value; assert(value >= 0); - input.task_infos.addInfo(std::move(results)); + context.task_infos.addInfo(std::move(results)); return value; } -TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { if (terminals_.empty()) @@ -109,11 +109,11 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu if (root_node.is_nil()) throw std::runtime_error("TaskComposerPipeline, with name '" + name_ + "' does not have a root node!"); - runRecursive(*(nodes_.at(root_node)), input, executor); + runRecursive(*(nodes_.at(root_node)), context, executor); for (std::size_t i = 0; i < terminals_.size(); ++i) { - auto node_info = input.task_infos.getInfo(terminals_[i]); + auto node_info = context.task_infos.getInfo(terminals_[i]); if (node_info != nullptr) { timer.stop(); @@ -133,7 +133,7 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu } void TaskComposerPipeline::runRecursive(const TaskComposerNode& node, - TaskComposerInput& input, + TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { if (node.getType() == TaskComposerNodeType::GRAPH) @@ -142,11 +142,11 @@ void TaskComposerPipeline::runRecursive(const TaskComposerNode& node, if (node.getType() == TaskComposerNodeType::TASK) { const auto& task = static_cast(node); - int rv = task.run(input, executor); + int rv = task.run(context, executor); if (task.isConditional()) { const auto& edge = node.getOutboundEdges().at(static_cast(rv)); - runRecursive(*nodes_.at(edge), input, executor); + runRecursive(*nodes_.at(edge), context, executor); } else { @@ -155,22 +155,22 @@ void TaskComposerPipeline::runRecursive(const TaskComposerNode& node, "'" + name_ + "'"); for (const auto& edge : node.getOutboundEdges()) - runRecursive(*(nodes_.at(edge)), input, executor); + runRecursive(*(nodes_.at(edge)), context, executor); } } else { const auto& pipeline = static_cast(node); - int rv = pipeline.run(input, executor); + int rv = pipeline.run(context, executor); if (pipeline.isConditional()) { const auto& edge = node.getOutboundEdges().at(static_cast(rv)); - runRecursive(*nodes_.at(edge), input, executor); + runRecursive(*nodes_.at(edge), context, executor); } else { for (const auto& edge : node.getOutboundEdges()) - runRecursive(*nodes_.at(edge), input, executor); + runRecursive(*nodes_.at(edge), context, executor); } } } diff --git a/tesseract_task_composer/core/src/task_composer_problem.cpp b/tesseract_task_composer/core/src/task_composer_problem.cpp index 091fb3a5671..f1d49c2ddd7 100644 --- a/tesseract_task_composer/core/src/task_composer_problem.cpp +++ b/tesseract_task_composer/core/src/task_composer_problem.cpp @@ -37,12 +37,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerProblem::TaskComposerProblem(std::string name) : name(std::move(name)) {} - -TaskComposerProblem::TaskComposerProblem(TaskComposerDataStorage input_data, std::string name) - : name(std::move(name)), input_data(std::move(input_data)) -{ -} +TaskComposerProblem::TaskComposerProblem(std::string name, bool dotgraph) : name(std::move(name)), dotgraph(dotgraph) {} TaskComposerProblem::UPtr TaskComposerProblem::clone() const { return std::make_unique(*this); } @@ -50,7 +45,7 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const { bool equal = true; equal &= name == rhs.name; - equal &= input_data == rhs.input_data; + equal &= dotgraph == rhs.dotgraph; return equal; } @@ -60,7 +55,7 @@ template void TaskComposerProblem::serialize(Archive& ar, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("name", name); - ar& boost::serialization::make_nvp("input_data", input_data); + ar& boost::serialization::make_nvp("dotgraph", dotgraph); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/core/src/task_composer_server.cpp b/tesseract_task_composer/core/src/task_composer_server.cpp index 25edca2c673..15c2a2e7bb1 100644 --- a/tesseract_task_composer/core/src/task_composer_server.cpp +++ b/tesseract_task_composer/core/src/task_composer_server.cpp @@ -104,28 +104,31 @@ std::vector TaskComposerServer::getAvailableTasks() const return tasks; } -TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerInput& task_input, const std::string& name) +TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, + const std::string& name) { auto e_it = executors_.find(name); if (e_it == executors_.end()) throw std::runtime_error("Executor with name '" + name + "' does not exist!"); - auto t_it = tasks_.find(task_input.problem->name); + auto t_it = tasks_.find(problem->name); if (t_it == tasks_.end()) - throw std::runtime_error("Task with name '" + task_input.problem->name + "' does not exist!"); + throw std::runtime_error("Task with name '" + problem->name + "' does not exist!"); - return e_it->second->run(*t_it->second, task_input); + return e_it->second->run(*t_it->second, std::move(problem), std::move(data_storage)); } TaskComposerFuture::UPtr TaskComposerServer::run(const TaskComposerNode& node, - TaskComposerInput& task_input, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, const std::string& name) { auto it = executors_.find(name); if (it == executors_.end()) throw std::runtime_error("Executor with name '" + name + "' does not exist!"); - return it->second->run(node, task_input); + return it->second->run(node, std::move(problem), std::move(data_storage)); } long TaskComposerServer::getWorkerCount(const std::string& name) const diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp index fb124da425f..d919d5d0ddf 100644 --- a/tesseract_task_composer/core/src/task_composer_task.cpp +++ b/tesseract_task_composer/core/src/task_composer_task.cpp @@ -46,9 +46,9 @@ TaskComposerTask::TaskComposerTask(std::string name, const YAML::Node& config) { } -int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor executor) const +int TaskComposerTask::run(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { - if (input.isAborted()) + if (context.isAborted()) { auto info = std::make_unique(*this); info->input_keys = input_keys_; @@ -57,7 +57,7 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor info->color = "white"; info->message = "Aborted"; info->aborted_ = true; - input.task_infos.addInfo(std::move(info)); + context.task_infos.addInfo(std::move(info)); return 0; } @@ -66,7 +66,7 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor timer.start(); try { - results = runImpl(input, executor); + results = runImpl(context, executor); } catch (const std::exception& e) { @@ -82,7 +82,7 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor int value = results->return_value; assert(value >= 0); - input.task_infos.addInfo(std::move(results)); + context.task_infos.addInfo(std::move(results)); return value; } diff --git a/tesseract_task_composer/core/src/test_suite/test_task.cpp b/tesseract_task_composer/core/src/test_suite/test_task.cpp index b516e8ea8ca..a9d3c819bca 100644 --- a/tesseract_task_composer/core/src/test_suite/test_task.cpp +++ b/tesseract_task_composer/core/src/test_suite/test_task.cpp @@ -72,7 +72,8 @@ void TestTask::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -TaskComposerNodeInfo::UPtr TestTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const +TaskComposerNodeInfo::UPtr TestTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const { if (throw_exception) throw std::runtime_error("TestTask, failure"); @@ -87,7 +88,7 @@ TaskComposerNodeInfo::UPtr TestTask::runImpl(TaskComposerInput& input, OptionalT if (set_abort) { node_info->color = "red"; - input.abort(uuid_); + context.abort(uuid_); } return node_info; diff --git a/tesseract_task_composer/examples/task_composer_example.cpp b/tesseract_task_composer/examples/task_composer_example.cpp index a8e235dbbf3..56b1292183a 100644 --- a/tesseract_task_composer/examples/task_composer_example.cpp +++ b/tesseract_task_composer/examples/task_composer_example.cpp @@ -20,15 +20,15 @@ class AddTaskComposerNode : public TaskComposerTask { } - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const override final { auto info = std::make_unique(*this); info->return_value = 0; std::cout << name_ << std::endl; double result = - input.data_storage.getData(left_key_).as() + input.data_storage.getData(right_key_).as(); - input.data_storage.setData(output_key_, result); + context.data_storage->getData(left_key_).as() + context.data_storage->getData(right_key_).as(); + context.data_storage->setData(output_key_, result); return info; } @@ -49,15 +49,15 @@ class MultiplyTaskComposerNode : public TaskComposerTask { } - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const override final { auto info = std::make_unique(*this); info->return_value = 0; std::cout << name_ << std::endl; double result = - input.data_storage.getData(left_key_).as() * input.data_storage.getData(right_key_).as(); - input.data_storage.setData(output_key_, result); + context.data_storage->getData(left_key_).as() * context.data_storage->getData(right_key_).as(); + context.data_storage->setData(output_key_, result); return info; } @@ -73,15 +73,13 @@ int main() double b{ 3 }; double c{ 5 }; double d{ 9 }; - TaskComposerDataStorage task_data; - task_data.setData("a", a); - task_data.setData("b", b); - task_data.setData("c", c); - task_data.setData("d", d); + auto task_data = std::make_unique(); + task_data->setData("a", a); + task_data->setData("b", b); + task_data->setData("c", c); + task_data->setData("d", d); - auto task_problem = std::make_unique(task_data); - - auto task_input = std::make_shared(std::move(task_problem)); + auto task_problem = std::make_unique(); // result = a * (b + c) + d auto task1 = std::make_unique("b", "c", "task1_output"); @@ -100,8 +98,8 @@ int main() TaskComposerPluginFactory factory(config_path); auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(task_composer, *task_input); + TaskComposerFuture::UPtr future = task_executor->run(task_composer, std::move(task_problem), std::move(task_data)); future->wait(); - std::cout << "Output: " << task_input->data_storage.getData("task3_output").as() << std::endl; + std::cout << "Output: " << future->context->data_storage->getData("task3_output").as() << std::endl; } diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index 96739478343..b29c5017281 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -57,29 +57,26 @@ int main() program.print(); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData(input_key, program); + auto task_data = std::make_unique(); + task_data->setData(input_key, program); // Create problem - auto task_problem = std::make_unique(env, task_data, profiles); - - // Create task input - auto task_input = std::make_shared(std::move(task_problem)); - task_input->dotgraph = true; + auto task_problem = std::make_unique(env, profiles); + task_problem->dotgraph = true; // Solve raster plan auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(*task, *task_input); + TaskComposerFuture::UPtr future = task_executor->run(*task, std::move(task_problem), std::move(task_data)); future->wait(); // Save dot graph std::ofstream tc_out_data; tc_out_data.open(tesseract_common::getTempPath() + "task_composer_raster_example.dot"); - task->dump(tc_out_data, nullptr, task_input->task_infos.getInfoMap()); + task->dump(tc_out_data, nullptr, future->context->task_infos.getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = task_input->data_storage.getData(output_key).as(); + auto output_program = future->context->data_storage->getData(output_key).as(); if (plotter != nullptr && plotter->isConnected()) { plotter->waitForInput(); diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp index 90b24bf1232..2d2e8ed813f 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -57,27 +57,25 @@ int main() CompositeInstruction program = test_suite::freespaceExampleProgramIIWA(); program.print(); - TaskComposerDataStorage task_data; - task_data.setData(input_key, program); + // Create data storage + auto task_data = std::make_unique(); + task_data->setData(input_key, program); // Create problem - auto task_problem = std::make_unique(env, task_data, profiles); - - // Create task input - auto task_input = std::make_shared(std::move(task_problem)); + auto task_problem = std::make_unique(env, profiles); auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(*task, *task_input); + TaskComposerFuture::UPtr future = task_executor->run(*task, std::move(task_problem), std::move(task_data)); future->wait(); // Save dot graph std::ofstream tc_out_data; tc_out_data.open(tesseract_common::getTempPath() + "task_composer_trajopt_graph_example.dot"); - task->dump(tc_out_data, nullptr, task_input->task_infos.getInfoMap()); + task->dump(tc_out_data, nullptr, future->context->task_infos.getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = task_input->data_storage.getData(output_key).as(); + auto output_program = future->context->data_storage->getData(output_key).as(); if (plotter != nullptr && plotter->isConnected()) { plotter->waitForInput(); diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/check_input_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/check_input_task.h index 87d1944329c..29baa891704 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/check_input_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/check_input_task.h @@ -65,7 +65,7 @@ class CheckInputTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h index 4ad0690d47d..3786952eb58 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h @@ -68,7 +68,7 @@ class ContinuousContactCheckTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h index 14fe861b1b5..e4538ab865e 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h @@ -68,7 +68,7 @@ class DiscreteContactCheckTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h index 6686b2d079e..5b5ee8acdab 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h @@ -69,7 +69,7 @@ class FixStateBoundsTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h index 28522d8ab75..99452d4c06a 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h @@ -81,7 +81,7 @@ class FixStateCollisionTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_input_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_input_task.h index 28c516399ef..c28eca5eadd 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_input_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_input_task.h @@ -81,7 +81,7 @@ class FormatAsInputTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h index 0d242ff23a3..776b0d6cc6c 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h @@ -73,7 +73,7 @@ class IterativeSplineParameterizationTask : public TaskComposerTask bool add_points_{ true }; IterativeSplineParameterization solver_; - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h index b0e5f541836..ccfbea59191 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h @@ -65,7 +65,7 @@ class MinLengthTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index 0fd17557891..eb71d0ed63b 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -112,11 +112,11 @@ class MotionPlannerTask : public TaskComposerTask ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -125,7 +125,7 @@ class MotionPlannerTask : public TaskComposerTask // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input instructions to MotionPlannerTask: " + name_ + " must be a composite instruction"; @@ -163,7 +163,7 @@ class MotionPlannerTask : public TaskComposerTask // -------------------- if (response) { - input.data_storage.setData(output_keys_[0], response.results); + context.data_storage->setData(output_keys_[0], response.results); info->return_value = 1; info->color = "green"; @@ -180,7 +180,7 @@ class MotionPlannerTask : public TaskComposerTask // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = response.message; return info; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h index 8f1411f156e..e252452b806 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h @@ -66,7 +66,7 @@ class ProfileSwitchTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_motion_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_motion_task.h index 83cca523f8f..87a4e50a9b9 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_motion_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_motion_task.h @@ -102,7 +102,7 @@ class RasterMotionTask : public TaskComposerTask static void checkTaskInput(const tesseract_common::AnyPoly& input); - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const override final; }; } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_only_motion_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_only_motion_task.h index d5b081287b2..c4ee0346192 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_only_motion_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/raster_only_motion_task.h @@ -93,7 +93,7 @@ class RasterOnlyMotionTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int /*version*/); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const override final; static void checkTaskInput(const tesseract_common::AnyPoly& input); diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h index 29071ea10e4..fc16d794cf0 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h @@ -65,7 +65,7 @@ class RuckigTrajectorySmoothingTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h index 50df8ee5143..59ad5b1f988 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h @@ -69,7 +69,7 @@ class TimeOptimalParameterizationTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_end_state_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_end_state_task.h index 459d24e7ae7..536e138ee23 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_end_state_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_end_state_task.h @@ -63,7 +63,7 @@ class UpdateEndStateTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_and_end_state_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_and_end_state_task.h index 126eb366c45..34ed956e5cb 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_and_end_state_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_and_end_state_task.h @@ -67,7 +67,7 @@ class UpdateStartAndEndStateTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_state_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_state_task.h index b8dce54cd3d..ee53d237c0d 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_state_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/update_start_state_task.h @@ -62,7 +62,7 @@ class UpdateStartStateTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override; }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h index 171a8773df6..95e3b4d7766 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h @@ -79,7 +79,7 @@ class UpsampleTrajectoryTask : public TaskComposerTask InstructionPoly& start_instruction, double longest_valid_segment_length) const; - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; 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 43331ca2c93..2d551429eef 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 @@ -46,13 +46,10 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem using ConstUPtr = std::unique_ptr; PlanningTaskComposerProblem(std::string name = "unset"); - PlanningTaskComposerProblem(TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles = nullptr, - std::string name = "unset"); + PlanningTaskComposerProblem(ProfileDictionary::ConstPtr profiles, std::string name = "unset"); PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, tesseract_common::ManipulatorInfo manip_info, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"); @@ -60,19 +57,16 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem tesseract_common::ManipulatorInfo manip_info, ProfileRemapping move_profile_remapping, ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"); PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, ProfileRemapping move_profile_remapping, ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"); PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"); diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/check_input_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/check_input_profile.h index 96f4a90e5f9..a74ad098ad9 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/check_input_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/check_input_profile.h @@ -32,7 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include namespace tesseract_planning @@ -46,13 +46,13 @@ struct CheckInputProfile /** * @brief Check if the task input is valid - * @param task_input The task input to check + * @param context The task context to check * @return True if valid otherwise false */ - virtual bool isValid(const TaskComposerInput& input) const + virtual bool isValid(const TaskComposerContext& context) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + const auto& problem = dynamic_cast(*context.problem); // Check Input if (!problem.env) diff --git a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp index 9fa7e121ca0..2ab0c06fb6a 100644 --- a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp @@ -60,19 +60,19 @@ CheckInputTask::CheckInputTask(std::string name, throw std::runtime_error("CheckInputTask, config missing 'inputs' entry"); } -TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); // Get Composite Profile info->return_value = 0; for (const auto& key : input_keys_) { - auto input_data_poly = input.data_storage.getData(key); + auto input_data_poly = context.data_storage->getData(key); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input key '" + key + "' is missing"; @@ -87,7 +87,7 @@ TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input, getProfile(name_, profile, *problem.profiles, std::make_shared()); cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); - if (!cur_composite_profile->isValid(input)) + if (!cur_composite_profile->isValid(context)) { info->message = "Validator failed"; return info; diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index 92c0357e628..fe3d24d7062 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -66,11 +66,11 @@ ContinuousContactCheckTask::ContinuousContactCheckTask(std::string name, "key"); } -TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -79,7 +79,7 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input seed to ContinuousContactCheckTask must be a composite instruction"; diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index 33ccc29ccd1..48adbd072dc 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -66,11 +66,11 @@ DiscreteContactCheckTask::DiscreteContactCheckTask(std::string name, "key"); } -TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -79,7 +79,7 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input to DiscreteContactCheckTask must be a composite instruction"; diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp index c75ecdf859f..191bd380173 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp @@ -69,11 +69,11 @@ FixStateBoundsTask::FixStateBoundsTask(std::string name, throw std::runtime_error("FixStateBoundsTask, config 'outputs' entry currently only supports one output key"); } -TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -81,7 +81,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input instruction to FixStateBounds must be a composite instruction"; @@ -122,7 +122,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, // If the output key is not the same as the input key the output data should be assigned the input data // for error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to clamp to joint limits"; return info; @@ -148,7 +148,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, // If the output key is not the same as the input key the output data should be assigned the input data // for error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to clamp to joint limits"; return info; @@ -164,7 +164,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "FixStateBoundsTask found no MoveInstructions to process"; @@ -194,7 +194,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to clamp to joint limits"; return info; @@ -206,7 +206,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, case FixStateBoundsProfile::Settings::DISABLED: { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful, DISABLED"; info->return_value = 1; @@ -214,7 +214,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, } } - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 7658488d0a6..468f6324bd7 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -342,11 +342,11 @@ FixStateCollisionTask::FixStateCollisionTask(std::string name, throw std::runtime_error("FixStateCollisionTask, config 'outputs' entry currently only supports one output key"); } -TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -355,7 +355,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input to FixStateCollision must be a composite instruction"; @@ -390,7 +390,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -419,7 +419,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -439,7 +439,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "FixStateCollisionTask found no MoveInstructions to process"; info->return_value = 1; @@ -450,7 +450,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp if (flattened.size() <= 2) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "FixStateCollisionTask found no intermediate MoveInstructions to process"; info->return_value = 1; @@ -484,7 +484,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -504,7 +504,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "FixStateCollisionTask found no MoveInstructions to process"; info->return_value = 1; @@ -538,7 +538,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -558,7 +558,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "FixStateCollisionTask found no MoveInstructions to process"; info->return_value = 1; @@ -592,7 +592,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -612,7 +612,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp if (flattened.size() <= 1) { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "FixStateCollisionTask found no MoveInstructions to process"; info->return_value = 1; @@ -646,7 +646,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp // If the output key is not the same as the input key the output data should be assigned the input data for // error branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); // Save space for (auto& contact_map : info->contact_results) @@ -662,7 +662,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp case FixStateCollisionProfile::Settings::DISABLED: { if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Successful, DISABLED"; info->return_value = 1; @@ -670,7 +670,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp } } - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp index a3b68ef3836..fc0c2970dac 100644 --- a/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/format_as_input_task.cpp @@ -68,7 +68,7 @@ FormatAsInputTask::FormatAsInputTask(std::string name, throw std::runtime_error("FormatAsInputTask, config 'outputs' entry requires one output key"); } -TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); @@ -77,7 +77,7 @@ TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerInput& input, // -------------------- // Check that inputs are valid // -------------------- - auto input_formatted_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_formatted_data_poly = context.data_storage->getData(input_keys_[0]); if (input_formatted_data_poly.isNull() || input_formatted_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { @@ -86,7 +86,7 @@ TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerInput& input, return info; } - auto input_unformatted_data_poly = input.data_storage.getData(input_keys_[1]); + auto input_unformatted_data_poly = context.data_storage->getData(input_keys_[1]); if (input_unformatted_data_poly.isNull() || input_unformatted_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { @@ -139,7 +139,7 @@ TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerInput& input, } } - input.data_storage.setData(output_keys_[0], input_formatted_data_poly); + context.data_storage->setData(output_keys_[0], input_formatted_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index 9aaed735dda..a89f4a2c43d 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -88,11 +88,11 @@ IterativeSplineParameterizationTask::IterativeSplineParameterizationTask( } } -TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -100,7 +100,7 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input results to iterative spline parameterization must be a composite instruction"; @@ -127,7 +127,7 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->color = "green"; info->message = "Iterative spline time parameterization found no MoveInstructions to process"; @@ -172,7 +172,7 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to perform iterative spline time parameterization for process input: " + ci.getDescription(); @@ -182,7 +182,7 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp info->color = "green"; info->message = "Successful"; - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->return_value = 1; CONSOLE_BRIDGE_logDebug("Iterative spline time parameterization succeeded"); return info; diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index ca9b419c5ab..0cdf7aea2fc 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -69,17 +69,17 @@ MinLengthTask::MinLengthTask(std::string name, throw std::runtime_error("MinLengthTask, config 'outputs' entry currently only supports one output key"); } -TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; // Check that inputs are valid - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input seed to MinLengthTask must be a composite instruction"; @@ -133,11 +133,11 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerInput& input, return info; } - input.data_storage.setData(output_keys_[0], response.results); + context.data_storage->setData(output_keys_[0], response.results); } else { - input.data_storage.setData(output_keys_[0], ci); + context.data_storage->setData(output_keys_[0], ci); } info->color = "green"; diff --git a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp index 280dfebf8e5..8c1e1108e47 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -60,11 +60,11 @@ ProfileSwitchTask::ProfileSwitchTask(std::string name, throw std::runtime_error("ProfileSwitchTask, does not support 'outputs' entry"); } -TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -72,7 +72,7 @@ TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerInput& input, // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input instruction to ProfileSwitch must be a composite instruction"; 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 5750c630e6c..e73314e9766 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -296,11 +296,11 @@ void RasterMotionTask::serialize(Archive& ar, const unsigned int /*version*/) / ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -309,7 +309,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); try { checkTaskInput(input_data_poly); @@ -356,7 +356,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); - input.data_storage.setData(raster_results.input_key, raster_input); + context.data_storage->setData(raster_results.input_key, raster_input); task_graph.addEdges(start_uuid, { raster_uuid }); @@ -402,7 +402,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - input.data_storage.setData(transition_results.input_key, transition_input); + context.data_storage->setData(transition_results.input_key, transition_input); task_graph.addEdges(transition_mux_uuid, { transition_uuid }); task_graph.addEdges(prev.first, { transition_mux_uuid }); @@ -426,7 +426,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, false); auto update_end_state_uuid = task_graph.addNode(std::move(update_end_state_task)); - input.data_storage.setData(from_start_results.input_key, from_start_input); + context.data_storage->setData(from_start_results.input_key, from_start_input); task_graph.addEdges(update_end_state_uuid, { from_start_pipeline_uuid }); task_graph.addEdges(raster_tasks[0].first, { update_end_state_uuid }); @@ -451,17 +451,21 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, "UpdateStartStateTask", to_end_results.input_key, last_raster_output_key, to_end_results.output_key, false); auto update_start_state_uuid = task_graph.addNode(std::move(update_start_state_task)); - input.data_storage.setData(to_end_results.input_key, to_end_input); + context.data_storage->setData(to_end_results.input_key, to_end_input); task_graph.addEdges(update_start_state_uuid, { to_end_pipeline_uuid }); task_graph.addEdges(raster_tasks.back().first, { update_start_state_uuid }); - TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, input); + TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - auto info_map = input.task_infos.getInfoMap(); + // Merge child context data into parent context + context.task_infos.mergeInfoMap(std::move(future->context->task_infos)); + if (future->context->isAborted()) + context.abort(future->context->task_infos.getAbortingNode()); - if (input.dotgraph) + auto info_map = context.task_infos.getInfoMap(); + if (context.problem->dotgraph) { std::stringstream dot_graph; dot_graph << "subgraph cluster_" << toString(uuid_) << " {\n color=black;\n label = \"" << name_ << "\\n(" @@ -471,7 +475,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, info->dotgraph = dot_graph.str(); } - if (input.isAborted()) + if (context.isAborted()) { info->message = "Raster subgraph failed"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); @@ -479,27 +483,27 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, } program.clear(); - program.emplace_back(input.data_storage.getData(from_start_results.output_key).as()); + program.emplace_back(context.data_storage->getData(from_start_results.output_key).as()); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { const auto& raster_output_key = raster_tasks[i].second.second; - CompositeInstruction segment = input.data_storage.getData(raster_output_key).as(); + CompositeInstruction segment = context.data_storage->getData(raster_output_key).as(); segment.erase(segment.begin()); program.emplace_back(segment); if (i < raster_tasks.size() - 1) { const auto& transition_output_key = transition_keys[i].second; - CompositeInstruction transition = input.data_storage.getData(transition_output_key).as(); + CompositeInstruction transition = context.data_storage->getData(transition_output_key).as(); transition.erase(transition.begin()); program.emplace_back(transition); } } - CompositeInstruction to_end = input.data_storage.getData(to_end_results.output_key).as(); + CompositeInstruction to_end = context.data_storage->getData(to_end_results.output_key).as(); to_end.erase(to_end.begin()); program.emplace_back(to_end); - input.data_storage.setData(output_keys_[0], program); + context.data_storage->setData(output_keys_[0], program); info->color = "green"; info->message = "Successful"; 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 672e222a79c..5f9e2fabad2 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 @@ -246,11 +246,11 @@ void RasterOnlyMotionTask::serialize(Archive& ar, const unsigned int /*version*/ ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -259,7 +259,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); try { checkTaskInput(input_data_poly); @@ -309,7 +309,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); - input.data_storage.setData(raster_results.input_key, raster_input); + context.data_storage->setData(raster_results.input_key, raster_input); task_graph.addEdges(start_uuid, { raster_uuid }); @@ -354,7 +354,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - input.data_storage.setData(transition_results.input_key, transition_input); + context.data_storage->setData(transition_results.input_key, transition_input); task_graph.addEdges(transition_mux_uuid, { transition_uuid }); task_graph.addEdges(prev.first, { transition_mux_uuid }); @@ -363,12 +363,16 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu transition_idx++; } - TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, input); + TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - auto info_map = input.task_infos.getInfoMap(); + // Merge child context data into parent context + context.task_infos.mergeInfoMap(std::move(future->context->task_infos)); + if (future->context->isAborted()) + context.abort(future->context->task_infos.getAbortingNode()); - if (input.dotgraph) + auto info_map = context.task_infos.getInfoMap(); + if (context.problem->dotgraph) { std::stringstream dot_graph; dot_graph << "subgraph cluster_" << toString(uuid_) << " {\n color=black;\n label = \"" << name_ << "\\n(" @@ -378,7 +382,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu info->dotgraph = dot_graph.str(); } - if (input.isAborted()) + if (context.isAborted()) { info->message = "Raster only subgraph failed"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); @@ -388,7 +392,8 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu program.clear(); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second.second).as(); + CompositeInstruction segment = + context.data_storage->getData(raster_tasks[i].second.second).as(); if (i != 0) segment.erase(segment.begin()); @@ -397,13 +402,13 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu if (i < raster_tasks.size() - 1) { CompositeInstruction transition = - input.data_storage.getData(transition_keys[i].second).as(); + context.data_storage->getData(transition_keys[i].second).as(); transition.erase(transition.begin()); program.emplace_back(transition); } } - input.data_storage.setData(output_keys_[0], program); + context.data_storage->setData(output_keys_[0], program); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp index dc07ddf0f08..7e60a00bb84 100644 --- a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -73,11 +73,11 @@ RuckigTrajectorySmoothingTask::RuckigTrajectorySmoothingTask(std::string name, "output key"); } -TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -85,7 +85,7 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input results to ruckig trajectory smoothing must be a composite instruction"; @@ -115,7 +115,7 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->color = "green"; info->message = "Ruckig trajectory smoothing found no MoveInstructions to process"; @@ -165,14 +165,14 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to perform ruckig trajectory smoothing for process input: %s" + ci.getDescription(); CONSOLE_BRIDGE_logInform("%s", info->message.c_str()); return info; } - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp index c6a75765358..5abebcbf4c9 100644 --- a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp @@ -79,11 +79,11 @@ TimeOptimalParameterizationTask::TimeOptimalParameterizationTask(std::string nam "output key"); } -TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -91,7 +91,7 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input results to TOTG must be a composite instruction"; @@ -118,7 +118,7 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->color = "green"; info->message = "TOTG found no MoveInstructions to process"; @@ -147,14 +147,14 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer // If the output key is not the same as the input key the output data should be assigned the input data for error // branching if (output_keys_[0] != input_keys_[0]) - input.data_storage.setData(output_keys_[0], input.data_storage.getData(input_keys_[0])); + context.data_storage->setData(output_keys_[0], context.data_storage->getData(input_keys_[0])); info->message = "Failed to perform TOTG for process input: " + ci.getDescription(); CONSOLE_BRIDGE_logInform("%s", info->message.c_str()); return info; } - input.data_storage.setData(output_keys_[0], copy_ci); + context.data_storage->setData(output_keys_[0], copy_ci); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp index b9918e1ba7a..f642ac4986f 100644 --- a/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_end_state_task.cpp @@ -57,14 +57,14 @@ UpdateEndStateTask::UpdateEndStateTask(std::string name, output_keys_.push_back(std::move(output_key)); } -TaskComposerNodeInfo::UPtr UpdateEndStateTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr UpdateEndStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = input.data_storage.getData(input_keys_[0]); - auto input_next_data_poly = input.data_storage.getData(input_keys_[1]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); + auto input_next_data_poly = context.data_storage->getData(input_keys_[1]); // -------------------- // Check that inputs are valid @@ -100,7 +100,7 @@ TaskComposerNodeInfo::UPtr UpdateEndStateTask::runImpl(TaskComposerInput& input, throw std::runtime_error("Invalid waypoint type"); // Store results - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp index b8e6363da7b..ab9e1ec0e9d 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_and_end_state_task.cpp @@ -61,15 +61,15 @@ UpdateStartAndEndStateTask::UpdateStartAndEndStateTask(std::string name, output_keys_.push_back(std::move(output_key)); } -TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = input.data_storage.getData(input_keys_[0]); - auto input_prev_data_poly = input.data_storage.getData(input_keys_[1]); - auto input_next_data_poly = input.data_storage.getData(input_keys_[2]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); + auto input_prev_data_poly = context.data_storage->getData(input_keys_[1]); + auto input_next_data_poly = context.data_storage->getData(input_keys_[2]); // -------------------- // Check that inputs are valid @@ -127,7 +127,7 @@ TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerInput throw std::runtime_error("Invalid waypoint type"); // Store results - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp index b9ae4845f5c..a1dc3d6466f 100644 --- a/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/update_start_state_task.cpp @@ -57,14 +57,14 @@ UpdateStartStateTask::UpdateStartStateTask(std::string name, output_keys_.push_back(std::move(output_key)); } -TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = input.data_storage.getData(input_keys_[0]); - auto input_prev_data_poly = input.data_storage.getData(input_keys_[1]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); + auto input_prev_data_poly = context.data_storage->getData(input_keys_[1]); // -------------------- // Check that inputs are valid @@ -100,7 +100,7 @@ TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerInput& inpu throw std::runtime_error("Invalid waypoint type"); // Store results - input.data_storage.setData(output_keys_[0], input_data_poly); + context.data_storage->setData(output_keys_[0], input_data_poly); info->color = "green"; info->message = "Successful"; diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp index dcff7b39ebd..a588223cc24 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -69,17 +69,17 @@ UpsampleTrajectoryTask::UpsampleTrajectoryTask(std::string name, "key"); } -TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; // Check that inputs are valid - auto input_data_poly = input.data_storage.getData(input_keys_[0]); + auto input_data_poly = context.data_storage->getData(input_keys_[0]); if (input_data_poly.isNull() || input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) { info->message = "Input seed to UpsampleTrajectoryTask must be a composite instruction"; @@ -101,7 +101,7 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& in new_results.clear(); upsample(new_results, ci, start_instruction, cur_composite_profile->longest_valid_segment_length); - input.data_storage.setData(output_keys_[0], new_results); + context.data_storage->setData(output_keys_[0], new_results); info->color = "green"; info->message = "Successful"; 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 bc7eeec122b..c453aafc862 100644 --- a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp +++ b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp @@ -39,19 +39,16 @@ namespace tesseract_planning { PlanningTaskComposerProblem::PlanningTaskComposerProblem(std::string name) : TaskComposerProblem(std::move(name)) {} -PlanningTaskComposerProblem::PlanningTaskComposerProblem(TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles, - std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)), profiles(std::move(profiles)) +PlanningTaskComposerProblem::PlanningTaskComposerProblem(ProfileDictionary::ConstPtr profiles, std::string name) + : TaskComposerProblem(std::move(name)), profiles(std::move(profiles)) { } PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, tesseract_common::ManipulatorInfo manip_info, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles, std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)) + : TaskComposerProblem(std::move(name)) , env(std::move(env)) , manip_info(std::move(manip_info)) , profiles(std::move(profiles)) @@ -62,10 +59,9 @@ PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment:: tesseract_common::ManipulatorInfo manip_info, ProfileRemapping move_profile_remapping, ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles, std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)) + : TaskComposerProblem(std::move(name)) , env(std::move(env)) , manip_info(std::move(manip_info)) , profiles(std::move(profiles)) @@ -77,10 +73,9 @@ PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment:: PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, ProfileRemapping move_profile_remapping, ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles, std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)) + : TaskComposerProblem(std::move(name)) , env(std::move(env)) , profiles(std::move(profiles)) , move_profile_remapping(std::move(move_profile_remapping)) @@ -89,10 +84,9 @@ PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment:: } PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles, std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)), env(std::move(env)), profiles(std::move(profiles)) + : TaskComposerProblem(std::move(name)), env(std::move(env)), profiles(std::move(profiles)) { } diff --git a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h index 2524d90d235..d7613c55500 100644 --- a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h +++ b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h @@ -59,8 +59,6 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor TaskflowTaskComposerExecutor(TaskflowTaskComposerExecutor&&) = delete; TaskflowTaskComposerExecutor& operator=(TaskflowTaskComposerExecutor&&) = delete; - TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerInput& task_input) override final; - long getWorkerCount() const override final; long getTaskCount() const override final; @@ -84,18 +82,22 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor std::size_t num_threads_; std::unique_ptr executor_; + TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerContext::Ptr context) override final; + static std::shared_ptr>> convertToTaskflow(const TaskComposerGraph& task_graph, - TaskComposerInput& task_input, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> convertToTaskflow(const TaskComposerPipeline& task_pipeline, - TaskComposerInput& task_input, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> - convertToTaskflow(const TaskComposerTask& task, TaskComposerInput& task_input, TaskComposerExecutor& task_executor); + convertToTaskflow(const TaskComposerTask& task, + TaskComposerContext& task_context, + TaskComposerExecutor& task_executor); }; } // namespace tesseract_planning diff --git a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_future.h b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_future.h index dd0ced083ea..c2bdd1ea3d6 100644 --- a/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_future.h +++ b/tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_future.h @@ -47,7 +47,8 @@ class TaskflowTaskComposerFuture : public TaskComposerFuture public: TaskflowTaskComposerFuture() = default; TaskflowTaskComposerFuture(std::shared_future future, - std::shared_ptr>> container); + std::shared_ptr>> container, + TaskComposerContext::Ptr context); ~TaskflowTaskComposerFuture() override; TaskflowTaskComposerFuture(const TaskflowTaskComposerFuture&) = default; TaskflowTaskComposerFuture& operator=(const TaskflowTaskComposerFuture&) = default; diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp index 4f3ae029474..3931f2a0681 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -69,15 +69,16 @@ TaskflowTaskComposerExecutor::TaskflowTaskComposerExecutor(std::string name, con TaskflowTaskComposerExecutor::~TaskflowTaskComposerExecutor() = default; -TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNode& node, TaskComposerInput& task_input) +TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNode& node, + TaskComposerContext::Ptr context) { std::shared_ptr>> taskflow; if (node.getType() == TaskComposerNodeType::TASK) - taskflow = convertToTaskflow(static_cast(node), task_input, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else if (node.getType() == TaskComposerNodeType::PIPELINE) - taskflow = convertToTaskflow(static_cast(node), task_input, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else if (node.getType() == TaskComposerNodeType::GRAPH) - taskflow = convertToTaskflow(static_cast(node), task_input, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else throw std::runtime_error("TaskComposerExecutor, unsupported node type!"); @@ -90,7 +91,7 @@ TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNod #endif std::shared_future f = executor_->run(*(taskflow->front())); - return std::make_unique(f, std::move(taskflow)); + return std::make_unique(f, std::move(taskflow), std::move(context)); } long TaskflowTaskComposerExecutor::getWorkerCount() const { return static_cast(executor_->num_workers()); } @@ -134,7 +135,7 @@ void TaskflowTaskComposerExecutor::serialize(Archive& ar, const unsigned int ver std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_graph, - TaskComposerInput& task_input, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor) { auto tf_container = std::make_shared>>(); @@ -145,7 +146,7 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr info->color = "green"; info->input_keys = task_graph.getInputKeys(); info->output_keys = task_graph.getOutputKeys(); - task_input.task_infos.addInfo(std::move(info)); + task_context.task_infos.addInfo(std::move(info)); // Generate process tasks for each node std::map tasks; @@ -159,31 +160,33 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr if (edges.size() > 1 && task->isConditional()) tasks[pair.first] = tf_container->front() - ->emplace([task, &task_input, &task_executor] { return task->run(task_input, task_executor); }) + ->emplace([task, &task_context, &task_executor] { return task->run(task_context, task_executor); }) .name(pair.second->getName()); else - tasks[pair.first] = tf_container->front() - ->emplace([task, &task_input, &task_executor] { task->run(task_input, task_executor); }) - .name(pair.second->getName()); + tasks[pair.first] = + tf_container->front() + ->emplace([task, &task_context, &task_executor] { task->run(task_context, task_executor); }) + .name(pair.second->getName()); } else if (pair.second->getType() == TaskComposerNodeType::PIPELINE) { auto pipeline = std::static_pointer_cast(pair.second); if (edges.size() > 1 && pipeline->isConditional()) - tasks[pair.first] = - tf_container->front() - ->emplace([pipeline, &task_input, &task_executor] { return pipeline->run(task_input, task_executor); }) - .name(pair.second->getName()); + tasks[pair.first] = tf_container->front() + ->emplace([pipeline, &task_context, &task_executor] { + return pipeline->run(task_context, task_executor); + }) + .name(pair.second->getName()); else tasks[pair.first] = tf_container->front() - ->emplace([pipeline, &task_input, &task_executor] { pipeline->run(task_input, task_executor); }) + ->emplace([pipeline, &task_context, &task_executor] { pipeline->run(task_context, task_executor); }) .name(pair.second->getName()); } else if (pair.second->getType() == TaskComposerNodeType::GRAPH) { const auto& graph = static_cast(*pair.second); - auto sub_tf_container = convertToTaskflow(graph, task_input, task_executor); + auto sub_tf_container = convertToTaskflow(graph, task_context, task_executor); tasks[pair.first] = tf_container->front()->composed_of(*sub_tf_container->front()); tf_container->insert(tf_container->end(), std::make_move_iterator(sub_tf_container->begin()), @@ -206,26 +209,27 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerPipeline& task_pipeline, - TaskComposerInput& task_input, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor) { auto tf_container = std::make_shared>>(); tf_container->emplace_back(std::make_unique(task_pipeline.getName())); tf_container->front() - ->emplace([&task_pipeline, &task_input, &task_executor] { return task_pipeline.run(task_input, task_executor); }) + ->emplace( + [&task_pipeline, &task_context, &task_executor] { return task_pipeline.run(task_context, task_executor); }) .name(task_pipeline.getName()); return tf_container; } std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerTask& task, - TaskComposerInput& task_input, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor) { auto tf_container = std::make_shared>>(); tf_container->emplace_back(std::make_unique(task.getName())); tf_container->front() - ->emplace([&task, &task_input, &task_executor] { return task.run(task_input, task_executor); }) + ->emplace([&task, &task_context, &task_executor] { return task.run(task_context, task_executor); }) .name(task.getName()); return tf_container; } diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_future.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_future.cpp index d847bc21ea9..9be9058ff98 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_future.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_future.cpp @@ -30,8 +30,9 @@ namespace tesseract_planning { TaskflowTaskComposerFuture::TaskflowTaskComposerFuture( std::shared_future future, - std::shared_ptr>> container) - : future_(std::move(future)), container_(std::move(container)) + std::shared_ptr>> container, + TaskComposerContext::Ptr context) + : TaskComposerFuture(std::move(context)), future_(std::move(future)), container_(std::move(container)) { } @@ -41,6 +42,7 @@ void TaskflowTaskComposerFuture::clear() { future_ = std::shared_future(); container_ = nullptr; + context = nullptr; } bool TaskflowTaskComposerFuture::valid() const { return future_.valid(); } @@ -65,10 +67,11 @@ TaskflowTaskComposerFuture::waitUntil(const std::chrono::time_point(); - clone->future_ = future_; - clone->container_ = container_; - return clone; + return std::make_unique(*this); + // clone->future_ = future_; + // clone->container_ = container_; + // clone->context = context; + // return clone; } } // namespace tesseract_planning diff --git a/tesseract_task_composer/test/CMakeLists.txt b/tesseract_task_composer/test/CMakeLists.txt index b782092ba9e..ca4eafe05a4 100644 --- a/tesseract_task_composer/test/CMakeLists.txt +++ b/tesseract_task_composer/test/CMakeLists.txt @@ -15,16 +15,6 @@ if(NOT WIN32) set(TESSERACT_TCMALLOC_LIB tcmalloc::tcmalloc_minimal) endif() -# add_executable(${PROJECT_NAME}_unit tesseract_task_composer_unit.cpp) target_link_libraries( ${PROJECT_NAME}_unit -# PRIVATE GTest::GTest GTest::Main tesseract::tesseract_support tesseract::tesseract_kinematics_opw ${PROJECT_NAME}) -# target_include_directories(${PROJECT_NAME}_unit PUBLIC "$") -# target_compile_options(${PROJECT_NAME}_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} -# ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_clang_tidy(${PROJECT_NAME}_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -# target_cxx_version(${PROJECT_NAME}_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( -# ${PROJECT_NAME}_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -# add_gtest_discover_tests(${PROJECT_NAME}_unit) add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME}) -# add_dependencies(run_tests ${PROJECT_NAME}_unit) - add_executable(${PROJECT_NAME}_fix_state_bounds_task_unit fix_state_bounds_task_unit.cpp) target_link_libraries( ${PROJECT_NAME}_fix_state_bounds_task_unit @@ -69,17 +59,6 @@ target_code_coverage( add_gtest_discover_tests(${PROJECT_NAME}_fix_state_collision_task_unit) add_dependencies(run_tests ${PROJECT_NAME}_fix_state_collision_task_unit) -# Serialize Tests add_executable(${PROJECT_NAME}_serialization_unit ${PROJECT_NAME}_serialization_unit.cpp) -# target_link_libraries(${PROJECT_NAME}_serialization_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) -# target_include_directories(${PROJECT_NAME}_serialization_unit PUBLIC -# "$") target_compile_options(${PROJECT_NAME}_serialization_unit PRIVATE -# ${TESSERACT_COMPILE_OPTIONS_PRIVATE} ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -# target_clang_tidy(${PROJECT_NAME}_serialization_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -# target_cxx_version(${PROJECT_NAME}_serialization_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) target_code_coverage( -# ${PROJECT_NAME}_serialization_unit PRIVATE ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -# add_gtest_discover_tests(${PROJECT_NAME}_serialization_unit) add_dependencies(run_tests -# ${PROJECT_NAME}_serialization_unit) add_dependencies(${PROJECT_NAME}_serialization_unit ${PROJECT_NAME}) - # Plugin Factories Tests add_executable(${PROJECT_NAME}_plugin_factories_unit ${PROJECT_NAME}_plugin_factories_unit.cpp) target_link_libraries(${PROJECT_NAME}_plugin_factories_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 3e890e85361..314423258e0 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -8,7 +8,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -81,14 +81,14 @@ void checkProgram(const Environment::Ptr& env, CompositeInstruction program = createProgram(start_state, goal_state, DEFAULT_PROFILE_KEY); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); + auto task_data = std::make_unique(); + task_data->setData("input_program", program); // Create problem - auto task_problem = std::make_unique(env, task_data, profiles); + auto task_problem = std::make_unique(env, profiles); - // Create input - auto task_input = std::make_shared(std::move(task_problem)); + // Create context + auto task_context = std::make_shared(std::move(task_problem), std::move(task_data)); // Create task FixStateBoundsTask task(FIX_STATE_BOUNDS_TASK_NAME, "input_program", "output_program"); @@ -100,11 +100,11 @@ void checkProgram(const Environment::Ptr& env, inside_limits &= isWithinJointLimits(instruction.get().as().getWaypoint(), joint_limits); EXPECT_EQ(inside_limits, pre_check_return); - EXPECT_EQ(task.run(*task_input), expected_return); + EXPECT_EQ(task.run(*task_context), expected_return); if (expected_return == 1) { - auto task_program = task_input->data_storage.getData("output_program").as(); + auto task_program = task_context->data_storage->getData("output_program").as(); auto task_flattened = task_program.flatten(moveFilter); switch (setting) diff --git a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp index 22be1629073..5c1d5f15900 100644 --- a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp @@ -45,11 +45,11 @@ TEST_F(FixStateCollisionTaskUnit, StateInCollisionTest) // NOLINT CompositeInstruction program = test_suite::freespaceExampleProgramABB(); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); + auto task_data = std::make_unique(); + task_data->setData("input_program", program); // Create problem - auto task_problem = std::make_unique(env_, task_data); + auto task_problem = std::make_unique(env_); FixStateCollisionProfile profile; @@ -88,11 +88,11 @@ TEST_F(FixStateCollisionTaskUnit, WaypointInCollisionTest) // NOLINT CompositeInstruction program = test_suite::freespaceExampleProgramABB(); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); + auto task_data = std::make_unique(); + task_data->setData("input_program", program); // Create problem - auto task_problem = std::make_unique(env_, task_data); + auto task_problem = std::make_unique(env_); FixStateCollisionProfile profile; @@ -139,11 +139,11 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionRandomSamplerTest) / CompositeInstruction program = test_suite::freespaceExampleProgramABB(); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); + auto task_data = std::make_shared(); + task_data->setData("input_program", program); // Create problem - auto task_problem = std::make_unique(env_, task_data); + auto task_problem = std::make_unique(env_); FixStateCollisionProfile profile; @@ -175,11 +175,11 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptTest) // NOLI CompositeInstruction program = test_suite::freespaceExampleProgramABB(); // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); + auto task_data = std::make_shared(); + task_data->setData("input_program", program); // Create problem - auto task_problem = std::make_unique(env_, task_data); + auto task_problem = std::make_unique(env_); FixStateCollisionProfile profile; diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp index 33e53841d4f..ac7c2710c7f 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -117,36 +117,30 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerDataStorageTests) // NOLINT } } -TEST(TesseractTaskComposerCoreUnit, TaskComposerInputTests) // NOLINT +TEST(TesseractTaskComposerCoreUnit, TaskComposerContextTests) // NOLINT { TaskComposerNode node; - auto input = std::make_unique(std::make_unique()); - EXPECT_FALSE(input->dotgraph); - EXPECT_FALSE(input->isAborted()); - EXPECT_TRUE(input->isSuccessful()); - EXPECT_TRUE(input->task_infos.getInfoMap().empty()); - input->task_infos.addInfo(std::make_unique(node)); - input->abort(node.getUUID()); - EXPECT_EQ(input->task_infos.getAbortingNode(), node.getUUID()); - EXPECT_TRUE(input->isAborted()); - EXPECT_FALSE(input->isSuccessful()); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 1); + auto context = std::make_unique(std::make_unique(), + std::make_unique()); + EXPECT_FALSE(context->isAborted()); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_TRUE(context->task_infos.getInfoMap().empty()); + context->task_infos.addInfo(std::make_unique(node)); + context->abort(node.getUUID()); + EXPECT_EQ(context->task_infos.getAbortingNode(), node.getUUID()); + EXPECT_TRUE(context->isAborted()); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); // Serialization - test_suite::runSerializationPointerTest(input, "TaskComposerInputTests"); - - input->reset(); - EXPECT_TRUE(input->problem != nullptr); - EXPECT_FALSE(input->dotgraph); - EXPECT_FALSE(input->isAborted()); - EXPECT_TRUE(input->isSuccessful()); - EXPECT_TRUE(input->task_infos.getInfoMap().empty()); + test_suite::runSerializationPointerTest(context, "TaskComposerContextTests"); } TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT { auto problem = std::make_unique(); EXPECT_EQ(problem->name, "unset"); + EXPECT_FALSE(problem->dotgraph); // Serialization test_suite::runSerializationPointerTest(problem, "TaskComposerProblemTests"); @@ -154,11 +148,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT auto problem2 = std::make_unique("TaskComposerProblemTests"); EXPECT_EQ(problem2->name, "TaskComposerProblemTests"); - auto problem3 = std::make_unique(TaskComposerDataStorage(), "TaskComposerProblemTests"); + auto problem3 = std::make_unique("TaskComposerProblemTests", true); EXPECT_EQ(problem3->name, "TaskComposerProblemTests"); + EXPECT_TRUE(problem3->dotgraph); auto problem_clone = problem3->clone(); EXPECT_EQ(*problem3, *problem_clone); + EXPECT_TRUE(problem_clone->dotgraph); } TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLINT @@ -167,10 +163,14 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLI auto node_info = std::make_unique(node); auto node_info_container = std::make_unique(); + EXPECT_TRUE(node_info_container->getAbortingNode().is_nil()); EXPECT_TRUE(node_info_container->getInfoMap().empty()); + auto aborted_uuid = node.getUUID(); node_info_container->addInfo(std::move(node_info)); + node_info_container->setAborted(aborted_uuid); EXPECT_EQ(node_info_container->getInfoMap().size(), 1); EXPECT_TRUE(node_info_container->getInfo(node.getUUID()) != nullptr); + EXPECT_TRUE(node_info_container->getAbortingNode() == aborted_uuid); // Serialization test_suite::runSerializationPointerTest(node_info_container, "TaskComposerNodeInfoContainerTests"); @@ -179,15 +179,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLI auto copy_node_info_container = std::make_unique(*node_info_container); EXPECT_EQ(copy_node_info_container->getInfoMap().size(), 1); EXPECT_TRUE(copy_node_info_container->getInfo(node.getUUID()) != nullptr); + EXPECT_TRUE(copy_node_info_container->getAbortingNode() == aborted_uuid); // Move auto move_node_info_container = std::make_unique(std::move(*node_info_container)); EXPECT_EQ(move_node_info_container->getInfoMap().size(), 1); EXPECT_TRUE(move_node_info_container->getInfo(node.getUUID()) != nullptr); + EXPECT_TRUE(move_node_info_container->getAbortingNode() == aborted_uuid); move_node_info_container->clear(); EXPECT_TRUE(move_node_info_container->getInfoMap().empty()); EXPECT_TRUE(move_node_info_container->getInfo(node.getUUID()) == nullptr); + EXPECT_TRUE(move_node_info_container->getAbortingNode().is_nil()); } TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeTests) // NOLINT @@ -289,16 +292,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(task->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(task->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 0); std::stringstream os; - EXPECT_NO_THROW(task->dump(os)); // NOLINT - EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { // Conditional @@ -312,16 +316,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(task->run(input), 1); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 1); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(task->run(*context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 1); std::stringstream os; - EXPECT_NO_THROW(task->dump(os)); // NOLINT - EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { @@ -342,16 +347,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(task->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(task->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 0); std::stringstream os; - EXPECT_NO_THROW(task->dump(os)); // NOLINT - EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { @@ -373,16 +379,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(task->run(input), 1); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 1); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(task->run(*context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 1); std::stringstream os; - EXPECT_NO_THROW(task->dump(os)); // NOLINT - EXPECT_NO_THROW(task->dump(os, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { // Failure due to exception during run @@ -393,14 +400,15 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT outputs: [output_data])"; YAML::Node config = YAML::Load(str); auto task = std::make_unique(name, config["config"], factory); - TaskComposerInput input(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); task->throw_exception = true; - EXPECT_EQ(task->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(task->getUUID())->return_value, 0); + EXPECT_EQ(task->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 0); } } @@ -479,12 +487,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 5); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 5); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test1a.dot"); @@ -493,7 +502,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test1b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -534,12 +543,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 1); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 1); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 4); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test2a.dot"); @@ -548,7 +558,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test2b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -590,12 +600,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 4); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test3a.dot"); @@ -604,7 +615,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test3b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -646,12 +657,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_FALSE(input.isSuccessful()); - EXPECT_TRUE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 4); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_TRUE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 4); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test4a.dot"); @@ -660,7 +672,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test4b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -704,12 +716,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline3->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 9); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline3->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 9); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test5a.dot"); @@ -718,7 +731,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test5b.dot"); - EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -764,12 +777,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline3->run(input), 1); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 9); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline3->run(*context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 9); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test6a.dot"); @@ -778,7 +792,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test6b.dot"); - EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -825,12 +839,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline3->run(input), 1); - EXPECT_FALSE(input.isSuccessful()); - EXPECT_TRUE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 6); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline3->run(*context), 1); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_TRUE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 6); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline3->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test7a.dot"); @@ -839,7 +854,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test7b.dot"); - EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -994,12 +1009,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test8a.dot"); @@ -1008,7 +1024,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test8b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1025,12 +1041,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT pipeline->addEdges(uuid2, { uuid3 }); pipeline->addEdges(uuid3, { uuid1 }); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 1); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test9a.dot"); @@ -1039,7 +1056,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test9b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1055,12 +1072,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT pipeline->addEdges(uuid1, { uuid3 }); pipeline->setTerminals({ uuid2, uuid3 }); - TaskComposerInput input(std::make_unique()); - EXPECT_EQ(pipeline->run(input), 0); - EXPECT_TRUE(input.isSuccessful()); - EXPECT_FALSE(input.isAborted()); - EXPECT_EQ(input.task_infos.getInfoMap().size(), 2); - EXPECT_EQ(input.task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); + EXPECT_EQ(pipeline->run(*context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 2); + EXPECT_EQ(context->task_infos.getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test10a.dot"); @@ -1069,7 +1087,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT std::ofstream os2; os2.open(tesseract_common::getTempPath() + "task_composer_pipeline_test10b.dot"); - EXPECT_NO_THROW(pipeline->dump(os2, nullptr, input.task_infos.getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1532,17 +1550,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerAbortTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); AbortTask task; - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(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, "Aborted"); EXPECT_EQ(node_info->isAborted(), false); - EXPECT_EQ(input->isAborted(), true); - EXPECT_EQ(input->isSuccessful(), false); - EXPECT_EQ(input->task_infos.getAbortingNode(), task.getUUID()); + EXPECT_EQ(context->isAborted(), true); + EXPECT_EQ(context->isSuccessful(), false); + EXPECT_EQ(context->task_infos.getAbortingNode(), task.getUUID()); } } @@ -1578,17 +1597,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerErrorTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); ErrorTask task; - EXPECT_EQ(task.run(*input), 0); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(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, "Error"); 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()); } } @@ -1624,17 +1644,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerDoneTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); DoneTask task; - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(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()); } } @@ -1683,54 +1704,57 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT tesseract_common::JointState js(joint_names, joint_values); { // Test run method copy auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); std::map remap; remap[key] = remap_key; RemapTask task("RemapTaskTest", remap, true, true); - EXPECT_EQ(task.run(*input), 1); - EXPECT_TRUE(input->data_storage.hasKey(key)); - EXPECT_TRUE(input->data_storage.hasKey(remap_key)); - EXPECT_EQ(input->data_storage.getData(key), input->data_storage.getData(remap_key)); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + EXPECT_TRUE(context->data_storage->hasKey(key)); + EXPECT_TRUE(context->data_storage->hasKey(remap_key)); + EXPECT_EQ(context->data_storage->getData(key), context->data_storage->getData(remap_key)); + 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()); } { // Test run method move auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); std::map remap; remap[key] = remap_key; RemapTask task("RemapTaskTest", remap, false, true); - EXPECT_EQ(task.run(*input), 1); - EXPECT_FALSE(input->data_storage.hasKey(key)); - EXPECT_TRUE(input->data_storage.hasKey(remap_key)); - EXPECT_EQ(input->data_storage.getData(remap_key).as(), js); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + EXPECT_FALSE(context->data_storage->hasKey(key)); + EXPECT_TRUE(context->data_storage->hasKey(remap_key)); + EXPECT_EQ(context->data_storage->getData(remap_key).as(), js); + 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()); } { // Test run method copy with config auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); TaskComposerPluginFactory factory; std::string str = R"(config: @@ -1741,24 +1765,25 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); - EXPECT_EQ(task.run(*input), 1); - EXPECT_TRUE(input->data_storage.hasKey(key)); - EXPECT_TRUE(input->data_storage.hasKey(remap_key)); - EXPECT_EQ(input->data_storage.getData(key), input->data_storage.getData(remap_key)); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + EXPECT_TRUE(context->data_storage->hasKey(key)); + EXPECT_TRUE(context->data_storage->hasKey(remap_key)); + EXPECT_EQ(context->data_storage->getData(key), context->data_storage->getData(remap_key)); + 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()); } { // Test run method move with config auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); TaskComposerPluginFactory factory; std::string str = R"(config: @@ -1769,18 +1794,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); - EXPECT_EQ(task.run(*input), 1); - EXPECT_FALSE(input->data_storage.hasKey(key)); - EXPECT_TRUE(input->data_storage.hasKey(remap_key)); - EXPECT_EQ(input->data_storage.getData(remap_key).as(), js); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 1); + EXPECT_FALSE(context->data_storage->hasKey(key)); + EXPECT_TRUE(context->data_storage->hasKey(remap_key)); + EXPECT_EQ(context->data_storage->getData(remap_key).as(), js); + 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()); } { // Failures @@ -1809,46 +1834,48 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT { // Test run method copy failure auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); std::map remap; remap["does_not_exits"] = remap_key; RemapTask task("RemapTaskTest", remap, true, true); - EXPECT_EQ(task.run(*input), 0); - EXPECT_TRUE(input->data_storage.hasKey(key)); - EXPECT_FALSE(input->data_storage.hasKey(remap_key)); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + EXPECT_TRUE(context->data_storage->hasKey(key)); + EXPECT_FALSE(context->data_storage->hasKey(remap_key)); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_FALSE(node_info->message.empty()); 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()); } { // Test run method copy failure auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto data_storage = std::make_unique(); + data_storage->setData(key, js); + auto context = std::make_shared(std::move(problem), std::move(data_storage)); std::map remap; remap["does_not_exits"] = remap_key; RemapTask task("RemapTaskTest", remap, false, true); - EXPECT_EQ(task.run(*input), 0); - EXPECT_TRUE(input->data_storage.hasKey(key)); - EXPECT_FALSE(input->data_storage.hasKey(remap_key)); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(task.run(*context), 0); + EXPECT_TRUE(context->data_storage->hasKey(key)); + EXPECT_FALSE(context->data_storage->hasKey(remap_key)); + auto node_info = context->task_infos.getInfo(task.getUUID()); EXPECT_EQ(node_info->color, "red"); EXPECT_EQ(node_info->return_value, 0); EXPECT_FALSE(node_info->message.empty()); 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()); } } @@ -1905,17 +1932,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); StartTask task; - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(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()); } } @@ -1972,17 +2000,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); SyncTask task; - EXPECT_EQ(task.run(*input), 1); - auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(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()); } } @@ -2082,41 +2111,41 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerServerTests) // NOLINT EXPECT_ANY_THROW(server.getWorkerCount("DoesNotExist")); // NOLINT EXPECT_ANY_THROW(server.getTaskCount("DoesNotExist")); // NOLINT - { // Run method using TaskComposerInput - auto input = std::make_unique(std::make_unique()); - input->problem->name = "TestPipeline"; - auto future = server.run(*input, "TaskflowExecutor"); + { // Run method using TaskComposerContext + auto problem = std::make_unique("TestPipeline"); + auto data_storage = std::make_unique(); + auto future = server.run(std::move(problem), std::move(data_storage), "TaskflowExecutor"); future->wait(); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 4); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 4); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); } { // Run method using Pipeline - auto input = std::make_unique(std::make_unique()); - input->problem->name = "TestPipeline"; + auto problem = std::make_unique("TestPipeline"); + auto data_storage = std::make_unique(); const auto& pipeline = server.getTask("TestPipeline"); - auto future = server.run(pipeline, *input, "TaskflowExecutor"); + auto future = server.run(pipeline, std::move(problem), std::move(data_storage), "TaskflowExecutor"); future->wait(); - EXPECT_EQ(input->isAborted(), false); - EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->task_infos.getInfoMap().size(), 4); - EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(future->context->isAborted(), false); + EXPECT_EQ(future->context->isSuccessful(), true); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 4); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); } { // Failures, executor does not exist - auto input = std::make_unique(std::make_unique()); - input->problem->name = "TestPipeline"; - EXPECT_ANY_THROW(server.run(*input, "DoesNotExist")); // NOLINT + auto problem = std::make_unique("TestPipeline"); + auto data_storage = std::make_unique(); + EXPECT_ANY_THROW(server.run(std::move(problem), std::move(data_storage), "DoesNotExist")); // NOLINT } { // Failures, task does not exist - auto input = std::make_unique(std::make_unique()); - input->problem->name = "DoesNotExist"; - EXPECT_ANY_THROW(server.run(*input, "TaskflowExecutor")); // NOLINT + auto problem = std::make_unique("DoesNotExist"); + auto data_storage = std::make_unique(); + EXPECT_ANY_THROW(server.run(std::move(problem), std::move(data_storage), "TaskflowExecutor")); // NOLINT } }; 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 0338bdd1c43..51e8040b8ea 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -221,7 +221,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble 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)); + auto input = std::make_unique(std::move(problem)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -241,7 +241,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble auto problem = std::make_unique(env_, manip_, data, profiles, "TaskComposerCheckInputTaskTests"); problem->env = nullptr; - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -249,9 +249,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble 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 @@ -259,7 +259,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "TaskComposerCheckInputTaskTests"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); CheckInputTask task("TaskComposerCheckInputTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -327,7 +327,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); ContinuousContactCheckTask task("TaskComposerContinuousContactCheckTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -349,7 +349,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask TaskComposerDataStorage data; auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + 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()); @@ -375,7 +375,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerContinuousContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + 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()); @@ -444,7 +444,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -466,7 +466,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe TaskComposerDataStorage data; auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -492,7 +492,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe data.setData("input_data", test_suite::jointInterpolateExampleProgramABB()); auto problem = std::make_unique( env_, manip_, data, profiles, "TaskComposerDiscreteContactCheckTaskTests"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); DiscreteContactCheckTask task("TaskComposerDiscreteContactCheckTaskTests", "input_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -593,7 +593,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 1); @@ -604,7 +604,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -626,7 +626,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 1); @@ -637,7 +637,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -654,7 +654,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 1); @@ -665,7 +665,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -675,7 +675,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 1); @@ -686,7 +686,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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_EQ(input->data_storage->getData("output_data2"), data.getData("input_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -695,7 +695,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / TaskComposerDataStorage data; data.setData("output_data", test_suite::jointInterpolateExampleProgramABB(false)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 0); @@ -714,7 +714,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 0); @@ -734,7 +734,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatAsInputTaskTests) / 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); std::array input_keys{ "input_data", "output_data" }; FormatAsInputTask task("abc", input_keys, "output_data2", true); EXPECT_EQ(task.run(*input), 0); @@ -823,7 +823,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); MinLengthTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -833,7 +833,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO 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_GE(input->data_storage->getData("output_data").as().size(), 10); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -841,7 +841,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); MinLengthTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -929,7 +929,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); FixStateBoundsTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -939,7 +939,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -950,7 +950,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) program.clear(); data.setData("input_data", program); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); FixStateBoundsTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -960,7 +960,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -968,7 +968,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); FixStateBoundsTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1056,7 +1056,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); FixStateCollisionTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1066,7 +1066,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); // Serialization @@ -1078,7 +1078,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); FixStateCollisionTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1155,7 +1155,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerProfileSwitchTaskTests) / TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); ProfileSwitchTask task("abc", "input_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1172,7 +1172,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerProfileSwitchTaskTests) / auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); ProfileSwitchTask task("abc", "input_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1237,7 +1237,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateEndStateTaskTests) data.setData("input_data", input_program); data.setData("next_data", next_program); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1247,7 +1247,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateEndStateTaskTests) 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(); + auto output_program = input->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()), @@ -1259,7 +1259,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateEndStateTaskTests) auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1277,7 +1277,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateEndStateTaskTests) TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1342,7 +1342,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartStateTaskTests) data.setData("input_data", input_program); data.setData("prev_data", prev_program); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1352,7 +1352,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartStateTaskTests) 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(); + auto output_program = input->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()), @@ -1364,7 +1364,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartStateTaskTests) auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1382,7 +1382,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartStateTaskTests) TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1456,7 +1456,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask data.setData("prev_data", prev_program); data.setData("next_data", next_program); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1466,7 +1466,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask 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(); + auto output_program = input->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()), @@ -1482,7 +1482,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1500,7 +1500,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1519,7 +1519,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpdateStartAndEndStateTask 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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()); @@ -1607,7 +1607,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest TaskComposerDataStorage data; data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1617,7 +1617,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest 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_EQ(input->data_storage->getData("output_data").as().size(), 17); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1625,7 +1625,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1733,15 +1733,15 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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); + data.setData("input_data", input->data_storage->getData("output_data")); + EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1751,7 +1751,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz 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_EQ(input->data_storage->getData("output_data").as().size(), 17); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1762,7 +1762,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz data.setData("input_data", program); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1772,7 +1772,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1780,7 +1780,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); IterativeSplineParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1870,15 +1870,15 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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); + data.setData("input_data", input->data_storage->getData("output_data")); + EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1888,7 +1888,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio 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_EQ(input->data_storage->getData("output_data").as().size(), 17); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); // Serialization @@ -1903,7 +1903,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio data.setData("input_data", program); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -1913,7 +1913,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1921,7 +1921,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); TimeOptimalParameterizationTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2011,22 +2011,22 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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); + data.setData("input_data", input->data_storage->getData("output_data")); + EXPECT_EQ(input->data_storage->getData("output_data").as().size(), 17); auto problem2 = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input2 = std::make_unique(std::move(problem2)); + auto context2 = std::make_unique(std::move(problem2)); TimeOptimalParameterizationTask task2("abc", "input_data", "output_data", true); - EXPECT_EQ(task2.run(*input2), 1); - data.setData("input_data", input2->data_storage.getData("output_data")); - EXPECT_EQ(input2->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(task2.run(*context2), 1); + 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 input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2034,10 +2034,10 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT 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 @@ -2047,7 +2047,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT data.setData("input_data", program); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2057,7 +2057,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT 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->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -2065,7 +2065,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); RuckigTrajectorySmoothingTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2159,15 +2159,15 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / data.setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); 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); + data.setData("input_data", input->data_storage->getData("output_data")); + EXPECT_GE(input->data_storage->getData("output_data").as().size(), 10); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); MotionPlannerTask task("abc", "input_data", "output_data", false, true); EXPECT_EQ(task.run(*input), 1); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2177,7 +2177,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / 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_GE(input->data_storage->getData("output_data").as().size(), 10); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); // Serialization @@ -2189,7 +2189,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / auto profiles = std::make_shared(); TaskComposerDataStorage data; auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_unique(std::move(problem)); MotionPlannerTask task("abc", "input_data", "output_data", false, true); EXPECT_EQ(task.run(*input), 0); auto node_info = input->task_infos.getInfo(task.getUUID()); @@ -2557,7 +2557,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // auto problem = std::make_unique(env_, data, profiles); // Create task input - auto input = std::make_shared(std::move(problem)); + auto context = std::make_unique(std::move(problem)); input->dotgraph = true; // Solve raster plan @@ -2572,7 +2572,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage.hasKey(output_key)); + 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); @@ -2659,7 +2659,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -2701,7 +2701,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -2743,7 +2743,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -2785,7 +2785,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -3038,7 +3038,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) auto problem = std::make_unique(env_, data, profiles); // Create task input - auto input = std::make_shared(std::move(problem)); + auto context = std::make_unique(std::move(problem)); input->dotgraph = true; // Solve raster plan @@ -3053,7 +3053,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->data_storage.hasKey(output_key)); + 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); @@ -3134,7 +3134,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -3171,7 +3171,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -3208,7 +3208,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); @@ -3245,7 +3245,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create problem auto profiles = std::make_shared(); auto problem = std::make_unique(env_, data, profiles); - auto input = std::make_shared(std::move(problem)); + 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(node_info->color, "red"); diff --git a/tesseract_task_composer/test/tesseract_task_composer_serialization_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_serialization_unit.cpp deleted file mode 100644 index 992351764a3..00000000000 --- a/tesseract_task_composer/test/tesseract_task_composer_serialization_unit.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/** - * @file teasseract_task_composer_serialize_test.cpp - * @brief - * - * @author Levi Armstrong - * @author Matthew Powelson - * @date June 22, 2021 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, 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 -//#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "raster_example_program.h" -#include "freespace_example_program.h" - -using namespace tesseract_planning; - -void setNodeInfoData(TaskComposerNodeInfo& info) -{ - info.elapsed_time = 123.456; - info.message = "Test message"; - info.return_value = 1; - info.results = rasterExampleProgram(); -} - -TEST(TesseractTaskComposerSerializeUnit, TaskComposerNodeInfo) // NOLINT -{ - StartTask task("my task"); - auto info = std::make_shared(task); - setNodeInfoData(*info); - tesseract_common::testSerialization(*info, "TaskComposerNodeInfo"); -} - -TEST(TesseractTaskComposerSerializeUnit, ContinuousContactCheckTaskInfo) // NOLINT -{ - ContinuousContactCheckTask task; - auto info = std::make_shared(task); - setNodeInfoData(*info); - tesseract_common::testSerialization(*info, "ContinuousContactCheckTaskInfo"); - tesseract_common::testSerializationDerivedClass(info, - "ContinuousCont" - "actCheckTask" - "Info"); -} - -TEST(TesseractTaskComposerSerializeUnit, DiscreteContactCheckTaskInfo) // NOLINT -{ - DiscreteContactCheckTask task; - auto info = std::make_shared(task); - setNodeInfoData(*info); - tesseract_common::testSerialization(*info, "DiscreteContactCheckTaskInfo"); - tesseract_common::testSerializationDerivedClass(info, - "DiscreteContactC" - "heckTaskInfo"); -} - -TEST(TesseractTaskComposerSerializeUnit, FixStateCollisionTaskInfo) // NOLINT -{ - FixStateCollisionTask task; - auto info = std::make_shared(task); - setNodeInfoData(*info); - tesseract_common::testSerialization(*info, "FixStateCollisionTaskInfo"); - tesseract_common::testSerializationDerivedClass(info, - "FixStateCollisionTa" - "skInfo"); -} - -TEST(TesseractTaskComposerSerializeUnit, TaskComposerInput) // NOLINT -{ - StartTask task("my task"); - auto info = std::make_unique(task); - setNodeInfoData(*info); - - // Define profiles - auto profiles = std::make_shared(); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", rasterExampleProgram()); - - // Create problem - TaskComposerProblem task_problem(nullptr, task_data); - - // Create task input - auto task_input = std::make_shared(task_problem, profiles); - - task_input->task_infos.addInfo(std::move(info)); - task_input->abort(); - - tesseract_common::testSerialization(*task_input, "TaskComposerInput"); -} - -TEST(TesseractTaskComposerSerializeUnit, TaskComposerFuture) // NOLINT -{ - TaskflowTaskComposerFuture future; - tesseract_common::testSerialization(future, "TaskflowTaskComposerFuture"); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp deleted file mode 100644 index 300a57cd1a9..00000000000 --- a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp +++ /dev/null @@ -1,598 +0,0 @@ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "raster_example_program.h" -#include "freespace_example_program.h" - -using namespace tesseract_environment; -using namespace tesseract_scene_graph; -using namespace tesseract_planning; -using namespace tesseract_planning::node_names; -using namespace tesseract_planning::profile_ns; - -class TesseractTaskComposerUnit : public ::testing::Test -{ -protected: - Environment::Ptr env_; - ManipulatorInfo manip; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - - manip.manipulator = "manipulator"; - manip.manipulator_ik_solver = "OPWInvKin"; - manip.working_frame = "base_link"; - manip.tcp_frame = "tool0"; - } -}; - -TEST_F(TesseractTaskComposerUnit, MinLengthTaskTest) // NOLINT -{ - tesseract_planning::CompositeInstruction program = freespaceExampleProgramABB(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - // Define the Process Input - auto cur_state = env_->getState(); - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env_); - - TaskComposerDataStorage task_data; - task_data.setData("input_program", interpolated_program); - - TaskComposerProblem task_problem(env_, task_data); - - auto profiles = std::make_shared(); - long current_length = interpolated_program.getMoveInstructionCount(); - profiles->addProfile( - MIN_LENGTH_TASK_NAME, program.getProfile(), std::make_shared(current_length)); - - auto task_input = std::make_shared(task_problem, profiles); - - MinLengthTask task("input_program", "output_program", true); - EXPECT_TRUE(task.run(*task_input) == 1); - long final_length = - task_input->data_storage.getData("output_program").as().getMoveInstructionCount(); - EXPECT_TRUE(final_length == current_length); - - profiles->addProfile( - MIN_LENGTH_TASK_NAME, program.getProfile(), std::make_shared(2 * current_length)); - - task_input->reset(); - EXPECT_TRUE(task.run(*task_input) == 1); - long final_length2 = - task_input->data_storage.getData("output_program").as().getMoveInstructionCount(); - EXPECT_TRUE(final_length2 >= (2 * current_length)); - - profiles->addProfile( - MIN_LENGTH_TASK_NAME, program.getProfile(), std::make_shared(3 * current_length)); - - task_input->reset(); - EXPECT_TRUE(task.run(*task_input) == 1); - long final_length3 = - task_input->data_storage.getData("output_program").as().getMoveInstructionCount(); - EXPECT_TRUE(final_length3 >= (3 * current_length)); -} - -TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlanProfileTest) // NOLINT -{ - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, std::make_shared()); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, std::make_shared()); - - auto interpolator = std::make_shared(); - - // Create Planning Request - PlannerRequest request; - request.instructions = program; - request.env = env_; - request.env_state = env_->getState(); - request.profiles = profiles; - - PlannerResponse response = interpolator->solve(request); - EXPECT_TRUE(response); - - auto pcnt = request.instructions.getMoveInstructionCount(); - auto mcnt = response.results.getMoveInstructionCount(); - - // The first plan instruction is the start instruction and every other plan instruction should be converted into - // ten move instruction. - EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_FALSE(response.results.getManipulatorInfo().empty()); -} - -TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) // NOLINT -{ - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, std::make_shared()); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, std::make_shared()); - - auto interpolator = std::make_shared(); - - // Create Planning Request - PlannerRequest request; - request.instructions = program; - request.env = env_; - request.env_state = env_->getState(); - request.profiles = profiles; - - PlannerResponse response = interpolator->solve(request); - EXPECT_TRUE(response); - - auto mcnt = response.results.getMoveInstructionCount(); - - // The first plan instruction is the start instruction and every other plan instruction should be converted into - // ten move instruction. - EXPECT_EQ(98, mcnt); - EXPECT_FALSE(response.results.getManipulatorInfo().empty()); -} - -TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanProfileTest) // NOLINT -{ - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, std::make_shared()); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, std::make_shared()); - - auto interpolator = std::make_shared(); - - // Create Planning Request - PlannerRequest request; - request.instructions = program; - request.env = env_; - request.env_state = env_->getState(); - request.profiles = profiles; - - PlannerResponse response = interpolator->solve(request); - EXPECT_TRUE(response); - - auto mcnt = response.results.getMoveInstructionCount(); - - // The first plan instruction is the start instruction and every other plan instruction should be converted into - // ten move instruction. - EXPECT_EQ(83, mcnt); - EXPECT_FALSE(response.results.getManipulatorInfo().empty()); -} - -TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerFixedSizeAssignPlanProfileTest) // NOLINT -{ - CompositeInstruction program = freespaceExampleProgramABB(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - auto interpolator = std::make_shared(); - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); - - // Create Planning Request - PlannerRequest request; - request.instructions = program; - request.env = env_; - request.env_state = env_->getState(); - request.profiles = profiles; - - PlannerResponse response = interpolator->solve(request); - EXPECT_TRUE(response); - - auto pcnt = request.instructions.getMoveInstructionCount(); - auto mcnt = response.results.getMoveInstructionCount(); - - // The first plan instruction is the start instruction and every other plan instruction should be converted into - // ten move instruction. - EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_FALSE(response.results.getManipulatorInfo().empty()); -} - -TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerDefaultLVSPlanProfileTest) // NOLINT -{ - CompositeInstruction program = freespaceExampleProgramABB(); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - program.setManipulatorInfo(manip); - EXPECT_FALSE(program.getManipulatorInfo().empty()); - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); - - auto interpolator = std::make_shared(); - - // Create Planning Request - PlannerRequest request; - request.instructions = program; - request.env = env_; - request.env_state = env_->getState(); - request.profiles = profiles; - - PlannerResponse response = interpolator->solve(request); - EXPECT_TRUE(response); - - auto mcnt = response.results.getMoveInstructionCount(); - - // The first plan instruction is the start instruction and every other plan instruction should be converted into - // 32 move instruction. - EXPECT_EQ(37, mcnt); - EXPECT_FALSE(response.results.getManipulatorInfo().empty()); -} - -TEST_F(TesseractTaskComposerUnit, RasterProcessManagerDefaultPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtMotionTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtGlobalPipelineTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultLVSPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtGlobalPipelineTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtOnlyMotionTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtOnlyMotionTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtOnlyGlobalPipelineTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlanProfileTest) // NOLINT -{ - // Create Executor - auto executor = std::make_unique(); - - // Define the program - std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; - - CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); - - // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); - auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); - - // Create data storage - TaskComposerDataStorage task_data; - task_data.setData("input_program", program); - - // Create problem - TaskComposerProblem task_problem(env_, task_data); - - // Create input - auto task_input = std::make_shared(task_problem, profiles); - - // Create task - RasterFtOnlyGlobalPipelineTask task("input_program", "output_program"); - - // Solve task - TaskComposerFuture::UPtr future = executor->run(task, *task_input); - future->wait(); - - // Confirm that the task is finished - EXPECT_TRUE(future->ready()); - - // Solve - EXPECT_TRUE(task_input->isSuccessful()); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -}