Skip to content

Commit

Permalink
Modify composite instruction constructor
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jan 4, 2025
1 parent de4fba4 commit f6c8709
Show file tree
Hide file tree
Showing 15 changed files with 34 additions and 48 deletions.
4 changes: 2 additions & 2 deletions tesseract_command_language/include/tesseract_command_language/composite_instruction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,8 @@ class CompositeInstruction
using const_reverse_iterator = typename std::vector<value_type>::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 <class InputIt>
CompositeInstruction(InputIt first, InputIt last) : CompositeInstruction()
Expand Down
4 changes: 2 additions & 2 deletions tesseract_command_language/src/composite_instruction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
8 changes: 5 additions & 3 deletions tesseract_command_language/test/command_language_test_program.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions tesseract_command_language/test/command_language_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<MoveInstructionPoly>());

Expand Down
9 changes: 5 additions & 4 deletions tesseract_command_language/test/type_erasure_benchmark.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 1 addition & 2 deletions tesseract_examples/src/basic_cartesian_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) };
Expand Down
6 changes: 2 additions & 4 deletions tesseract_examples/src/car_seat_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
3 changes: 1 addition & 2 deletions tesseract_examples/src/freespace_hybrid_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) };
Expand Down
3 changes: 1 addition & 2 deletions tesseract_examples/src/freespace_ompl_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) };
Expand Down
3 changes: 1 addition & 2 deletions tesseract_examples/src/glass_upright_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) };
Expand Down
8 changes: 2 additions & 6 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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) };
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion tesseract_examples/src/puzzle_piece_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
18 changes: 6 additions & 12 deletions ...ract_task_composer/core/include/tesseract_task_composer/core/test_suite/test_programs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4",
Expand Down Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand All @@ -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<std::string> joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4",
Expand Down Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand Down Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand Down Expand Up @@ -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)
{
Expand Down
3 changes: 1 addition & 2 deletions tesseract_task_composer/test/fix_state_bounds_task_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
Expand Down

0 comments on commit f6c8709

Please sign in to comment.