diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index a76c0cc8da..47b4ca72c5 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -110,8 +110,8 @@ class CompositeInstruction using const_reverse_iterator = typename std::vector::const_reverse_iterator; CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY, - CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED, - tesseract_common::ManipulatorInfo manipulator_info = tesseract_common::ManipulatorInfo()); + tesseract_common::ManipulatorInfo manipulator_info = tesseract_common::ManipulatorInfo(), + CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED); template CompositeInstruction(InputIt first, InputIt last) : CompositeInstruction() diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 5da63e0017..20e7b7417b 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -54,8 +54,8 @@ bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& } CompositeInstruction::CompositeInstruction(std::string profile, - CompositeInstructionOrder order, - tesseract_common::ManipulatorInfo manipulator_info) + tesseract_common::ManipulatorInfo manipulator_info, + CompositeInstructionOrder order) : uuid_(boost::uuids::random_generator()()) , manipulator_info_(std::move(manipulator_info)) , profile_(std::move(profile)) diff --git a/tesseract_command_language/test/command_language_test_program.hpp b/tesseract_command_language/test/command_language_test_program.hpp index 03271baab3..67961b5432 100644 --- a/tesseract_command_language/test/command_language_test_program.hpp +++ b/tesseract_command_language/test/command_language_test_program.hpp @@ -17,7 +17,7 @@ inline CompositeInstruction getTestProgram(std::string profile, CompositeInstructionOrder order, tesseract_common::ManipulatorInfo manipulator_info) { - CompositeInstruction program(std::move(profile), order, std::move(manipulator_info)); + CompositeInstruction program(std::move(profile), std::move(manipulator_info), order); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; @@ -88,7 +88,8 @@ inline CompositeInstruction getTestProgram(std::string profile, transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions( + DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); @@ -117,7 +118,8 @@ inline CompositeInstruction getTestProgram(std::string profile, transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions( + DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); diff --git a/tesseract_command_language/test/command_language_unit.cpp b/tesseract_command_language/test/command_language_unit.cpp index 6e4077c103..7df9658ac9 100644 --- a/tesseract_command_language/test/command_language_unit.cpp +++ b/tesseract_command_language/test/command_language_unit.cpp @@ -464,7 +464,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(insert_program == instr); EXPECT_TRUE(insert_program != instr); - T assign_program(profile, order, manip_info); + T assign_program(profile, manip_info, order); assign_program.setUUID(instr.getUUID()); assign_program.setParentUUID(instr.getParentUUID()); assign_program.setInstructions(instr.getInstructions()); @@ -535,7 +535,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_TRUE(copy_program == instr); EXPECT_FALSE(copy_program != instr); - T empty_program(profile, order, manip_info); + T empty_program(profile, manip_info, order); EXPECT_FALSE(empty_program.getUUID().is_nil()); EXPECT_TRUE(empty_program.getParentUUID().is_nil()); EXPECT_EQ(empty_program.getProfile(), profile); @@ -662,7 +662,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT // Flatten auto mis = instr.flatten(moveFilter); - T insert_mi_program(profile, order, manip_info); + T insert_mi_program(profile, manip_info, order); for (const auto& mi : mis) insert_mi_program.insertMoveInstruction(insert_mi_program.end(), mi.get().as()); diff --git a/tesseract_command_language/test/type_erasure_benchmark.cpp b/tesseract_command_language/test/type_erasure_benchmark.cpp index ae613d55ca..c77278e8ab 100644 --- a/tesseract_command_language/test/type_erasure_benchmark.cpp +++ b/tesseract_command_language/test/type_erasure_benchmark.cpp @@ -52,8 +52,7 @@ using tesseract_common::ManipulatorInfo; CompositeInstruction getProgram() { - CompositeInstruction program( - "raster_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "tool0")); + CompositeInstruction program("raster_program", ManipulatorInfo("manipulator", "world", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; @@ -114,7 +113,8 @@ CompositeInstruction getProgram() transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions( + DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); @@ -143,7 +143,8 @@ CompositeInstruction getProgram() transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions( + DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index ff307f5724..7b12ded29c 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -181,8 +181,7 @@ bool BasicCartesianExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program( - "cartesian_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program("cartesian_program", ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_pos) }; diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index bc9e461979..42c5a09fcd 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -350,8 +350,7 @@ bool CarSeatExample::run() CONSOLE_BRIDGE_logInform("Car Seat Demo Started"); { // Create Program to pick up first seat - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "end_effector")); + CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Pick up the first seat!"); // Start and End Joint Position for the program @@ -437,8 +436,7 @@ bool CarSeatExample::run() return false; { // Create Program to place first seat - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "end_effector")); + CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Place the first seat!"); // Start and End Joint Position for the program diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index c89589adf3..6607e9eefc 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -167,8 +167,7 @@ bool FreespaceHybridExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index f843a581cb..3f0eff5535 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -153,8 +153,7 @@ bool FreespaceOMPLExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program("FREESPACE", ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 51f40022fa..9bc14af4cd 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -169,8 +169,7 @@ bool GlassUprightExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program( - "UPRIGHT", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program("UPRIGHT", ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 4815440e6e..547158ecf9 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -184,9 +184,7 @@ bool PickAndPlaceExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction pick_program("DEFAULT", - CompositeInstructionOrder::ORDERED, - ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); + CompositeInstruction pick_program("DEFAULT", ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); StateWaypointPoly pick_swp{ StateWaypoint(joint_names, joint_pos) }; MoveInstruction start_instruction(pick_swp, MoveInstructionType::FREESPACE, "FREESPACE"); @@ -384,9 +382,7 @@ bool PickAndPlaceExample::run() place_approach_pose.translation() += Eigen::Vector3d(0.0, -0.25, 0); // Create Program - CompositeInstruction place_program("DEFAULT", - CompositeInstructionOrder::ORDERED, - ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); + CompositeInstruction place_program("DEFAULT", ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); // Define the approach pose CartesianWaypointPoly place_wp0{ CartesianWaypoint(retreat_pose) }; diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index c4dfe33ce4..8df6dcc160 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -203,7 +203,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); + CompositeInstruction program("DEFAULT", mi); // Create cartesian waypoint for (std::size_t i = 0; i < tool_poses.size(); ++i) diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 6d6a7e21be..a969e1b923 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -197,7 +197,7 @@ bool PuzzlePieceExample::run() TaskComposerPluginFactory factory(config_path); // Create Program - CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); + CompositeInstruction program("DEFAULT", mi); // Create cartesian waypoint for (std::size_t i = 0; i < tool_poses.size(); ++i) diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_programs.hpp b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_programs.hpp index 200e01371c..d7e1d90d45 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_programs.hpp +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_programs.hpp @@ -43,8 +43,7 @@ inline CompositeInstruction freespaceExampleProgramIIWA( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(composite_profile, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", @@ -72,8 +71,7 @@ inline CompositeInstruction freespaceExampleProgramABB( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(composite_profile, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; @@ -100,8 +98,7 @@ jointInterpolatedExampleSolutionIIWA(bool use_joint_waypoint = false, const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(composite_profile, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", @@ -146,8 +143,7 @@ jointInterpolateExampleProgramABB(bool use_joint_waypoint = false, const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(composite_profile, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; @@ -189,8 +185,7 @@ jointInterpolateExampleProgramABB(bool use_joint_waypoint = false, inline CompositeInstruction rasterExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(DEFAULT_PROFILE_KEY, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; @@ -295,8 +290,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(DEFAULT_PROFILE_KEY, ManipulatorInfo("manipulator", "base_link", "tool0")); for (int i = 0; i < 4; ++i) { 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 55c8d9def9..6759063820 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -50,8 +50,7 @@ CompositeInstruction createProgram(const Eigen::VectorXd& start_state, const Eigen::VectorXd& goal_state, const std::string& composite_profile) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + CompositeInstruction program(composite_profile, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };