From 77efd2a9ea7e911c56a340a3ed011a5c6a1c10de Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 16 Sep 2023 17:44:54 -0500 Subject: [PATCH 1/6] Rename TaskComposerInput to TaskComposerContext and simplify interfaces --- .../src/basic_cartesian_example.cpp | 9 +- tesseract_examples/src/car_seat_example.cpp | 16 +- .../src/freespace_hybrid_example.cpp | 9 +- .../src/freespace_ompl_example.cpp | 9 +- .../src/glass_upright_example.cpp | 9 +- .../src/pick_and_place_example.cpp | 19 +- .../puzzle_piece_auxillary_axes_example.cpp | 9 +- .../src/puzzle_piece_example.cpp | 9 +- tesseract_task_composer/core/CMakeLists.txt | 2 +- .../core/nodes/abort_task.h | 2 +- .../core/nodes/done_task.h | 2 +- .../core/nodes/error_task.h | 2 +- .../core/nodes/remap_task.h | 2 +- .../core/nodes/start_task.h | 2 +- .../core/nodes/sync_task.h | 2 +- ...mposer_input.h => task_composer_context.h} | 76 ++- .../core/task_composer_executor.h | 16 +- .../core/task_composer_future.h | 6 + .../core/task_composer_node.h | 2 +- .../core/task_composer_node_info.h | 8 +- .../core/task_composer_pipeline.h | 6 +- .../core/task_composer_problem.h | 3 + .../core/task_composer_server.h | 10 +- .../core/task_composer_task.h | 4 +- .../task_composer_executor_unit.hpp | 70 +- .../task_composer_node_info_unit.hpp | 2 +- .../core/test_suite/test_task.h | 2 +- .../core/src/nodes/abort_task.cpp | 5 +- .../core/src/nodes/done_task.cpp | 2 +- .../core/src/nodes/error_task.cpp | 2 +- .../core/src/nodes/remap_task.cpp | 5 +- .../core/src/nodes/start_task.cpp | 2 +- .../core/src/nodes/sync_task.cpp | 2 +- .../core/src/task_composer_context.cpp | 117 ++++ .../core/src/task_composer_executor.cpp | 5 + .../core/src/task_composer_input.cpp | 95 --- .../core/src/task_composer_node_info.cpp | 32 +- .../core/src/task_composer_pipeline.cpp | 30 +- .../core/src/task_composer_problem.cpp | 2 + .../core/src/task_composer_server.cpp | 12 +- .../core/src/task_composer_task.cpp | 10 +- .../core/src/test_suite/test_task.cpp | 5 +- .../examples/task_composer_example.cpp | 22 +- .../examples/task_composer_raster_example.cpp | 11 +- .../task_composer_trajopt_example.cpp | 9 +- .../planning/nodes/check_input_task.h | 2 +- .../nodes/continuous_contact_check_task.h | 2 +- .../nodes/discrete_contact_check_task.h | 2 +- .../planning/nodes/fix_state_bounds_task.h | 2 +- .../planning/nodes/fix_state_collision_task.h | 2 +- .../planning/nodes/format_as_input_task.h | 2 +- .../iterative_spline_parameterization_task.h | 2 +- .../planning/nodes/min_length_task.h | 2 +- .../planning/nodes/motion_planner_task.hpp | 10 +- .../planning/nodes/profile_switch_task.h | 2 +- .../planning/nodes/raster_motion_task.h | 2 +- .../planning/nodes/raster_only_motion_task.h | 2 +- .../nodes/ruckig_trajectory_smoothing_task.h | 2 +- .../time_optimal_parameterization_task.h | 2 +- .../planning/nodes/update_end_state_task.h | 2 +- .../nodes/update_start_and_end_state_task.h | 2 +- .../planning/nodes/update_start_state_task.h | 2 +- .../planning/nodes/upsample_trajectory_task.h | 2 +- .../planning/profiles/check_input_profile.h | 8 +- .../planning/src/nodes/check_input_task.cpp | 8 +- .../nodes/continuous_contact_check_task.cpp | 6 +- .../src/nodes/discrete_contact_check_task.cpp | 6 +- .../src/nodes/fix_state_bounds_task.cpp | 18 +- .../src/nodes/fix_state_collision_task.cpp | 32 +- .../src/nodes/format_as_input_task.cpp | 8 +- ...iterative_spline_parameterization_task.cpp | 12 +- .../planning/src/nodes/min_length_task.cpp | 10 +- .../src/nodes/profile_switch_task.cpp | 6 +- .../planning/src/nodes/raster_motion_task.cpp | 33 +- .../src/nodes/raster_only_motion_task.cpp | 25 +- .../ruckig_trajectory_smoothing_task.cpp | 12 +- .../time_optimal_parameterization_task.cpp | 12 +- .../src/nodes/update_end_state_task.cpp | 8 +- .../nodes/update_start_and_end_state_task.cpp | 10 +- .../src/nodes/update_start_state_task.cpp | 8 +- .../src/nodes/upsample_trajectory_task.cpp | 8 +- .../taskflow_task_composer_executor.h | 10 +- .../taskflow/taskflow_task_composer_future.h | 3 +- .../src/taskflow_task_composer_executor.cpp | 46 +- .../src/taskflow_task_composer_future.cpp | 15 +- tesseract_task_composer/test/CMakeLists.txt | 21 - .../test/fix_state_bounds_task_unit.cpp | 10 +- .../tesseract_task_composer_core_unit.cpp | 463 +++++++------- .../tesseract_task_composer_planning_unit.cpp | 202 +++--- ...eract_task_composer_serialization_unit.cpp | 136 ---- .../test/tesseract_task_composer_unit.cpp | 598 ------------------ 91 files changed, 897 insertions(+), 1557 deletions(-) rename tesseract_task_composer/core/include/tesseract_task_composer/core/{task_composer_input.h => task_composer_context.h} (57%) create mode 100644 tesseract_task_composer/core/src/task_composer_context.cpp delete mode 100644 tesseract_task_composer/core/src/task_composer_input.cpp delete mode 100644 tesseract_task_composer/test/tesseract_task_composer_serialization_unit.cpp delete mode 100644 tesseract_task_composer/test/tesseract_task_composer_unit.cpp diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 9900785aba8..d8d432c4c8b 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 @@ -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)); 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->getDataStorage().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..4be59b4de81 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 @@ -309,17 +309,16 @@ bool CarSeatExample::run() auto problem = std::make_unique(env_, input_data, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); 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->getDataStorage().getData(output_key).as(); tesseract_common::Toolpath toolpath = toToolpath(ci, *env_); tesseract_common::JointTrajectory trajectory = toJointTrajectory(ci); auto state_solver = env_->getStateSolver(); @@ -398,17 +397,16 @@ bool CarSeatExample::run() auto problem = std::make_unique(env_, input_data, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); 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->getDataStorage().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..90471a48ced 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 @@ -187,15 +187,14 @@ bool FreespaceHybridExample::run() auto problem = std::make_unique(env_, input_data, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); 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->getDataStorage().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..4e581fafb5c 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 @@ -187,15 +187,14 @@ bool FreespaceOMPLExample::run() auto problem = std::make_unique(env_, input_data, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); 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->getDataStorage().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..43107e07ee7 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 @@ -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)); 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->getDataStorage().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..a8b49c93b30 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 @@ -234,18 +234,17 @@ bool PickAndPlaceExample::run() auto pick_problem = std::make_unique(env_, pick_input_data, 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)); 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->getDataStorage().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->getDataStorage().getData(pick_output_key).as(); const MoveInstructionPoly* pick_final_state = pick_composite.getLastMoveInstruction(); // Retreat to the approach pose @@ -355,18 +355,17 @@ bool PickAndPlaceExample::run() auto place_problem = std::make_unique(env_, place_input_data, 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)); 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->getDataStorage().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..4d6809b0bf3 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 @@ -240,21 +240,20 @@ bool PuzzlePieceAuxillaryAxesExample::run() auto problem = std::make_unique(env_, input_data, profiles); // Solve task - TaskComposerInput input(std::move(problem)); - TaskComposerFuture::UPtr future = executor->run(*task, input); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); 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->getDataStorage().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..ece7ea5e3f8 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 @@ -234,8 +234,7 @@ bool PuzzlePieceExample::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)); 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->getDataStorage().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..e241d2b0b41 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(const TaskComposerContext::Ptr& 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..07b6c7bf3c6 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(const TaskComposerContext::Ptr& 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..410c3d83ec5 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(const TaskComposerContext::Ptr& 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..30f274ac18c 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(const TaskComposerContext::Ptr& 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..3255aa992c1 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(const TaskComposerContext::Ptr& 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..e90b378ea16 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(const TaskComposerContext::Ptr& 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 57% 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..2754aec0443 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 @@ -43,34 +44,35 @@ namespace tesseract_planning * 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 */ -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::UPtr problem); + 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& getProblem(); + const TaskComposerProblem& getProblem() const; /** * @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& getDataStorage(); + const TaskComposerDataStorage& getDataStorage() const; /** @brief The location where task info is stored during execution */ - TaskComposerNodeInfoContainer task_infos; - - /** @brief Indicate if dotgraph should be provided */ - bool dotgraph{ false }; + TaskComposerNodeInfoContainer& getTaskInfos(); + const TaskComposerNodeInfoContainer& getTaskInfos() const; /** * @brief Check if process has been aborted @@ -102,23 +104,41 @@ struct TaskComposerInput /** @brief Reset abort and data storage to constructed state */ void reset(); - bool operator==(const TaskComposerInput& rhs) const; - bool operator!=(const TaskComposerInput& rhs) const; + /** + * @brief Create a child context which is used for dynamic tasking + * @details + * - Everything is shared between the parent and the child except abort_ and task_infos_. + * - Task infos should be merged into the parent by the developer + */ + TaskComposerContext::UPtr createChild(); + + bool operator==(const TaskComposerContext& rhs) const; + bool operator!=(const TaskComposerContext& rhs) const; protected: 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 + /** @brief The 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::Ptr data_storage_; + + /** @brief The location where task info is stored during execution */ + TaskComposerNodeInfoContainer task_infos_; + mutable std::atomic aborted_{ false }; }; } // 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..c726f64463f 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,19 @@ 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 * @return The future associated with execution */ - virtual TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerInput& task_input) = 0; + TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerProblem::UPtr problem); + + /** + * @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; /** @brief Queries the number of workers (example: number of threads) */ virtual long getWorkerCount() const = 0; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_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..6ad6ec058c1 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(const TaskComposerContext::Ptr& 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor = std::nullopt) const; void runRecursive(const TaskComposerNode& node, - TaskComposerInput& input, + const TaskComposerContext::Ptr& 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..5801e3e9b36 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 @@ -59,6 +59,9 @@ struct TaskComposerProblem /** @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 * @return A clone of 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..21968e7bbad 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,22 @@ 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::UPtr problem, 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::UPtr problem, + 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..b13f3899068 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(const TaskComposerContext::Ptr& 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(const TaskComposerContext::Ptr& 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..518b2e9d522 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,13 @@ 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 future = executor->run(*task, std::move(problem)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -61,10 +61,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->getTaskInfos().getInfoMap().size(), 1); + EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -167,13 +167,13 @@ 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 future = executor->run(*pipeline, std::move(problem)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -196,10 +196,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->getTaskInfos().getInfoMap().size(), 6); + EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -232,13 +232,13 @@ 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 future = executor->run(*graph, std::move(problem)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -261,10 +261,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->getTaskInfos().getInfoMap().size(), 6); + EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -301,13 +301,13 @@ 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 future = executor->run(*graph, std::move(problem)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -335,10 +335,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->getTaskInfos().getInfoMap().size(), 6); + EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -371,13 +371,13 @@ 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 future = executor->run(*graph, std::move(problem)); future->wait(); EXPECT_TRUE(future->valid()); EXPECT_TRUE(future->ready()); @@ -400,10 +400,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->getTaskInfos().getInfoMap().size(), 6); + EXPECT_TRUE(future->context->getTaskInfos().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..2a2a69532b9 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(const TaskComposerContext::Ptr& 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..1dfeb5e6eef 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(const TaskComposerContext::Ptr& 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..4cc9b54f993 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(const TaskComposerContext::Ptr& /*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..e7ee4df0b92 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(const TaskComposerContext::Ptr& /*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..49e4f14ef60 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(const TaskComposerContext::Ptr& context, + OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); - if (input.data_storage.remapData(remap_, copy_)) + if (context->getDataStorage().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..62ece9466c7 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(const TaskComposerContext::Ptr& /*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..52070ad4642 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(const TaskComposerContext::Ptr& /*context*/, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); diff --git a/tesseract_task_composer/core/src/task_composer_context.cpp b/tesseract_task_composer/core/src/task_composer_context.cpp new file mode 100644 index 00000000000..d835a64f683 --- /dev/null +++ b/tesseract_task_composer/core/src/task_composer_context.cpp @@ -0,0 +1,117 @@ +/** + * @file task_composer_context.cpp + * @brief The context data structure to the pipeline + * + * @author Levi Armstrong + * @date July 29. 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500) +#include +#endif +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +TaskComposerContext::TaskComposerContext(TaskComposerProblem::UPtr problem) + : problem_(std::move(problem)), data_storage_(std::make_shared(this->problem_->input_data)) +{ +} + +TaskComposerProblem& TaskComposerContext::getProblem() { return *problem_; } +const TaskComposerProblem& TaskComposerContext::getProblem() const { return *problem_; } + +TaskComposerDataStorage& TaskComposerContext::getDataStorage() { return *data_storage_; } +const TaskComposerDataStorage& TaskComposerContext::getDataStorage() const { return *data_storage_; } + +TaskComposerNodeInfoContainer& TaskComposerContext::getTaskInfos() { return task_infos_; } +const TaskComposerNodeInfoContainer& TaskComposerContext::getTaskInfos() const { return task_infos_; } + +bool TaskComposerContext::isAborted() const { return aborted_; } + +bool TaskComposerContext::isSuccessful() const { return !aborted_; } + +void TaskComposerContext::abort(const boost::uuids::uuid& calling_node) +{ + if (!calling_node.is_nil()) + task_infos_.setAborted(calling_node); + + aborted_ = true; +} + +void TaskComposerContext::reset() +{ + aborted_ = false; + data_storage_ = std::make_shared(problem_->input_data); + task_infos_.clear(); +} + +/** @brief Create a copy */ +TaskComposerContext::UPtr TaskComposerContext::createChild() +{ + auto child = std::make_unique(); + child->problem_ = problem_; + child->data_storage_ = data_storage_; + return child; +} + +bool TaskComposerContext::operator==(const TaskComposerContext& rhs) const +{ + bool equal = true; + if (problem_ != nullptr && rhs.problem_ != nullptr) + equal &= (*problem_ == *rhs.problem_); + else + equal &= (problem_ == nullptr && rhs.problem_ == nullptr); + + 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 TaskComposerContext::operator!=(const TaskComposerContext& rhs) const { return !operator==(rhs); } + +template +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_); + ar& boost::serialization::make_nvp("task_infos", task_infos_); + ar& boost::serialization::make_nvp("aborted", aborted_); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerContext) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerContext) diff --git a/tesseract_task_composer/core/src/task_composer_executor.cpp b/tesseract_task_composer/core/src/task_composer_executor.cpp index 92b771e1e6f..5b4f374a920 100644 --- a/tesseract_task_composer/core/src/task_composer_executor.cpp +++ b/tesseract_task_composer/core/src/task_composer_executor.cpp @@ -37,6 +37,11 @@ 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::UPtr problem) +{ + return run(node, std::make_shared(std::move(problem))); +} + bool TaskComposerExecutor::operator==(const TaskComposerExecutor& rhs) const { return (name_ == rhs.name_); } // LCOV_EXCL_START diff --git a/tesseract_task_composer/core/src/task_composer_input.cpp b/tesseract_task_composer/core/src/task_composer_input.cpp deleted file mode 100644 index 4ea467eab08..00000000000 --- a/tesseract_task_composer/core/src/task_composer_input.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/** - * @file task_composer_input.cpp - * @brief The input data structure to the pipeline - * - * @author Levi Armstrong - * @date July 29. 2022 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2022, Levi Armstrong - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500) -#include -#endif -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -TaskComposerInput::TaskComposerInput(TaskComposerProblem::UPtr problem) - : problem(std::move(problem)), data_storage(this->problem->input_data) -{ -} - -bool TaskComposerInput::isAborted() const { return aborted_; } - -bool TaskComposerInput::isSuccessful() const { return !aborted_; } - -void TaskComposerInput::abort(const boost::uuids::uuid& calling_node) -{ - if (!calling_node.is_nil()) - task_infos.setAborted(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 equal = true; - if (problem != nullptr && rhs.problem != nullptr) - equal &= (*problem == *rhs.problem); - else - equal &= (problem == nullptr && rhs.problem == nullptr); - - equal &= data_storage == rhs.data_storage; - equal &= task_infos == rhs.task_infos; - equal &= aborted_ == rhs.aborted_; - return equal; -} - -bool TaskComposerInput::operator!=(const TaskComposerInput& rhs) const { return !operator==(rhs); } - -template -void TaskComposerInput::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& boost::serialization::make_nvp("problem", problem); - ar& boost::serialization::make_nvp("data_storage", data_storage); - ar& boost::serialization::make_nvp("task_infos", task_infos); - ar& boost::serialization::make_nvp("aborted", aborted_); -} - -} // namespace tesseract_planning - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TaskComposerInput) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TaskComposerInput) 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..72953de1fc9 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -99,7 +99,7 @@ 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 }; @@ -107,7 +107,7 @@ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(const TaskComposerN } 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 }; @@ -119,16 +119,16 @@ 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); } 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 }; info_map_ = std::move(other.info_map_); @@ -185,6 +185,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 { diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp index 9828644528e..b6336fd794c 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(const TaskComposerContext::Ptr& 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->getTaskInfos().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->getTaskInfos().addInfo(std::move(results)); return value; } -TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& input, +TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(const TaskComposerContext::Ptr& 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->getTaskInfos().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, + const TaskComposerContext::Ptr& 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..dd0ab2b55cc 100644 --- a/tesseract_task_composer/core/src/task_composer_problem.cpp +++ b/tesseract_task_composer/core/src/task_composer_problem.cpp @@ -51,6 +51,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; } @@ -61,6 +62,7 @@ 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..c12716f8404 100644 --- a/tesseract_task_composer/core/src/task_composer_server.cpp +++ b/tesseract_task_composer/core/src/task_composer_server.cpp @@ -104,28 +104,28 @@ std::vector TaskComposerServer::getAvailableTasks() const return tasks; } -TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerInput& task_input, const std::string& name) +TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerProblem::UPtr problem, 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)); } TaskComposerFuture::UPtr TaskComposerServer::run(const TaskComposerNode& node, - TaskComposerInput& task_input, + TaskComposerProblem::UPtr problem, 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)); } 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..f8b5fa2d675 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(const TaskComposerContext::Ptr& 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->getTaskInfos().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->getTaskInfos().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..663d7b27e40 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(const TaskComposerContext::Ptr& 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..a17ee161881 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(const TaskComposerContext::Ptr& 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); + double result = context->getDataStorage().getData(left_key_).as() + + context->getDataStorage().getData(right_key_).as(); + context->getDataStorage().setData(output_key_, result); return info; } @@ -49,15 +49,15 @@ class MultiplyTaskComposerNode : public TaskComposerTask { } - TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input, + TaskComposerNodeInfo::UPtr runImpl(const TaskComposerContext::Ptr& 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); + double result = context->getDataStorage().getData(left_key_).as() * + context->getDataStorage().getData(right_key_).as(); + context->getDataStorage().setData(output_key_, result); return info; } @@ -81,8 +81,6 @@ int main() auto task_problem = std::make_unique(task_data); - auto task_input = std::make_shared(std::move(task_problem)); - // result = a * (b + c) + d auto task1 = std::make_unique("b", "c", "task1_output"); auto task2 = std::make_unique("a", "task1_output", "task2_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)); future->wait(); - std::cout << "Output: " << task_input->data_storage.getData("task3_output").as() << std::endl; + std::cout << "Output: " << future->context->getDataStorage().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..f3df5556900 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -62,24 +62,21 @@ int main() // 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; + 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)); 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->getTaskInfos().getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = task_input->data_storage.getData(output_key).as(); + auto output_program = future->context->getDataStorage().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..3afebe439ab 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -63,21 +63,18 @@ int main() // 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_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(*task, *task_input); + TaskComposerFuture::UPtr future = task_executor->run(*task, std::move(task_problem)); 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->getTaskInfos().getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = task_input->data_storage.getData(output_key).as(); + auto output_program = future->context->getDataStorage().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..fbb882d33f9 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(const TaskComposerContext::Ptr& 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..3891844283d 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(const TaskComposerContext::Ptr& 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..b3531c73148 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(const TaskComposerContext::Ptr& 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..acd312fd5cc 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(const TaskComposerContext::Ptr& 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..b41949072c7 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(const TaskComposerContext::Ptr& 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..2fd458f30b6 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(const TaskComposerContext::Ptr& 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..c878c07d460 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(const TaskComposerContext::Ptr& 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..a95797fcbd0 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(const TaskComposerContext::Ptr& 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..c36b108d02f 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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..d753342620f 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(const TaskComposerContext::Ptr& 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..41203a0e974 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(const TaskComposerContext::Ptr& 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..9706b8a6d2d 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(const TaskComposerContext::Ptr& 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..341896cdcf6 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(const TaskComposerContext::Ptr& 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..957b2cce93d 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(const TaskComposerContext::Ptr& 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..88140db8a5e 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(const TaskComposerContext::Ptr& 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..6076a1b8820 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(const TaskComposerContext::Ptr& 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..dd640951bfd 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(const TaskComposerContext::Ptr& 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..5ff8b93df14 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; 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..66c8d2250f3 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.getProblem()); // 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..8fd8dc261af 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); // 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->getDataStorage().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..b49cd7a6af2 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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..42556b73749 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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..405efce2865 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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->getDataStorage().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..70d6a049e69 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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..70538083346 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(const TaskComposerContext::Ptr& 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->getDataStorage().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->getDataStorage().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->getDataStorage().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..0c39d49102c 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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..6f458dc834a 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], response.results); } else { - input.data_storage.setData(output_keys_[0], ci); + context->getDataStorage().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..d24ee71e3c2 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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..34d747749cc 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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,17 @@ 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->getDataStorage().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); future->wait(); - auto info_map = input.task_infos.getInfoMap(); + auto info_map = context->getTaskInfos().getInfoMap(); - if (input.dotgraph) + if (context->getProblem().dotgraph) { std::stringstream dot_graph; dot_graph << "subgraph cluster_" << toString(uuid_) << " {\n color=black;\n label = \"" << name_ << "\\n(" @@ -471,7 +471,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 +479,28 @@ 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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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..abe24a13e4c 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().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->getDataStorage().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,12 @@ 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); future->wait(); - auto info_map = input.task_infos.getInfoMap(); + auto info_map = context->getTaskInfos().getInfoMap(); - if (input.dotgraph) + if (context->getProblem().dotgraph) { std::stringstream dot_graph; dot_graph << "subgraph cluster_" << toString(uuid_) << " {\n color=black;\n label = \"" << name_ << "\\n(" @@ -378,7 +378,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 +388,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->getDataStorage().getData(raster_tasks[i].second.second).as(); if (i != 0) segment.erase(segment.begin()); @@ -397,13 +398,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->getDataStorage().getData(transition_keys[i].second).as(); transition.erase(transition.begin()); program.emplace_back(transition); } } - input.data_storage.setData(output_keys_[0], program); + context->getDataStorage().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..5e851de1bc0 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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..d2eb06fe2cb 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().setData(output_keys_[0], context->getDataStorage().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->getDataStorage().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..77287eb2a9f 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(const TaskComposerContext::Ptr& 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->getDataStorage().getData(input_keys_[0]); + auto input_next_data_poly = context->getDataStorage().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->getDataStorage().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..8fec3d2d596 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(const TaskComposerContext::Ptr& 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->getDataStorage().getData(input_keys_[0]); + auto input_prev_data_poly = context->getDataStorage().getData(input_keys_[1]); + auto input_next_data_poly = context->getDataStorage().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->getDataStorage().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..bc6ee8a053b 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(const TaskComposerContext::Ptr& 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->getDataStorage().getData(input_keys_[0]); + auto input_prev_data_poly = context->getDataStorage().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->getDataStorage().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..4ac7fd6f186 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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(*input.problem); + auto& problem = dynamic_cast(context->getProblem()); 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->getDataStorage().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->getDataStorage().setData(output_keys_[0], new_results); info->color = "green"; info->message = "Successful"; 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..19263e3d093 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,7 +59,7 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor TaskflowTaskComposerExecutor(TaskflowTaskComposerExecutor&&) = delete; TaskflowTaskComposerExecutor& operator=(TaskflowTaskComposerExecutor&&) = delete; - TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerInput& task_input) override final; + TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerContext::Ptr context) override final; long getWorkerCount() const override final; @@ -86,16 +86,18 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor static std::shared_ptr>> convertToTaskflow(const TaskComposerGraph& task_graph, - TaskComposerInput& task_input, + const TaskComposerContext::Ptr& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> convertToTaskflow(const TaskComposerPipeline& task_pipeline, - TaskComposerInput& task_input, + const TaskComposerContext::Ptr& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> - convertToTaskflow(const TaskComposerTask& task, TaskComposerInput& task_input, TaskComposerExecutor& task_executor); + convertToTaskflow(const TaskComposerTask& task, + const TaskComposerContext::Ptr& 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..5345b9fd5f2 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, + const TaskComposerContext::Ptr& 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->getTaskInfos().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, + const TaskComposerContext::Ptr& 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, + const TaskComposerContext::Ptr& 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..dbfc7f78f4c 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 @@ -87,8 +87,8 @@ void checkProgram(const Environment::Ptr& env, // Create problem auto task_problem = std::make_unique(env, task_data, 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)); // 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->getDataStorage().getData("output_program").as(); auto task_flattened = task_program.flatten(moveFilter); switch (setting) 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..067d9a36c6e 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,35 @@ 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()); + EXPECT_FALSE(context->isAborted()); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_TRUE(context->getTaskInfos().getInfoMap().empty()); + context->getTaskInfos().addInfo(std::make_unique(node)); + context->abort(node.getUUID()); + EXPECT_EQ(context->getTaskInfos().getAbortingNode(), node.getUUID()); + EXPECT_TRUE(context->isAborted()); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_EQ(context->getTaskInfos().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"); + + context->reset(); + EXPECT_NO_THROW(context->getProblem()); // NOLINT + EXPECT_FALSE(context->isAborted()); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_TRUE(context->getTaskInfos().getInfoMap().empty()); } TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT { auto problem = std::make_unique(); EXPECT_EQ(problem->name, "unset"); + EXPECT_FALSE(problem->dotgraph); // Serialization test_suite::runSerializationPointerTest(problem, "TaskComposerProblemTests"); @@ -155,10 +154,12 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT EXPECT_EQ(problem2->name, "TaskComposerProblemTests"); auto problem3 = std::make_unique(TaskComposerDataStorage(), "TaskComposerProblemTests"); + problem3->dotgraph = true; EXPECT_EQ(problem3->name, "TaskComposerProblemTests"); auto problem_clone = problem3->clone(); EXPECT_EQ(*problem3, *problem_clone); + EXPECT_TRUE(problem_clone->dotgraph); } TEST(TesseractTaskComposerCoreUnit, TaskComposerNodeInfoContainerTests) // NOLINT @@ -289,16 +290,16 @@ 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()); + EXPECT_EQ(task->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().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->getTaskInfos().getInfoMap())); // NOLINT } { // Conditional @@ -312,16 +313,16 @@ 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()); + EXPECT_EQ(task->run(context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().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->getTaskInfos().getInfoMap())); // NOLINT } { @@ -342,16 +343,16 @@ 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()); + EXPECT_EQ(task->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().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->getTaskInfos().getInfoMap())); // NOLINT } { @@ -373,16 +374,16 @@ 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()); + EXPECT_EQ(task->run(context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().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->getTaskInfos().getInfoMap())); // NOLINT } { // Failure due to exception during run @@ -393,14 +394,14 @@ 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()); 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->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 0); } } @@ -479,12 +480,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 5); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test1a.dot"); @@ -493,7 +494,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -534,12 +535,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 4); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test2a.dot"); @@ -548,7 +549,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -590,12 +591,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 4); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test3a.dot"); @@ -604,7 +605,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -646,12 +647,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_TRUE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 4); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test4a.dot"); @@ -660,7 +661,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -704,12 +705,12 @@ 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()); + EXPECT_EQ(pipeline3->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 9); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test5a.dot"); @@ -718,7 +719,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -764,12 +765,12 @@ 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()); + EXPECT_EQ(pipeline3->run(context), 1); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 9); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test6a.dot"); @@ -778,7 +779,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -825,12 +826,12 @@ 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()); + EXPECT_EQ(pipeline3->run(context), 1); + EXPECT_FALSE(context->isSuccessful()); + EXPECT_TRUE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 6); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 1); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test7a.dot"); @@ -839,7 +840,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -994,12 +995,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test8a.dot"); @@ -1008,7 +1009,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -1025,12 +1026,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test9a.dot"); @@ -1039,7 +1040,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -1055,12 +1056,12 @@ 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()); + EXPECT_EQ(pipeline->run(context), 0); + EXPECT_TRUE(context->isSuccessful()); + EXPECT_FALSE(context->isAborted()); + EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 2); + EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); std::ofstream os1; os1.open(tesseract_common::getTempPath() + "task_composer_pipeline_test10a.dot"); @@ -1069,7 +1070,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->getTaskInfos().getInfoMap())); // NOLINT os2.close(); } @@ -1532,17 +1533,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerAbortTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(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->getTaskInfos().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->getTaskInfos().getAbortingNode(), task.getUUID()); } } @@ -1578,17 +1579,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerErrorTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(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->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } } @@ -1624,17 +1625,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerDoneTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(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->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } } @@ -1684,53 +1685,53 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT { // Test run method copy auto problem = std::make_unique(); problem->input_data.setData(key, js); - auto input = std::make_unique(std::move(problem)); + auto context = std::make_shared(std::move(problem)); 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->getDataStorage().hasKey(key)); + EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); + EXPECT_EQ(context->getDataStorage().getData(key), context->getDataStorage().getData(remap_key)); + auto node_info = context->getTaskInfos().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->getTaskInfos().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 context = std::make_shared(std::move(problem)); 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->getDataStorage().hasKey(key)); + EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); + EXPECT_EQ(context->getDataStorage().getData(remap_key).as(), js); + auto node_info = context->getTaskInfos().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->getTaskInfos().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 context = std::make_shared(std::move(problem)); TaskComposerPluginFactory factory; std::string str = R"(config: @@ -1741,24 +1742,24 @@ 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->getDataStorage().hasKey(key)); + EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); + EXPECT_EQ(context->getDataStorage().getData(key), context->getDataStorage().getData(remap_key)); + auto node_info = context->getTaskInfos().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->getTaskInfos().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 context = std::make_shared(std::move(problem)); TaskComposerPluginFactory factory; std::string str = R"(config: @@ -1769,18 +1770,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->getDataStorage().hasKey(key)); + EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); + EXPECT_EQ(context->getDataStorage().getData(remap_key).as(), js); + auto node_info = context->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } { // Failures @@ -1810,45 +1811,45 @@ 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 context = std::make_shared(std::move(problem)); 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->getDataStorage().hasKey(key)); + EXPECT_FALSE(context->getDataStorage().hasKey(remap_key)); + auto node_info = context->getTaskInfos().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->getTaskInfos().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 context = std::make_shared(std::move(problem)); 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->getDataStorage().hasKey(key)); + EXPECT_FALSE(context->getDataStorage().hasKey(remap_key)); + auto node_info = context->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } } @@ -1905,17 +1906,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(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->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } } @@ -1972,17 +1973,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT } { // Test run method - auto input = std::make_unique(std::make_unique()); + auto context = std::make_shared(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->getTaskInfos().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->getTaskInfos().getAbortingNode().is_nil()); } } @@ -2082,41 +2083,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(); + problem->name = "TestPipeline"; + auto future = server.run(std::move(problem), "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->getTaskInfos().getInfoMap().size(), 4); + EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); } { // Run method using Pipeline - auto input = std::make_unique(std::make_unique()); - input->problem->name = "TestPipeline"; + auto problem = std::make_unique(); + problem->name = "TestPipeline"; const auto& pipeline = server.getTask("TestPipeline"); - auto future = server.run(pipeline, *input, "TaskflowExecutor"); + auto future = server.run(pipeline, std::move(problem), "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->getTaskInfos().getInfoMap().size(), 4); + EXPECT_TRUE(future->context->getTaskInfos().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(); + problem->name = "TestPipeline"; + EXPECT_ANY_THROW(server.run(std::move(problem), "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(); + problem->name = "DoesNotExist"; + EXPECT_ANY_THROW(server.run(std::move(problem), "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..b730abd367c 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->getTaskInfos().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().getData("output_data")); + EXPECT_EQ(input->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().getData("output_data")); + EXPECT_EQ(input->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().getData("output_data")); + EXPECT_EQ(input->getDataStorage().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->getDataStorage().getData("output_data")); + EXPECT_EQ(context2->getDataStorage().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->getDataStorage().getData("output_data").as().size(), 17); + EXPECT_TRUE(context->getTaskInfos().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->getDataStorage().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->getDataStorage().getData("output_data")); + EXPECT_GE(input->getDataStorage().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->getDataStorage().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->getDataStorage().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->getDataStorage().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(); -} From 24a84b1c9f70b26d4c0015448aa4cfb22f596f77 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 17 Sep 2023 07:53:32 -0500 Subject: [PATCH 2/6] fixup --- .../core/src/task_composer_node_info.cpp | 10 ++++++++-- .../test/tesseract_task_composer_core_unit.cpp | 7 +++++++ 2 files changed, 15 insertions(+), 2 deletions(-) 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 72953de1fc9..fc2c853a896 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -103,7 +103,9 @@ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(const TaskComposerN 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) { @@ -111,6 +113,7 @@ TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const Ta 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(); @@ -123,7 +126,8 @@ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(TaskComposerNodeInf 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 { @@ -131,6 +135,7 @@ TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(TaskComp 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; @@ -252,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/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp index 067d9a36c6e..d97fe267618 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -168,10 +168,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"); @@ -180,15 +184,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 From 7fc216b87c9143280ff20163f595ae27cc3f6fab Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 18 Sep 2023 17:42:00 -0500 Subject: [PATCH 3/6] Make requested changes --- .../src/basic_cartesian_example.cpp | 10 +- tesseract_examples/src/car_seat_example.cpp | 20 +- .../src/freespace_hybrid_example.cpp | 10 +- .../src/freespace_ompl_example.cpp | 10 +- .../src/glass_upright_example.cpp | 10 +- .../src/pick_and_place_example.cpp | 23 +- .../puzzle_piece_auxillary_axes_example.cpp | 10 +- .../src/puzzle_piece_example.cpp | 10 +- .../core/nodes/abort_task.h | 2 +- .../core/nodes/done_task.h | 2 +- .../core/nodes/error_task.h | 2 +- .../core/nodes/remap_task.h | 2 +- .../core/nodes/start_task.h | 2 +- .../core/nodes/sync_task.h | 2 +- .../core/task_composer_context.h | 48 +-- .../core/task_composer_executor.h | 23 +- .../core/task_composer_pipeline.h | 6 +- .../core/task_composer_problem.h | 6 +- .../core/task_composer_server.h | 7 +- .../core/task_composer_task.h | 4 +- .../task_composer_executor_unit.hpp | 35 +- .../core/test_suite/test_task.h | 2 +- .../core/src/nodes/abort_task.cpp | 4 +- .../core/src/nodes/done_task.cpp | 2 +- .../core/src/nodes/error_task.cpp | 2 +- .../core/src/nodes/remap_task.cpp | 4 +- .../core/src/nodes/start_task.cpp | 2 +- .../core/src/nodes/sync_task.cpp | 2 +- .../core/src/task_composer_context.cpp | 52 +-- .../core/src/task_composer_data_storage.cpp | 19 +- .../core/src/task_composer_executor.cpp | 6 +- .../core/src/task_composer_pipeline.cpp | 14 +- .../core/src/task_composer_problem.cpp | 9 +- .../core/src/task_composer_server.cpp | 11 +- .../core/src/task_composer_task.cpp | 8 +- .../core/src/test_suite/test_task.cpp | 4 +- .../examples/task_composer_example.cpp | 32 +- .../examples/task_composer_raster_example.cpp | 12 +- .../task_composer_trajopt_example.cpp | 13 +- .../planning/nodes/check_input_task.h | 2 +- .../nodes/continuous_contact_check_task.h | 2 +- .../nodes/discrete_contact_check_task.h | 2 +- .../planning/nodes/fix_state_bounds_task.h | 2 +- .../planning/nodes/fix_state_collision_task.h | 2 +- .../planning/nodes/format_as_input_task.h | 2 +- .../iterative_spline_parameterization_task.h | 2 +- .../planning/nodes/min_length_task.h | 2 +- .../planning/nodes/motion_planner_task.hpp | 10 +- .../planning/nodes/profile_switch_task.h | 2 +- .../planning/nodes/raster_motion_task.h | 2 +- .../planning/nodes/raster_only_motion_task.h | 2 +- .../nodes/ruckig_trajectory_smoothing_task.h | 2 +- .../time_optimal_parameterization_task.h | 2 +- .../planning/nodes/update_end_state_task.h | 2 +- .../nodes/update_start_and_end_state_task.h | 2 +- .../planning/nodes/update_start_state_task.h | 2 +- .../planning/nodes/upsample_trajectory_task.h | 2 +- .../planning/planning_task_composer_problem.h | 8 +- .../planning/profiles/check_input_profile.h | 2 +- .../planning/src/nodes/check_input_task.cpp | 8 +- .../nodes/continuous_contact_check_task.cpp | 6 +- .../src/nodes/discrete_contact_check_task.cpp | 6 +- .../src/nodes/fix_state_bounds_task.cpp | 18 +- .../src/nodes/fix_state_collision_task.cpp | 32 +- .../src/nodes/format_as_input_task.cpp | 8 +- ...iterative_spline_parameterization_task.cpp | 12 +- .../planning/src/nodes/min_length_task.cpp | 10 +- .../src/nodes/profile_switch_task.cpp | 6 +- .../planning/src/nodes/raster_motion_task.cpp | 35 +- .../src/nodes/raster_only_motion_task.cpp | 24 +- .../ruckig_trajectory_smoothing_task.cpp | 12 +- .../time_optimal_parameterization_task.cpp | 12 +- .../src/nodes/update_end_state_task.cpp | 8 +- .../nodes/update_start_and_end_state_task.cpp | 10 +- .../src/nodes/update_start_state_task.cpp | 8 +- .../src/nodes/upsample_trajectory_task.cpp | 8 +- .../src/planning_task_composer_problem.cpp | 18 +- .../taskflow_task_composer_executor.h | 10 +- .../src/taskflow_task_composer_executor.cpp | 26 +- .../test/fix_state_bounds_task_unit.cpp | 12 +- .../test/fix_state_collision_task_unit.cpp | 24 +- .../tesseract_task_composer_core_unit.cpp | 367 +++++++++--------- .../tesseract_task_composer_planning_unit.cpp | 66 ++-- 83 files changed, 611 insertions(+), 631 deletions(-) diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index d8d432c4c8b..06b055087e5 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -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,7 +256,7 @@ bool BasicCartesianExample::run() // Solve task tesseract_common::Timer stopwatch; stopwatch.start(); - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -266,7 +266,7 @@ bool BasicCartesianExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = future->context->getDataStorage().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/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 4be59b4de81..4e045999866 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -302,14 +302,14 @@ 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::unique_ptr(); + 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 - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); if (!future->context->isSuccessful()) @@ -318,7 +318,7 @@ bool CarSeatExample::run() // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { - auto ci = future->context->getDataStorage().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(); @@ -390,14 +390,14 @@ 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::unique_ptr(); + 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 - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); if (!future->context->isSuccessful()) @@ -406,7 +406,7 @@ bool CarSeatExample::run() // Plot Process Trajectory if (plotter_ != nullptr && plotter_->isConnected()) { - auto ci = future->context->getDataStorage().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 90471a48ced..ef3f261e3a3 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -180,21 +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 - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + 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 = future->context->getDataStorage().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_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 4e581fafb5c..524c9664fd5 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -180,21 +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 - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + 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 = future->context->getDataStorage().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/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 43107e07ee7..308731f0152 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -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,7 +249,7 @@ bool GlassUprightExample::run() // Solve process plan tesseract_common::Timer stopwatch; stopwatch.start(); - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -259,7 +259,7 @@ bool GlassUprightExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = future->context->getDataStorage().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/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index a8b49c93b30..f622eb53365 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -227,14 +227,14 @@ 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 - TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem)); + TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem), std::move(pick_input_data)); pick_future->wait(); if (!pick_future->context->isSuccessful()) @@ -244,7 +244,7 @@ bool PickAndPlaceExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = pick_future->context->getDataStorage().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 +278,7 @@ bool PickAndPlaceExample::run() // Get the last move instruction CompositeInstruction pick_composite = - pick_future->context->getDataStorage().getData(pick_output_key).as(); + pick_future->context->data_storage->getData(pick_output_key).as(); const MoveInstructionPoly* pick_final_state = pick_composite.getLastMoveInstruction(); // Retreat to the approach pose @@ -348,14 +348,15 @@ 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 - TaskComposerFuture::UPtr place_future = executor->run(*place_task, std::move(place_problem)); + TaskComposerFuture::UPtr place_future = + executor->run(*place_task, std::move(place_problem), std::move(place_input_data)); place_future->wait(); if (!place_future->context->isSuccessful()) @@ -365,7 +366,7 @@ bool PickAndPlaceExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = place_future->context->getDataStorage().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 4d6809b0bf3..4452358818f 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -230,24 +230,24 @@ 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 - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + 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 = future->context->getDataStorage().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); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index ece7ea5e3f8..eaa2cda9662 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -222,19 +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(); - TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); + TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem), std::move(input_data)); future->wait(); stopwatch.stop(); @@ -244,7 +244,7 @@ bool PuzzlePieceExample::run() if (plotter_ != nullptr && plotter_->isConnected()) { plotter_->waitForInput(); - auto ci = future->context->getDataStorage().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); 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 e241d2b0b41..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(const TaskComposerContext::Ptr& context, + 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 07b6c7bf3c6..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(const TaskComposerContext::Ptr& context, + 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 410c3d83ec5..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(const TaskComposerContext::Ptr& context, + 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 30f274ac18c..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(const TaskComposerContext::Ptr& context, + 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 3255aa992c1..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(const TaskComposerContext::Ptr& context, + 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 e90b378ea16..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(const TaskComposerContext::Ptr& context, + 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_context.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h index 2754aec0443..8f5b76bc991 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_context.h @@ -41,8 +41,14 @@ 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 TaskComposerContext { @@ -52,7 +58,7 @@ struct TaskComposerContext using ConstUPtr = std::unique_ptr; TaskComposerContext() = default; // Required for serialization - TaskComposerContext(TaskComposerProblem::UPtr problem); + TaskComposerContext(TaskComposerProblem::Ptr problem, TaskComposerDataStorage::Ptr data_storage); TaskComposerContext(const TaskComposerContext&) = delete; TaskComposerContext(TaskComposerContext&&) noexcept = delete; TaskComposerContext& operator=(const TaskComposerContext&) = delete; @@ -60,19 +66,16 @@ struct TaskComposerContext virtual ~TaskComposerContext() = default; /** @brief The problem */ - TaskComposerProblem& getProblem(); - const TaskComposerProblem& getProblem() const; + 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& getDataStorage(); - const TaskComposerDataStorage& getDataStorage() const; + TaskComposerDataStorage::Ptr data_storage; - /** @brief The location where task info is stored during execution */ - TaskComposerNodeInfoContainer& getTaskInfos(); - const TaskComposerNodeInfoContainer& getTaskInfos() const; + /** @brief Container for meta-data generated by task(s) during execution */ + TaskComposerNodeInfoContainer task_infos; /** * @brief Check if process has been aborted @@ -101,39 +104,16 @@ struct TaskComposerContext */ void abort(const TaskComposerNode& caller); - /** @brief Reset abort and data storage to constructed state */ - void reset(); - - /** - * @brief Create a child context which is used for dynamic tasking - * @details - * - Everything is shared between the parent and the child except abort_ and task_infos_. - * - Task infos should be merged into the parent by the developer - */ - TaskComposerContext::UPtr createChild(); - bool operator==(const TaskComposerContext& rhs) const; bool operator!=(const TaskComposerContext& rhs) const; -protected: +private: friend struct tesseract_common::Serialization; friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT - /** @brief The 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::Ptr data_storage_; - - /** @brief The location where task info is stored during execution */ - TaskComposerNodeInfoContainer task_infos_; - mutable std::atomic aborted_{ false }; }; } // namespace tesseract_planning 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 c726f64463f..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 @@ -58,18 +58,12 @@ class TaskComposerExecutor * @brief Execute the provided node * @param node The node to execute * @param problem The problem + * @param data_storage The data storage object to leverage * @return The future associated with execution */ - TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerProblem::UPtr problem); - - /** - * @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; + 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; @@ -88,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_pipeline.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h index 6ad6ec058c1..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(const TaskComposerContext::Ptr& context, 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(const TaskComposerContext::Ptr& context, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const; void runRecursive(const TaskComposerNode& node, - const TaskComposerContext::Ptr& context, + 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 5801e3e9b36..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,9 +55,6 @@ 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 }; 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 21968e7bbad..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 @@ -120,7 +120,9 @@ class TaskComposerServer * @param name The name of the executor to use * @return The future associated with execution */ - TaskComposerFuture::UPtr run(TaskComposerProblem::UPtr problem, const std::string& name); + TaskComposerFuture::UPtr run(TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, + const std::string& name); /** * @brief Execute the provided node @@ -130,7 +132,8 @@ class TaskComposerServer * @return The future associated with execution */ TaskComposerFuture::UPtr run(const TaskComposerNode& node, - TaskComposerProblem::UPtr problem, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage, const std::string& name); /** @brief Queries the number of workers (example: number of threads) */ 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 b13f3899068..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(const TaskComposerContext::Ptr& context, 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(const TaskComposerContext::Ptr& context, + 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 518b2e9d522..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 @@ -48,7 +48,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(executor->getTaskCount(), 0); auto problem = std::make_unique(); - auto future = executor->run(*task, std::move(problem)); + 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()); @@ -63,8 +64,8 @@ void runTaskComposerExecutorTest() EXPECT_FALSE(task->isConditional()); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 1); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 1); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -173,7 +174,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(executor->getTaskCount(), 0); auto problem = std::make_unique(); - auto future = executor->run(*pipeline, std::move(problem)); + 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()); @@ -198,8 +200,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getOutboundEdges().size(), 0); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 6); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -238,7 +240,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(executor->getTaskCount(), 0); auto problem = std::make_unique(); - auto future = executor->run(*graph, std::move(problem)); + 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()); @@ -263,8 +266,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getOutboundEdges().size(), 0); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 6); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -307,7 +310,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(executor->getTaskCount(), 0); auto problem = std::make_unique(); - auto future = executor->run(*graph, std::move(problem)); + 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()); @@ -337,8 +341,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task3->getOutboundEdges().size(), 0); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 6); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 6); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); future->clear(); EXPECT_FALSE(future->valid()); @@ -377,7 +381,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(executor->getTaskCount(), 0); auto problem = std::make_unique(); - auto future = executor->run(*graph, std::move(problem)); + 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()); @@ -402,8 +407,8 @@ void runTaskComposerExecutorTest() EXPECT_EQ(task2->getOutboundEdges().size(), 0); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 6); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + 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/test_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_task.h index 2a2a69532b9..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(const TaskComposerContext::Ptr& context, + 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 1dfeb5e6eef..5769251cfbc 100644 --- a/tesseract_task_composer/core/src/nodes/abort_task.cpp +++ b/tesseract_task_composer/core/src/nodes/abort_task.cpp @@ -39,14 +39,14 @@ AbortTask::AbortTask(std::string name, const YAML::Node& config, const TaskCompo { } -TaskComposerNodeInfo::UPtr AbortTask::runImpl(const TaskComposerContext::Ptr& context, +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"; - context->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 4cc9b54f993..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(const TaskComposerContext::Ptr& /*context*/, +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 e7ee4df0b92..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(const TaskComposerContext::Ptr& /*context*/, +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 49e4f14ef60..6717b9a182c 100644 --- a/tesseract_task_composer/core/src/nodes/remap_task.cpp +++ b/tesseract_task_composer/core/src/nodes/remap_task.cpp @@ -58,11 +58,11 @@ RemapTask::RemapTask(std::string name, const YAML::Node& config, const TaskCompo copy_ = n.as(); } -TaskComposerNodeInfo::UPtr RemapTask::runImpl(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr RemapTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); - if (context->getDataStorage().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 62ece9466c7..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(const TaskComposerContext::Ptr& /*context*/, +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 52070ad4642..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(const TaskComposerContext::Ptr& /*context*/, +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_context.cpp b/tesseract_task_composer/core/src/task_composer_context.cpp index d835a64f683..08af8df248e 100644 --- a/tesseract_task_composer/core/src/task_composer_context.cpp +++ b/tesseract_task_composer/core/src/task_composer_context.cpp @@ -39,20 +39,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -TaskComposerContext::TaskComposerContext(TaskComposerProblem::UPtr problem) - : problem_(std::move(problem)), data_storage_(std::make_shared(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)) { } -TaskComposerProblem& TaskComposerContext::getProblem() { return *problem_; } -const TaskComposerProblem& TaskComposerContext::getProblem() const { return *problem_; } - -TaskComposerDataStorage& TaskComposerContext::getDataStorage() { return *data_storage_; } -const TaskComposerDataStorage& TaskComposerContext::getDataStorage() const { return *data_storage_; } - -TaskComposerNodeInfoContainer& TaskComposerContext::getTaskInfos() { return task_infos_; } -const TaskComposerNodeInfoContainer& TaskComposerContext::getTaskInfos() const { return task_infos_; } - bool TaskComposerContext::isAborted() const { return aborted_; } bool TaskComposerContext::isSuccessful() const { return !aborted_; } @@ -60,41 +52,25 @@ bool TaskComposerContext::isSuccessful() const { return !aborted_; } void TaskComposerContext::abort(const boost::uuids::uuid& calling_node) { if (!calling_node.is_nil()) - task_infos_.setAborted(calling_node); + task_infos.setAborted(calling_node); aborted_ = true; } -void TaskComposerContext::reset() -{ - aborted_ = false; - data_storage_ = std::make_shared(problem_->input_data); - task_infos_.clear(); -} - -/** @brief Create a copy */ -TaskComposerContext::UPtr TaskComposerContext::createChild() -{ - auto child = std::make_unique(); - child->problem_ = problem_; - child->data_storage_ = data_storage_; - return child; -} - bool TaskComposerContext::operator==(const TaskComposerContext& rhs) const { bool equal = true; - if (problem_ != nullptr && rhs.problem_ != nullptr) - equal &= (*problem_ == *rhs.problem_); + if (problem != nullptr && rhs.problem != nullptr) + equal &= (*problem == *rhs.problem); else - equal &= (problem_ == nullptr && rhs.problem_ == nullptr); + equal &= (problem == nullptr && rhs.problem == nullptr); - if (data_storage_ != nullptr && rhs.data_storage_ != 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 &= (data_storage == nullptr && rhs.data_storage == nullptr); - equal &= task_infos_ == rhs.task_infos_; + equal &= task_infos == rhs.task_infos; equal &= aborted_ == rhs.aborted_; return equal; } @@ -104,9 +80,9 @@ bool TaskComposerContext::operator!=(const TaskComposerContext& rhs) const { ret template 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_); - ar& boost::serialization::make_nvp("task_infos", task_infos_); + ar& boost::serialization::make_nvp("problem", problem); + ar& boost::serialization::make_nvp("data_storage", data_storage); + ar& boost::serialization::make_nvp("task_infos", task_infos); ar& boost::serialization::make_nvp("aborted", aborted_); } 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 5b4f374a920..99b507a7cc4 100644 --- a/tesseract_task_composer/core/src/task_composer_executor.cpp +++ b/tesseract_task_composer/core/src/task_composer_executor.cpp @@ -37,9 +37,11 @@ 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::UPtr problem) +TaskComposerFuture::UPtr TaskComposerExecutor::run(const TaskComposerNode& node, + TaskComposerProblem::Ptr problem, + TaskComposerDataStorage::Ptr data_storage) { - return run(node, std::make_shared(std::move(problem))); + return run(node, std::make_shared(std::move(problem), std::move(data_storage))); } bool TaskComposerExecutor::operator==(const TaskComposerExecutor& rhs) const { return (name_ == rhs.name_); } diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp index b6336fd794c..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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor) const +int TaskComposerPipeline::run(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { - if (context->isAborted()) + if (context.isAborted()) { auto info = std::make_unique(*this); info->input_keys = input_keys_; @@ -59,7 +59,7 @@ int TaskComposerPipeline::run(const TaskComposerContext::Ptr& context, OptionalT info->color = "white"; info->message = "Aborted"; info->aborted_ = true; - context->getTaskInfos().addInfo(std::move(info)); + context.task_infos.addInfo(std::move(info)); return 0; } @@ -84,11 +84,11 @@ int TaskComposerPipeline::run(const TaskComposerContext::Ptr& context, OptionalT int value = results->return_value; assert(value >= 0); - context->getTaskInfos().addInfo(std::move(results)); + context.task_infos.addInfo(std::move(results)); return value; } -TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { if (terminals_.empty()) @@ -113,7 +113,7 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(const TaskComposerConte for (std::size_t i = 0; i < terminals_.size(); ++i) { - auto node_info = context->getTaskInfos().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(const TaskComposerConte } void TaskComposerPipeline::runRecursive(const TaskComposerNode& node, - const TaskComposerContext::Ptr& context, + TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { if (node.getType() == TaskComposerNodeType::GRAPH) diff --git a/tesseract_task_composer/core/src/task_composer_problem.cpp b/tesseract_task_composer/core/src/task_composer_problem.cpp index dd0ab2b55cc..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,6 @@ 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; } @@ -61,7 +55,6 @@ 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); } diff --git a/tesseract_task_composer/core/src/task_composer_server.cpp b/tesseract_task_composer/core/src/task_composer_server.cpp index c12716f8404..15c2a2e7bb1 100644 --- a/tesseract_task_composer/core/src/task_composer_server.cpp +++ b/tesseract_task_composer/core/src/task_composer_server.cpp @@ -104,7 +104,9 @@ std::vector TaskComposerServer::getAvailableTasks() const return tasks; } -TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerProblem::UPtr problem, 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()) @@ -114,18 +116,19 @@ TaskComposerFuture::UPtr TaskComposerServer::run(TaskComposerProblem::UPtr probl if (t_it == tasks_.end()) throw std::runtime_error("Task with name '" + problem->name + "' does not exist!"); - return e_it->second->run(*t_it->second, std::move(problem)); + return e_it->second->run(*t_it->second, std::move(problem), std::move(data_storage)); } TaskComposerFuture::UPtr TaskComposerServer::run(const TaskComposerNode& node, - TaskComposerProblem::UPtr problem, + 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, std::move(problem)); + 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 f8b5fa2d675..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(const TaskComposerContext::Ptr& context, OptionalTaskComposerExecutor executor) const +int TaskComposerTask::run(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { - if (context->isAborted()) + if (context.isAborted()) { auto info = std::make_unique(*this); info->input_keys = input_keys_; @@ -57,7 +57,7 @@ int TaskComposerTask::run(const TaskComposerContext::Ptr& context, OptionalTaskC info->color = "white"; info->message = "Aborted"; info->aborted_ = true; - context->getTaskInfos().addInfo(std::move(info)); + context.task_infos.addInfo(std::move(info)); return 0; } @@ -82,7 +82,7 @@ int TaskComposerTask::run(const TaskComposerContext::Ptr& context, OptionalTaskC int value = results->return_value; assert(value >= 0); - context->getTaskInfos().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 663d7b27e40..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,7 @@ void TestTask::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); } -TaskComposerNodeInfo::UPtr TestTask::runImpl(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr TestTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { if (throw_exception) @@ -88,7 +88,7 @@ TaskComposerNodeInfo::UPtr TestTask::runImpl(const TaskComposerContext::Ptr& con if (set_abort) { node_info->color = "red"; - context->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 a17ee161881..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(const TaskComposerContext::Ptr& context, + 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 = context->getDataStorage().getData(left_key_).as() + - context->getDataStorage().getData(right_key_).as(); - context->getDataStorage().setData(output_key_, result); + double 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(const TaskComposerContext::Ptr& context, + 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 = context->getDataStorage().getData(left_key_).as() * - context->getDataStorage().getData(right_key_).as(); - context->getDataStorage().setData(output_key_, result); + double 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,13 +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_problem = std::make_unique(); // result = a * (b + c) + d auto task1 = std::make_unique("b", "c", "task1_output"); @@ -98,8 +98,8 @@ int main() TaskComposerPluginFactory factory(config_path); auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(task_composer, std::move(task_problem)); + TaskComposerFuture::UPtr future = task_executor->run(task_composer, std::move(task_problem), std::move(task_data)); future->wait(); - std::cout << "Output: " << future->context->getDataStorage().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 f3df5556900..b29c5017281 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -57,26 +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); + 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, std::move(task_problem)); + 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, future->context->getTaskInfos().getInfoMap()); + task->dump(tc_out_data, nullptr, future->context->task_infos.getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = future->context->getDataStorage().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 3afebe439ab..2d2e8ed813f 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -57,24 +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); + auto task_problem = std::make_unique(env, profiles); auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - TaskComposerFuture::UPtr future = task_executor->run(*task, std::move(task_problem)); + 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, future->context->getTaskInfos().getInfoMap()); + task->dump(tc_out_data, nullptr, future->context->task_infos.getInfoMap()); tc_out_data.close(); // Plot Process Trajectory - auto output_program = future->context->getDataStorage().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 fbb882d33f9..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(const TaskComposerContext::Ptr& context, + 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 3891844283d..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(const TaskComposerContext::Ptr& context, + 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 b3531c73148..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(const TaskComposerContext::Ptr& context, + 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 acd312fd5cc..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(const TaskComposerContext::Ptr& context, + 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 b41949072c7..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(const TaskComposerContext::Ptr& context, + 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 2fd458f30b6..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(const TaskComposerContext::Ptr& context, + 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 c878c07d460..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(const TaskComposerContext::Ptr& context, + 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 a95797fcbd0..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(const TaskComposerContext::Ptr& context, + 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 c36b108d02f..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(const TaskComposerContext::Ptr& context, + TaskComposerNodeInfo::UPtr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + 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 = context->getDataStorage().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) { - context->getDataStorage().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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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 d753342620f..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(const TaskComposerContext::Ptr& context, + 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 41203a0e974..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(const TaskComposerContext::Ptr& context, + 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 9706b8a6d2d..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(const TaskComposerContext::Ptr& context, + 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 341896cdcf6..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(const TaskComposerContext::Ptr& context, + 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 957b2cce93d..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(const TaskComposerContext::Ptr& context, + 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 88140db8a5e..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(const TaskComposerContext::Ptr& context, + 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 6076a1b8820..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(const TaskComposerContext::Ptr& context, + 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 dd640951bfd..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(const TaskComposerContext::Ptr& context, + 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 5ff8b93df14..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(const TaskComposerContext::Ptr& context, + 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 66c8d2250f3..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 @@ -52,7 +52,7 @@ struct CheckInputProfile virtual bool isValid(const TaskComposerContext& context) const { // Get the problem - const auto& problem = dynamic_cast(context.getProblem()); + 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 8fd8dc261af..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); // Get Composite Profile info->return_value = 0; for (const auto& key : input_keys_) { - auto input_data_poly = context->getDataStorage().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(const TaskComposerContext::Pt getProfile(name_, profile, *problem.profiles, std::make_shared()); cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); - if (!cur_composite_profile->isValid(*context)) + 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 b49cd7a6af2..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -79,7 +79,7 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(const TaskCompose // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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 42556b73749..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -79,7 +79,7 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(const TaskComposerC // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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 405efce2865..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -81,7 +81,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(const TaskComposerContext // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const TaskComposerContext // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerContext // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerContext if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().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(const TaskComposerContext // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerContext case FixStateBoundsProfile::Settings::DISABLED: { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().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(const TaskComposerContext } } - context->getDataStorage().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 70d6a049e69..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -355,7 +355,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(const TaskComposerCont // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont if (flattened.size() <= 2) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont if (flattened.empty()) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont if (flattened.size() <= 1) { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont case FixStateCollisionProfile::Settings::DISABLED: { if (output_keys_[0] != input_keys_[0]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskComposerCont } } - context->getDataStorage().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 70538083346..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); @@ -77,7 +77,7 @@ TaskComposerNodeInfo::UPtr FormatAsInputTask::runImpl(const TaskComposerContext: // -------------------- // Check that inputs are valid // -------------------- - auto input_formatted_data_poly = context->getDataStorage().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(const TaskComposerContext: return info; } - auto input_unformatted_data_poly = context->getDataStorage().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(const TaskComposerContext: } } - context->getDataStorage().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 0c39d49102c..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -100,7 +100,7 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(const Ta // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const Ta // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const Ta // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const Ta info->color = "green"; info->message = "Successful"; - context->getDataStorage().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 6f458dc834a..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + 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 = context->getDataStorage().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(const TaskComposerContext::Ptr return info; } - context->getDataStorage().setData(output_keys_[0], response.results); + context.data_storage->setData(output_keys_[0], response.results); } else { - context->getDataStorage().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 d24ee71e3c2..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -72,7 +72,7 @@ TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(const TaskComposerContext: // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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 34d747749cc..60df0f2883a 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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -309,7 +309,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(const TaskComposerContext:: // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const TaskComposerContext:: 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)); - context->getDataStorage().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(const TaskComposerContext:: false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - context->getDataStorage().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(const TaskComposerContext:: false); auto update_end_state_uuid = task_graph.addNode(std::move(update_end_state_task)); - context->getDataStorage().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,19 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(const TaskComposerContext:: "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)); - context->getDataStorage().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, context); + TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - auto info_map = context->getTaskInfos().getInfoMap(); + /** @todo Need to merge child context into parent */ - if (context->getProblem().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 +473,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(const TaskComposerContext:: info->dotgraph = dot_graph.str(); } - if (context->isAborted()) + if (context.isAborted()) { info->message = "Raster subgraph failed"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); @@ -479,28 +481,27 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(const TaskComposerContext:: } program.clear(); - program.emplace_back(context->getDataStorage().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 = context->getDataStorage().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 = - context->getDataStorage().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 = context->getDataStorage().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); - context->getDataStorage().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 abe24a13e4c..ef1e1e70f68 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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -259,7 +259,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(const TaskComposerConte // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const TaskComposerConte 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)); - context->getDataStorage().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(const TaskComposerConte false); auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task)); - context->getDataStorage().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,12 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(const TaskComposerConte transition_idx++; } - TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context); + TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - auto info_map = context->getTaskInfos().getInfoMap(); + auto info_map = context.task_infos.getInfoMap(); - if (context->getProblem().dotgraph) + if (context.problem->dotgraph) { std::stringstream dot_graph; dot_graph << "subgraph cluster_" << toString(uuid_) << " {\n color=black;\n label = \"" << name_ << "\\n(" @@ -378,7 +378,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(const TaskComposerConte info->dotgraph = dot_graph.str(); } - if (context->isAborted()) + if (context.isAborted()) { info->message = "Raster only subgraph failed"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); @@ -389,7 +389,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(const TaskComposerConte for (std::size_t i = 0; i < raster_tasks.size(); ++i) { CompositeInstruction segment = - context->getDataStorage().getData(raster_tasks[i].second.second).as(); + context.data_storage->getData(raster_tasks[i].second.second).as(); if (i != 0) segment.erase(segment.begin()); @@ -398,13 +398,13 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(const TaskComposerConte if (i < raster_tasks.size() - 1) { CompositeInstruction transition = - context->getDataStorage().getData(transition_keys[i].second).as(); + context.data_storage->getData(transition_keys[i].second).as(); transition.erase(transition.begin()); program.emplace_back(transition); } } - context->getDataStorage().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 5e851de1bc0..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -85,7 +85,7 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(const TaskComp // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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; } - context->getDataStorage().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 d2eb06fe2cb..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + auto& problem = dynamic_cast(*context.problem); auto info = std::make_unique(*this); info->return_value = 0; @@ -91,7 +91,7 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(const TaskCo // -------------------- // Check that inputs are valid // -------------------- - auto input_data_poly = context->getDataStorage().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(const TaskCo // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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(const TaskCo // 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]) - context->getDataStorage().setData(output_keys_[0], context->getDataStorage().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; } - context->getDataStorage().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 77287eb2a9f..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr UpdateEndStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = context->getDataStorage().getData(input_keys_[0]); - auto input_next_data_poly = context->getDataStorage().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(const TaskComposerContext throw std::runtime_error("Invalid waypoint type"); // Store results - context->getDataStorage().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 8fec3d2d596..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = context->getDataStorage().getData(input_keys_[0]); - auto input_prev_data_poly = context->getDataStorage().getData(input_keys_[1]); - auto input_next_data_poly = context->getDataStorage().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(const TaskCompose throw std::runtime_error("Invalid waypoint type"); // Store results - context->getDataStorage().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 bc6ee8a053b..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); info->return_value = 0; - auto input_data_poly = context->getDataStorage().getData(input_keys_[0]); - auto input_prev_data_poly = context->getDataStorage().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(const TaskComposerConte throw std::runtime_error("Invalid waypoint type"); // Store results - context->getDataStorage().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 4ac7fd6f186..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(const TaskComposerContext::Ptr& context, +TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { // Get the problem - auto& problem = dynamic_cast(context->getProblem()); + 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 = context->getDataStorage().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(const TaskComposerCon new_results.clear(); upsample(new_results, ci, start_instruction, cur_composite_profile->longest_valid_segment_length); - context->getDataStorage().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 19263e3d093..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, TaskComposerContext::Ptr context) override final; - long getWorkerCount() const override final; long getTaskCount() const override final; @@ -84,19 +82,21 @@ 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, - const TaskComposerContext::Ptr& task_context, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> convertToTaskflow(const TaskComposerPipeline& task_pipeline, - const TaskComposerContext::Ptr& task_context, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor); static std::shared_ptr>> convertToTaskflow(const TaskComposerTask& task, - const TaskComposerContext::Ptr& task_context, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor); }; } // namespace tesseract_planning 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 5345b9fd5f2..3931f2a0681 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -74,11 +74,11 @@ TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNod { std::shared_ptr>> taskflow; if (node.getType() == TaskComposerNodeType::TASK) - taskflow = convertToTaskflow(static_cast(node), context, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else if (node.getType() == TaskComposerNodeType::PIPELINE) - taskflow = convertToTaskflow(static_cast(node), context, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else if (node.getType() == TaskComposerNodeType::GRAPH) - taskflow = convertToTaskflow(static_cast(node), context, *this); + taskflow = convertToTaskflow(static_cast(node), *context, *this); else throw std::runtime_error("TaskComposerExecutor, unsupported node type!"); @@ -135,7 +135,7 @@ void TaskflowTaskComposerExecutor::serialize(Archive& ar, const unsigned int ver std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_graph, - const TaskComposerContext::Ptr& task_context, + TaskComposerContext& task_context, TaskComposerExecutor& task_executor) { auto tf_container = std::make_shared>>(); @@ -146,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_context->getTaskInfos().addInfo(std::move(info)); + task_context.task_infos.addInfo(std::move(info)); // Generate process tasks for each node std::map tasks; @@ -160,12 +160,12 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr if (edges.size() > 1 && task->isConditional()) tasks[pair.first] = tf_container->front() - ->emplace([task, task_context, &task_executor] { return task->run(task_context, 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_context, &task_executor] { task->run(task_context, task_executor); }) + ->emplace([task, &task_context, &task_executor] { task->run(task_context, task_executor); }) .name(pair.second->getName()); } else if (pair.second->getType() == TaskComposerNodeType::PIPELINE) @@ -173,14 +173,14 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr auto pipeline = std::static_pointer_cast(pair.second); if (edges.size() > 1 && pipeline->isConditional()) tasks[pair.first] = tf_container->front() - ->emplace([pipeline, task_context, &task_executor] { + ->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_context, &task_executor] { pipeline->run(task_context, 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) @@ -209,27 +209,27 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr std::shared_ptr>> TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerPipeline& task_pipeline, - const TaskComposerContext::Ptr& task_context, + 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_context, &task_executor] { return task_pipeline.run(task_context, task_executor); }) + [&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, - const TaskComposerContext::Ptr& task_context, + 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_context, &task_executor] { return task.run(task_context, 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/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index dbfc7f78f4c..314423258e0 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -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 context - auto task_context = std::make_shared(std::move(task_problem)); + 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_context), expected_return); + EXPECT_EQ(task.run(*task_context), expected_return); if (expected_return == 1) { - auto task_program = task_context->getDataStorage().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 d97fe267618..ac7c2710c7f 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -120,25 +120,20 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerDataStorageTests) // NOLINT TEST(TesseractTaskComposerCoreUnit, TaskComposerContextTests) // NOLINT { TaskComposerNode node; - auto context = std::make_unique(std::make_unique()); + auto context = std::make_unique(std::make_unique(), + std::make_unique()); EXPECT_FALSE(context->isAborted()); EXPECT_TRUE(context->isSuccessful()); - EXPECT_TRUE(context->getTaskInfos().getInfoMap().empty()); - context->getTaskInfos().addInfo(std::make_unique(node)); + EXPECT_TRUE(context->task_infos.getInfoMap().empty()); + context->task_infos.addInfo(std::make_unique(node)); context->abort(node.getUUID()); - EXPECT_EQ(context->getTaskInfos().getAbortingNode(), node.getUUID()); + EXPECT_EQ(context->task_infos.getAbortingNode(), node.getUUID()); EXPECT_TRUE(context->isAborted()); EXPECT_FALSE(context->isSuccessful()); - EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); // Serialization test_suite::runSerializationPointerTest(context, "TaskComposerContextTests"); - - context->reset(); - EXPECT_NO_THROW(context->getProblem()); // NOLINT - EXPECT_FALSE(context->isAborted()); - EXPECT_TRUE(context->isSuccessful()); - EXPECT_TRUE(context->getTaskInfos().getInfoMap().empty()); } TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT @@ -153,9 +148,9 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerProblemTests) // NOLINT auto problem2 = std::make_unique("TaskComposerProblemTests"); EXPECT_EQ(problem2->name, "TaskComposerProblemTests"); - auto problem3 = std::make_unique(TaskComposerDataStorage(), "TaskComposerProblemTests"); - problem3->dotgraph = true; + 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); @@ -297,16 +292,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(task->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 0); + 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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { // Conditional @@ -320,16 +316,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(task->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 1); + 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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { @@ -350,16 +347,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(task->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 0); + 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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(task->dump(os)); // NOLINT + EXPECT_NO_THROW(task->dump(os, nullptr, context->task_infos.getInfoMap())); // NOLINT } { @@ -381,16 +379,17 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerTaskTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(task, "TaskComposerTaskTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(task->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 1); + 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, context->getTaskInfos().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 @@ -401,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); - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); task->throw_exception = true; - EXPECT_EQ(task->run(context), 0); + EXPECT_EQ(task->run(*context), 0); EXPECT_TRUE(context->isSuccessful()); EXPECT_FALSE(context->isAborted()); - EXPECT_EQ(context->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(task->getUUID())->return_value, 0); + EXPECT_EQ(context->task_infos.getInfoMap().size(), 1); + EXPECT_EQ(context->task_infos.getInfoMap().at(task->getUUID())->return_value, 0); } } @@ -487,12 +487,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 5); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -501,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -542,12 +543,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 4); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 1); + 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"); @@ -556,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -598,12 +600,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 4); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -612,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -654,12 +657,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 4); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -668,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -712,12 +716,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline3->run(context), 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->getTaskInfos().getInfoMap().size(), 9); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 0); + 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"); @@ -726,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -772,12 +777,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline3->run(context), 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->getTaskInfos().getInfoMap().size(), 9); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 1); + 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"); @@ -786,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -833,12 +839,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline3, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline3->run(context), 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->getTaskInfos().getInfoMap().size(), 6); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline3->getUUID())->return_value, 1); + 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"); @@ -847,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline3->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1002,12 +1009,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT // Serialization test_suite::runSerializationPointerTest(pipeline, "TaskComposerPipelineTests"); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -1016,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1033,12 +1041,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT pipeline->addEdges(uuid2, { uuid3 }); pipeline->addEdges(uuid3, { uuid1 }); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 1); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -1047,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1063,12 +1072,13 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerPipelineTests) // NOLINT pipeline->addEdges(uuid1, { uuid3 }); pipeline->setTerminals({ uuid2, uuid3 }); - auto context = std::make_shared(std::make_unique()); - EXPECT_EQ(pipeline->run(context), 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->getTaskInfos().getInfoMap().size(), 2); - EXPECT_EQ(context->getTaskInfos().getInfoMap().at(pipeline->getUUID())->return_value, 0); + 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"); @@ -1077,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, context->getTaskInfos().getInfoMap())); // NOLINT + EXPECT_NO_THROW(pipeline->dump(os2, nullptr, context->task_infos.getInfoMap())); // NOLINT os2.close(); } @@ -1540,17 +1550,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerAbortTaskTests) // NOLINT } { // Test run method - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); AbortTask task; - EXPECT_EQ(task.run(context), 0); - auto node_info = context->getTaskInfos().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(context->isAborted(), true); EXPECT_EQ(context->isSuccessful(), false); - EXPECT_EQ(context->getTaskInfos().getAbortingNode(), task.getUUID()); + EXPECT_EQ(context->task_infos.getAbortingNode(), task.getUUID()); } } @@ -1586,17 +1597,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerErrorTaskTests) // NOLINT } { // Test run method - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); ErrorTask task; - EXPECT_EQ(task.run(context), 0); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1632,17 +1644,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerDoneTaskTests) // NOLINT } { // Test run method - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); DoneTask task; - EXPECT_EQ(task.run(context), 1); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1691,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 context = std::make_shared(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(context), 1); - EXPECT_TRUE(context->getDataStorage().hasKey(key)); - EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); - EXPECT_EQ(context->getDataStorage().getData(key), context->getDataStorage().getData(remap_key)); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method move auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto context = std::make_shared(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(context), 1); - EXPECT_FALSE(context->getDataStorage().hasKey(key)); - EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); - EXPECT_EQ(context->getDataStorage().getData(remap_key).as(), js); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + 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 context = std::make_shared(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: @@ -1749,24 +1765,25 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); - EXPECT_EQ(task.run(context), 1); - EXPECT_TRUE(context->getDataStorage().hasKey(key)); - EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); - EXPECT_EQ(context->getDataStorage().getData(key), context->getDataStorage().getData(remap_key)); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + 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 context = std::make_shared(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: @@ -1777,18 +1794,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); - EXPECT_EQ(task.run(context), 1); - EXPECT_FALSE(context->getDataStorage().hasKey(key)); - EXPECT_TRUE(context->getDataStorage().hasKey(remap_key)); - EXPECT_EQ(context->getDataStorage().getData(remap_key).as(), js); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failures @@ -1817,46 +1834,48 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT { // Test run method copy failure auto problem = std::make_unique(); - problem->input_data.setData(key, js); - auto context = std::make_shared(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(context), 0); - EXPECT_TRUE(context->getDataStorage().hasKey(key)); - EXPECT_FALSE(context->getDataStorage().hasKey(remap_key)); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + 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 context = std::make_shared(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(context), 0); - EXPECT_TRUE(context->getDataStorage().hasKey(key)); - EXPECT_FALSE(context->getDataStorage().hasKey(remap_key)); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1913,17 +1932,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT } { // Test run method - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); StartTask task; - EXPECT_EQ(task.run(context), 1); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -1980,17 +2000,18 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT } { // Test run method - auto context = std::make_shared(std::make_unique()); + auto context = std::make_shared(std::make_unique(), + std::make_unique()); SyncTask task; - EXPECT_EQ(task.run(context), 1); - auto node_info = context->getTaskInfos().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(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } } @@ -2091,40 +2112,40 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerServerTests) // NOLINT EXPECT_ANY_THROW(server.getTaskCount("DoesNotExist")); // NOLINT { // Run method using TaskComposerContext - auto problem = std::make_unique(); - problem->name = "TestPipeline"; - auto future = server.run(std::move(problem), "TaskflowExecutor"); + 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(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 4); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 4); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); } { // Run method using Pipeline - auto problem = std::make_unique(); - 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, std::move(problem), "TaskflowExecutor"); + auto future = server.run(pipeline, std::move(problem), std::move(data_storage), "TaskflowExecutor"); future->wait(); EXPECT_EQ(future->context->isAborted(), false); EXPECT_EQ(future->context->isSuccessful(), true); - EXPECT_EQ(future->context->getTaskInfos().getInfoMap().size(), 4); - EXPECT_TRUE(future->context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(future->context->task_infos.getInfoMap().size(), 4); + EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); } { // Failures, executor does not exist - auto problem = std::make_unique(); - problem->name = "TestPipeline"; - EXPECT_ANY_THROW(server.run(std::move(problem), "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 problem = std::make_unique(); - problem->name = "DoesNotExist"; - EXPECT_ANY_THROW(server.run(std::move(problem), "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 b730abd367c..51e8040b8ea 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -251,7 +251,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningTaskComposerProble EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Failure @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); // Serialization @@ -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->getDataStorage().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()), @@ -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->getDataStorage().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()), @@ -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->getDataStorage().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()), @@ -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->getDataStorage().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()); } @@ -1736,8 +1736,8 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz 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->getDataStorage().getData("output_data")); - EXPECT_EQ(input->getDataStorage().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"); @@ -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->getDataStorage().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()); } @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1873,8 +1873,8 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio 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->getDataStorage().getData("output_data")); - EXPECT_EQ(input->getDataStorage().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"); @@ -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->getDataStorage().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 @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -2014,15 +2014,15 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT 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->getDataStorage().getData("output_data")); - EXPECT_EQ(input->getDataStorage().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 context2 = std::make_unique(std::move(problem2)); TimeOptimalParameterizationTask task2("abc", "input_data", "output_data", true); EXPECT_EQ(task2.run(*context2), 1); - data.setData("input_data", context2->getDataStorage().getData("output_data")); - EXPECT_EQ(context2->getDataStorage().getData("output_data").as().size(), 17); + 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"); @@ -2036,8 +2036,8 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_EQ(context->getDataStorage().getData("output_data").as().size(), 17); - EXPECT_TRUE(context->getTaskInfos().getAbortingNode().is_nil()); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } { // Test run method @@ -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->getDataStorage().hasKey("output_data")); + EXPECT_TRUE(input->data_storage->hasKey("output_data")); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -2162,8 +2162,8 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / 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->getDataStorage().getData("output_data")); - EXPECT_GE(input->getDataStorage().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"); @@ -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->getDataStorage().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 @@ -2572,7 +2572,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->getDataStorage().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); @@ -3053,7 +3053,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_TRUE(input->getDataStorage().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); From dfcdc67e596f68f2f59fed49f93d6222795993a8 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 18 Sep 2023 17:53:08 -0500 Subject: [PATCH 4/6] Fixup --- .../planning/src/nodes/raster_motion_task.cpp | 6 ++++-- .../planning/src/nodes/raster_only_motion_task.cpp | 6 +++++- 2 files changed, 9 insertions(+), 3 deletions(-) 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 60df0f2883a..f722ec625d6 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -459,10 +459,12 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerContext& contex TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - /** @todo Need to merge child context into parent */ + // Merge child context data into parent context + context.task_infos.mergeInfoMap(future->context->task_infos); + if (future->context->isAborted()) + context.abort(future->context->task_infos.getAbortingNode()); auto info_map = context.task_infos.getInfoMap(); - if (context.problem->dotgraph) { std::stringstream dot_graph; 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 ef1e1e70f68..c9b5e1a4651 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 @@ -366,8 +366,12 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerContext& co TaskComposerFuture::UPtr future = executor.value().get().run(task_graph, context.problem, context.data_storage); future->wait(); - auto info_map = context.task_infos.getInfoMap(); + // Merge child context data into parent context + context.task_infos.mergeInfoMap(future->context->task_infos); + if (future->context->isAborted()) + context.abort(future->context->task_infos.getAbortingNode()); + auto info_map = context.task_infos.getInfoMap(); if (context.problem->dotgraph) { std::stringstream dot_graph; From 9378936f6b6d221deae9e9b73423eaf2134e6163 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 18 Sep 2023 17:58:40 -0500 Subject: [PATCH 5/6] fixup --- .../planning/src/nodes/raster_motion_task.cpp | 2 +- .../planning/src/nodes/raster_only_motion_task.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 f722ec625d6..e73314e9766 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -460,7 +460,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerContext& contex future->wait(); // Merge child context data into parent context - context.task_infos.mergeInfoMap(future->context->task_infos); + context.task_infos.mergeInfoMap(std::move(future->context->task_infos)); if (future->context->isAborted()) context.abort(future->context->task_infos.getAbortingNode()); 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 c9b5e1a4651..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 @@ -367,7 +367,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerContext& co future->wait(); // Merge child context data into parent context - context.task_infos.mergeInfoMap(future->context->task_infos); + context.task_infos.mergeInfoMap(std::move(future->context->task_infos)); if (future->context->isAborted()) context.abort(future->context->task_infos.getAbortingNode()); From efff644c6822a72e81b786cef21764fc1a9c64d5 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 19 Sep 2023 09:37:17 -0500 Subject: [PATCH 6/6] fixup --- tesseract_examples/src/car_seat_example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 4e045999866..584665d6e10 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -302,7 +302,7 @@ bool CarSeatExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - auto input_data = std::unique_ptr(); + auto input_data = std::make_unique(); input_data->setData(input_key, program); // Create Task Composer Problem @@ -390,7 +390,7 @@ bool CarSeatExample::run() const std::string output_key = task->getOutputKeys().front(); // Create Task Input Data - auto input_data = std::unique_ptr(); + auto input_data = std::make_unique(); input_data->setData(input_key, program); // Create Task Composer Problem