Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Modify composite instruction constructor #572

Merged
merged 1 commit into from
Jan 4, 2025
Merged

Modify composite instruction constructor #572

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions tesseract_command_language/include/tesseract_command_language/composite_instruction.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading