From 78c84c3fe1415df67f591413781a73b9cf448690 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 5 Nov 2023 15:33:25 -0600 Subject: [PATCH 1/2] remove results from TaskComposerNodeInfo --- tesseract_motion_planners/core/src/utils.cpp | 8 ++++---- .../core/task_composer_node_info.h | 3 --- .../core/src/task_composer_node_info.cpp | 2 -- 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp index d3d9fc98b5..f8e2da6834 100644 --- a/tesseract_motion_planners/core/src/utils.cpp +++ b/tesseract_motion_planners/core/src/utils.cpp @@ -317,8 +317,8 @@ bool contactCheckProgram(std::vector& con { // Grab the first waypoint to get the joint names const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); - traj_contacts = std::make_unique(joint_names, - static_cast(program.size())); + traj_contacts = + std::make_unique(joint_names, static_cast(program.size())); } contacts.clear(); @@ -624,8 +624,8 @@ bool contactCheckProgram(std::vector& con { // Grab the first waypoint to get the joint names const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); - traj_contacts = std::make_unique(joint_names, - static_cast(program.size())); + traj_contacts = + std::make_unique(joint_names, static_cast(program.size())); } contacts.clear(); 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 0ff4c0f1cb..5f89acf3f6 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 @@ -81,9 +81,6 @@ class TaskComposerNodeInfo /** @brief The output keys */ std::vector output_keys; - /** @brief Store the results of the task */ - tesseract_common::AnyPoly results; - /** @brief Value returned from the Task on completion */ int return_value{ -1 }; 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 fc2c853a89..41d516f41c 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -58,7 +58,6 @@ bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const equal &= name == rhs.name; equal &= uuid == rhs.uuid; equal &= parent_uuid == rhs.parent_uuid; - equal &= results == rhs.results; equal &= return_value == rhs.return_value; equal &= message == rhs.message; equal &= tesseract_common::almostEqualRelativeAndAbs(elapsed_time, rhs.elapsed_time, max_diff); @@ -84,7 +83,6 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/ ar& boost::serialization::make_nvp("name", name); ar& boost::serialization::make_nvp("uuid", uuid); ar& boost::serialization::make_nvp("parent_uuid", parent_uuid); - ar& boost::serialization::make_nvp("results", results); ar& boost::serialization::make_nvp("return_value", return_value); ar& boost::serialization::make_nvp("message", message); ar& boost::serialization::make_nvp("elapsed_time", elapsed_time); From fe80ad58d423a4073a1b43211309312f69c260e4 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 6 Nov 2023 14:04:01 -0600 Subject: [PATCH 2/2] Move TaskComposerProblem input to base class and change type to tesseract_common::AnyPoly --- .../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 +- .../core/task_composer_problem.h | 4 +++ .../core/src/task_composer_problem.cpp | 2 ++ .../planning/planning_task_composer_problem.h | 3 -- .../src/nodes/process_planning_input_task.cpp | 6 ++-- .../src/planning_task_composer_problem.cpp | 2 -- .../tesseract_task_composer_planning_unit.cpp | 28 +++++++++---------- 14 files changed, 33 insertions(+), 32 deletions(-) diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 2af69fc224..5fe07d2b45 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -244,7 +244,7 @@ bool BasicCartesianExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 1df97d38c5..825b728748 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -302,7 +302,7 @@ bool CarSeatExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); @@ -386,7 +386,7 @@ bool CarSeatExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index e6abca19d6..3b40c87e8c 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -180,7 +180,7 @@ bool FreespaceHybridExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 8cee960567..4e88ce2d4f 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -180,7 +180,7 @@ bool FreespaceOMPLExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index a22901f3e7..83be6b007f 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -237,7 +237,7 @@ bool GlassUprightExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; if (plotter_ != nullptr && plotter_->isConnected()) plotter_->waitForInput("Hit Enter to solve for trajectory."); diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 5c19f1af67..c37d868359 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -227,7 +227,7 @@ bool PickAndPlaceExample::run() // Create Task Composer Problem auto pick_problem = std::make_unique(env_, profiles); - pick_problem->input_instruction = pick_program; + pick_problem->input = pick_program; // Solve task TaskComposerFuture::UPtr pick_future = executor->run(*pick_task, std::move(pick_problem)); @@ -344,7 +344,7 @@ bool PickAndPlaceExample::run() // Create Task Composer Problem auto place_problem = std::make_unique(env_, profiles); - place_problem->input_instruction = place_program; + place_problem->input = place_program; // Solve task TaskComposerFuture::UPtr place_future = executor->run(*place_task, std::move(place_problem)); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 12420f88c2..2126b982d9 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -233,7 +233,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem)); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 56a65f23ed..9d591edc81 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -225,7 +225,7 @@ bool PuzzlePieceExample::run() // Create Task Composer Problem auto problem = std::make_unique(env_, profiles); - problem->input_instruction = program; + problem->input = program; // Solve task tesseract_common::Timer stopwatch; 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 3bf4300337..7f4b90ae0f 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 @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include namespace tesseract_planning @@ -58,6 +59,9 @@ struct TaskComposerProblem /** @brief Indicate if dotgraph should be provided */ bool dotgraph{ false }; + /** @brief The problem input */ + tesseract_common::AnyPoly input; + /** * @brief Clone the planning problem * @return A clone of the planning problem diff --git a/tesseract_task_composer/core/src/task_composer_problem.cpp b/tesseract_task_composer/core/src/task_composer_problem.cpp index f1d49c2ddd..341c716cdf 100644 --- a/tesseract_task_composer/core/src/task_composer_problem.cpp +++ b/tesseract_task_composer/core/src/task_composer_problem.cpp @@ -46,6 +46,7 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const bool equal = true; equal &= name == rhs.name; equal &= dotgraph == rhs.dotgraph; + equal &= input == input; return equal; } @@ -56,6 +57,7 @@ void TaskComposerProblem::serialize(Archive& ar, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("name", name); ar& boost::serialization::make_nvp("dotgraph", dotgraph); + ar& boost::serialization::make_nvp("input", dotgraph); } } // namespace tesseract_planning 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 3463fb94a6..c984dd4a49 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 @@ -86,9 +86,6 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem /** @brief The Profiles to use */ ProfileDictionary::ConstPtr profiles; - /** @brief The problem input instruction */ - InstructionPoly input_instruction; - /** * @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a * given motion planner. diff --git a/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp index bfb37b4be7..07e5775714 100644 --- a/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/process_planning_input_task.cpp @@ -66,10 +66,10 @@ TaskComposerNodeInfo::UPtr ProcessPlanningInputTask::runImpl(TaskComposerContext auto info = std::make_unique(*this); - if (!problem.input_instruction.isCompositeInstruction()) + if (problem.input.isNull() || problem.input.getType() != std::type_index(typeid(CompositeInstruction))) { info->color = "red"; - info->message = "Input instructon is not a Composite Instruction, aborting..."; + info->message = "Input is not a Composite Instruction, aborting..."; info->return_value = 0; // Abort @@ -77,7 +77,7 @@ TaskComposerNodeInfo::UPtr ProcessPlanningInputTask::runImpl(TaskComposerContext return info; } - context.data_storage->setData(output_keys_[0], problem.input_instruction.as()); + context.data_storage->setData(output_keys_[0], problem.input.as()); 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 9af8313b4e..c453aafc86 100644 --- a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp +++ b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp @@ -102,7 +102,6 @@ bool PlanningTaskComposerProblem::operator==(const PlanningTaskComposerProblem& equal &= tesseract_common::pointersEqual(env, rhs.env); equal &= manip_info == rhs.manip_info; // equal &= tesseract_common::pointersEqual(profiles, rhs.profiles); - equal &= input_instruction == rhs.input_instruction; equal &= move_profile_remapping == rhs.move_profile_remapping; equal &= composite_profile_remapping == rhs.composite_profile_remapping; return equal; @@ -118,7 +117,6 @@ void PlanningTaskComposerProblem::serialize(Archive& ar, const unsigned int /*ve ar& boost::serialization::make_nvp("manip_info", manip_info); /** @todo Fix after profiles are serializable */ // ar& boost::serialization::make_nvp("profiles", profiles); - ar& boost::serialization::make_nvp("input_instruction", input_instruction); ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping); ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping); } diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 3beb3ba157..16cbd0bdeb 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -91,9 +91,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_instruction.isNull()); - problem.input_instruction = test_suite::freespaceExampleProgramABB(); - EXPECT_FALSE(problem.input_instruction.isNull()); + EXPECT_TRUE(problem.input.isNull()); + problem.input = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input.isNull()); // Test clone auto clone = problem.clone(); @@ -110,9 +110,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_instruction.isNull()); - problem.input_instruction = test_suite::freespaceExampleProgramABB(); - EXPECT_FALSE(problem.input_instruction.isNull()); + EXPECT_TRUE(problem.input.isNull()); + problem.input = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input.isNull()); // Test clone auto clone = problem.clone(); @@ -129,9 +129,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_instruction.isNull()); - problem.input_instruction = test_suite::freespaceExampleProgramABB(); - EXPECT_FALSE(problem.input_instruction.isNull()); + EXPECT_TRUE(problem.input.isNull()); + problem.input = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input.isNull()); // Test clone auto clone = problem.clone(); @@ -148,9 +148,9 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerPlanningProblemTests) // EXPECT_TRUE(problem.move_profile_remapping.empty()); EXPECT_TRUE(problem.composite_profile_remapping.empty()); EXPECT_TRUE(problem.profiles != nullptr); - EXPECT_TRUE(problem.input_instruction.isNull()); - problem.input_instruction = test_suite::freespaceExampleProgramABB(); - EXPECT_FALSE(problem.input_instruction.isNull()); + EXPECT_TRUE(problem.input.isNull()); + problem.input = test_suite::freespaceExampleProgramABB(); + EXPECT_FALSE(problem.input.isNull()); // Test clone auto clone = problem.clone(); @@ -2554,7 +2554,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // // Create problem auto problem = std::make_unique(env_, profiles); problem->dotgraph = true; - problem->input_instruction = test_suite::rasterExampleProgram(); + problem->input = test_suite::rasterExampleProgram(); // Solve raster plan auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); @@ -3025,7 +3025,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) // Create problem auto problem = std::make_unique(env_, profiles); problem->dotgraph = true; - problem->input_instruction = test_suite::rasterOnlyExampleProgram(); + problem->input = test_suite::rasterOnlyExampleProgram(); // Solve raster plan auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");