From 44fb0988d7ada17343a56a80349ba64d99f707fb Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 17 Jul 2024 14:09:01 -0500 Subject: [PATCH] Environment should be stored as const in data storage --- .../src/basic_cartesian_example.cpp | 2 +- tesseract_examples/src/car_seat_example.cpp | 4 +- .../src/freespace_hybrid_example.cpp | 2 +- .../src/freespace_ompl_example.cpp | 2 +- .../src/glass_upright_example.cpp | 2 +- .../src/pick_and_place_example.cpp | 4 +- .../puzzle_piece_auxillary_axes_example.cpp | 2 +- .../src/puzzle_piece_example.cpp | 2 +- .../examples/task_composer_raster_example.cpp | 2 +- .../task_composer_trajopt_example.cpp | 2 +- .../planning/nodes/motion_planner_task.hpp | 7 +- .../nodes/continuous_contact_check_task.cpp | 5 +- .../src/nodes/discrete_contact_check_task.cpp | 5 +- .../src/nodes/fix_state_bounds_task.cpp | 5 +- .../src/nodes/fix_state_collision_task.cpp | 5 +- ...iterative_spline_parameterization_task.cpp | 5 +- .../planning/src/nodes/min_length_task.cpp | 5 +- .../planning/src/nodes/raster_motion_task.cpp | 5 +- .../src/nodes/raster_only_motion_task.cpp | 5 +- .../ruckig_trajectory_smoothing_task.cpp | 5 +- .../time_optimal_parameterization_task.cpp | 5 +- .../test/fix_state_bounds_task_unit.cpp | 2 +- .../test/fix_state_collision_task_unit.cpp | 8 +- .../tesseract_task_composer_planning_unit.cpp | 90 +++++++++---------- 24 files changed, 85 insertions(+), 96 deletions(-) diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index f97711a650..69f2cafadb 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -271,7 +271,7 @@ bool BasicCartesianExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); if (plotter_ != nullptr && plotter_->isConnected()) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 820d62785c..d81eedcd11 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -378,7 +378,7 @@ bool CarSeatExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task @@ -465,7 +465,7 @@ bool CarSeatExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 1811e21903..caafb55513 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -229,7 +229,7 @@ bool FreespaceHybridExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index a1b57c8bf7..6db760c438 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -197,7 +197,7 @@ bool FreespaceOMPLExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index a8cb2787d7..41d7751ee2 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -261,7 +261,7 @@ bool GlassUprightExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); if (plotter_ != nullptr && plotter_->isConnected()) diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 0c93649047..ba05e644c9 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -306,7 +306,7 @@ bool PickAndPlaceExample::run() // Create Task Composer Data Storage auto pick_data = std::make_unique(); pick_data->setData("planning_input", pick_program); - pick_data->setData("environment", env_); + pick_data->setData("environment", std::shared_ptr(env_)); pick_data->setData("profiles", profiles); // Solve task @@ -425,7 +425,7 @@ bool PickAndPlaceExample::run() // Create Task Composer Data Storage auto place_data = std::make_unique(); place_data->setData("planning_input", place_program); - place_data->setData("environment", env_); + place_data->setData("environment", std::shared_ptr(env_)); place_data->setData("profiles", profiles); // Solve task diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 6f4d915cda..e8f1825e4a 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -307,7 +307,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 092136a704..fc7f6ef45f 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -293,7 +293,7 @@ bool PuzzlePieceExample::run() // Create Task Composer Data Storage auto data = std::make_unique(); data->setData("planning_input", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve task diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index 55a12ba588..b159e6bed5 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -67,7 +67,7 @@ int main() // Create data storage auto task_data = std::make_unique(); task_data->setData(input_key, program); - task_data->setData("environment", env); + task_data->setData("environment", std::shared_ptr(env)); task_data->setData("profiles", profiles); // Solve raster plan diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp index 830c0d33dc..ade959acca 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -68,7 +68,7 @@ int main() // Create data storage auto task_data = std::make_unique(); task_data->setData(input_key, program); - task_data->setData("environment", env); + task_data->setData("environment", std::shared_ptr(env)); task_data->setData("profiles", profiles); auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor"); 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 1825b12896..0fad182cdf 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 @@ -149,17 +149,16 @@ class MotionPlannerTask : public TaskComposerTask // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; - info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is missing"; + info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; CONSOLE_BRIDGE_logError("%s", info->status_message.c_str()); info->return_value = 0; return info; } - std::shared_ptr env = - env_poly.template as>(); + auto env = env_poly.template as>(); info->env = env; auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); 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 461afe8322..fe9e5c7731 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 @@ -113,7 +113,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -122,8 +122,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); info->env = env; auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT); 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 a2ffe545d2..11970b35a5 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 @@ -113,7 +113,7 @@ std::unique_ptr DiscreteContactCheckTask::runImpl(TaskComp // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -122,8 +122,7 @@ std::unique_ptr DiscreteContactCheckTask::runImpl(TaskComp return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); info->env = env; auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT); 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 5363f4adb7..572a32dc60 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 @@ -106,7 +106,7 @@ std::unique_ptr FixStateBoundsTask::runImpl(TaskComposerCo // Check that inputs are valid // -------------------- tesseract_common::AnyPoly env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -114,8 +114,7 @@ std::unique_ptr FixStateBoundsTask::runImpl(TaskComposerCo info->return_value = 0; return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) 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 becdc36503..90538f15d0 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 @@ -399,7 +399,7 @@ std::unique_ptr FixStateCollisionTask::runImpl(TaskCompose // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -408,8 +408,7 @@ std::unique_ptr FixStateCollisionTask::runImpl(TaskCompose return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); info->env = env; auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); 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 5e5d2baf5b..936d983e21 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 @@ -128,7 +128,7 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -137,8 +137,7 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) 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 ee973865ed..5fe9706963 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -108,7 +108,7 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -117,8 +117,7 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) 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 ad581a8509..fe153251c5 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -360,7 +360,7 @@ std::unique_ptr RasterMotionTask::runImpl(TaskComposerCont // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -369,8 +369,7 @@ std::unique_ptr RasterMotionTask::runImpl(TaskComposerCont return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); info->env = env; auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); 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 498cd69b48..0bbda24d5f 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 @@ -301,7 +301,7 @@ std::unique_ptr RasterOnlyMotionTask::runImpl(TaskComposer // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -310,8 +310,7 @@ std::unique_ptr RasterOnlyMotionTask::runImpl(TaskComposer return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); info->env = env; tesseract_common::ManipulatorInfo input_manip_info; 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 add4fae28e..19f118ef91 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 @@ -112,7 +112,7 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -121,8 +121,7 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) 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 47516d614b..f5e2e277d9 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 @@ -119,7 +119,7 @@ TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalT // Check that inputs are valid // -------------------- auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); - if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) { info->status_code = 0; info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; @@ -128,8 +128,7 @@ TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalT return info; } - std::shared_ptr env = - env_poly.as>(); + auto env = env_poly.as>(); auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) 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 7815c38e83..94ef2c3ef1 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -86,7 +86,7 @@ void checkProgram(const Environment::Ptr& env, // Create data storage auto task_data = std::make_unique(); task_data->setData("input_program", program); - task_data->setData("environment", env); + task_data->setData("environment", std::shared_ptr(env)); task_data->setData("profiles", profiles); // Create context 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 cfe5b7b99e..3ae1084ead 100644 --- a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp @@ -49,7 +49,7 @@ TEST_F(FixStateCollisionTaskUnit, StateInCollisionTest) // NOLINT // Create data storage auto task_data = std::make_unique(); task_data->setData("input_program", program); - task_data->setData("environment", env_); + task_data->setData("environment", std::shared_ptr(env_)); FixStateCollisionProfile profile; @@ -90,7 +90,7 @@ TEST_F(FixStateCollisionTaskUnit, WaypointInCollisionTest) // NOLINT // Create data storage auto task_data = std::make_unique(); task_data->setData("input_program", program); - task_data->setData("environment", env_); + task_data->setData("environment", std::shared_ptr(env_)); FixStateCollisionProfile profile; @@ -139,7 +139,7 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionRandomSamplerTest) / // Create data storage auto task_data = std::make_shared(); task_data->setData("input_program", program); - task_data->setData("environment", env_); + task_data->setData("environment", std::shared_ptr(env_)); FixStateCollisionProfile profile; @@ -173,7 +173,7 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptTest) // NOLI // Create data storage auto task_data = std::make_shared(); task_data->setData("input_program", program); - task_data->setData("environment", env_); + task_data->setData("environment", std::shared_ptr(env_)); FixStateCollisionProfile profile; 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 b7adac9e43..f792fa62b0 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -139,7 +139,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerContinuousContactCheckTaskTests", std::move(data)); @@ -164,7 +164,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerContinuousContactCheckTaskTests", std::move(data)); @@ -206,7 +206,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("TaskComposerContinuousContactCheckTaskTests", std::move(data)); ContinuousContactCheckTask task( @@ -234,7 +234,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerContinuousContactCheckTaskTests", std::move(data)); @@ -316,7 +316,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerDiscreteContactCheckTaskTests", std::move(data)); DiscreteContactCheckTask task( @@ -340,7 +340,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerDiscreteContactCheckTaskTests", std::move(data)); DiscreteContactCheckTask task( @@ -380,7 +380,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe { // Failure missing profiles dat auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("TaskComposerDiscreteContactCheckTaskTests", std::move(data)); DiscreteContactCheckTask task( "TaskComposerDiscreteContactCheckTaskTests", "input_data", "environment", "profiles", true); @@ -407,7 +407,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("TaskComposerDiscreteContactCheckTaskTests", std::move(data)); DiscreteContactCheckTask task( @@ -897,7 +897,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); MinLengthTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -917,7 +917,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); MinLengthTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -955,7 +955,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); MinLengthTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -1049,7 +1049,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); FixStateBoundsTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1072,7 +1072,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) auto program = test_suite::jointInterpolateExampleProgramABB(true); program.clear(); data->setData("input_data", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); FixStateBoundsTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1092,7 +1092,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); FixStateBoundsTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1130,7 +1130,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); FixStateBoundsTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -1224,7 +1224,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); FixStateCollisionTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1248,7 +1248,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); FixStateCollisionTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1286,7 +1286,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); FixStateCollisionTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -1965,7 +1965,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -1988,7 +1988,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); data->setData("input_data", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2008,7 +2008,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2046,7 +2046,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); IterativeSplineParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -2157,7 +2157,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2184,7 +2184,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); data->setData("input_data", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2204,7 +2204,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2242,7 +2242,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); TimeOptimalParameterizationTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -2353,7 +2353,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT auto data3 = std::make_unique(); data3->setData("input_data", context->data_storage->getData("output_data")); - data3->setData("environment", env_); + data3->setData("environment", std::shared_ptr(env_)); data3->setData("profiles", profiles); auto context2 = std::make_unique("abc", std::move(data3)); TimeOptimalParameterizationTask task2("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2362,7 +2362,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT EXPECT_EQ(context2->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2385,7 +2385,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT auto program = test_suite::jointInterpolateExampleProgramABB(false); program.clear(); data->setData("input_data", program); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2405,7 +2405,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2443,7 +2443,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); RuckigTrajectorySmoothingTask task("abc", "input_data", "environment", "profiles", "output_data", true); EXPECT_EQ(task.run(*context), 0); @@ -2546,7 +2546,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / auto profiles = std::make_shared(); auto data2 = std::make_unique(); data2->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data2->setData("environment", env_); + data2->setData("environment", std::shared_ptr(env_)); data2->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data2)); MinLengthTask task("abc", "input_data", "environment", "profiles", "output_data", true); @@ -2555,7 +2555,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / EXPECT_GE(context->data_storage->getData("output_data").as().size(), 10); } auto profiles = std::make_shared(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); MotionPlannerTask task( @@ -2580,7 +2580,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / { // Failure missing input data auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); auto context = std::make_unique("abc", std::move(data)); MotionPlannerTask task( @@ -2620,7 +2620,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / { // Failure missing profiles data auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(false)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); auto context = std::make_unique("abc", std::move(data)); MotionPlannerTask task( "abc", "input_data", "environment", "profiles", "output_data", false, true); @@ -2959,7 +2959,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create Data Storage auto data_storage = std::make_unique(); data_storage->setData("planning_input", test_suite::rasterExampleProgram()); - data_storage->setData("environment", env_); + data_storage->setData("environment", std::shared_ptr(env_)); data_storage->setData("profiles", profiles); // Solve raster plan @@ -3064,7 +3064,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create Data Storage auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3109,7 +3109,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", tesseract_common::AnyPoly()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3154,7 +3154,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3199,7 +3199,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3426,7 +3426,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create Data Storage auto data_storage = std::make_unique(); data_storage->setData("planning_input", test_suite::rasterOnlyExampleProgram()); - data_storage->setData("environment", env_); + data_storage->setData("environment", std::shared_ptr(env_)); data_storage->setData("profiles", profiles); // Solve raster plan @@ -3526,7 +3526,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create Data Storage auto profiles = std::make_shared(); auto data = std::make_unique(); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3567,7 +3567,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", tesseract_common::AnyPoly()); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3608,7 +3608,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", tesseract_common::AnyPoly(tesseract_common::JointState())); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve @@ -3649,7 +3649,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) auto profiles = std::make_shared(); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); - data->setData("environment", env_); + data->setData("environment", std::shared_ptr(env_)); data->setData("profiles", profiles); // Solve