Skip to content

Commit 84922a8

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Simplify the use of poly types
1 parent 607e089 commit 84922a8

30 files changed

+229
-236
lines changed

‎tesseract_command_language/include/tesseract_command_language/composite_instruction.h‎

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -189,12 +189,6 @@ class CompositeInstruction : public InstructionInterface
189189
std::vector<InstructionPoly>& getInstructions();
190190
const std::vector<InstructionPoly>& getInstructions() const;
191191

192-
void appendMoveInstruction(const MoveInstructionPoly& mi);
193-
void appendMoveInstruction(const MoveInstructionPoly&& mi);
194-
195-
iterator insertMoveInstruction(const_iterator p, const MoveInstructionPoly& x);
196-
iterator insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x);
197-
198192
/**
199193
* @brief Get the first Move Instruction in a Composite Instruction
200194
* This does not consider the start instruction in child composite instruction
@@ -383,9 +377,10 @@ class CompositeInstruction : public InstructionInterface
383377
void clear();
384378

385379
/** @brief inserts element */
386-
iterator insert(const_iterator p, const value_type& x);
387380
iterator insert(const_iterator p, value_type&& x);
381+
iterator insert(const_iterator p, const value_type& x);
388382
iterator insert(const_iterator p, std::initializer_list<value_type> l);
383+
iterator insert(const_iterator p, const MoveInstructionPoly& x);
389384
template <class InputIt>
390385
void insert(const_iterator pos, InputIt first, InputIt last)
391386
{

‎tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,8 @@ struct uuid;
4747

4848
namespace tesseract_planning
4949
{
50+
class MoveInstructionInterface;
51+
5052
/**
5153
* @brief The InstructionInterface class
5254
*/
@@ -132,6 +134,7 @@ class InstructionPoly
132134
InstructionPoly(InstructionPoly&& other) noexcept = default;
133135
InstructionPoly& operator=(InstructionPoly&& other) noexcept = default;
134136
InstructionPoly(const InstructionInterface& impl);
137+
InstructionPoly(const MoveInstructionInterface& impl);
135138

136139
/**
137140
* @brief Get the UUID

‎tesseract_command_language/src/composite_instruction.cpp‎

Lines changed: 8 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -137,20 +137,6 @@ std::vector<InstructionPoly>& CompositeInstruction::getInstructions() { return c
137137

138138
const std::vector<InstructionPoly>& CompositeInstruction::getInstructions() const { return container_; }
139139

140-
void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly& mi) { container_.emplace_back(mi); }
141-
142-
void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly&& mi) { container_.emplace_back(mi); }
143-
144-
CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p,
145-
const MoveInstructionPoly& x)
146-
{
147-
return container_.insert(p, x);
148-
}
149-
CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x)
150-
{
151-
return container_.insert(p, std::move(x)); // NOLINT
152-
}
153-
154140
MoveInstructionPoly* CompositeInstruction::getFirstMoveInstruction()
155141
{
156142
InstructionPoly* mi = getFirstInstruction(moveFilter);
@@ -310,18 +296,22 @@ CompositeInstruction::const_reference CompositeInstruction::operator[](size_type
310296
///////////////
311297
void CompositeInstruction::clear() { container_.clear(); }
312298

313-
CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, const value_type& x)
314-
{
315-
return container_.insert(p, x);
316-
}
317299
CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, value_type&& x)
318300
{
319301
return container_.insert(p, std::move(x));
320302
}
303+
CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, const value_type& x)
304+
{
305+
return container_.insert(p, x);
306+
}
321307
CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::initializer_list<value_type> l)
322308
{
323309
return container_.insert(p, l);
324310
}
311+
CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, const MoveInstructionPoly& x)
312+
{
313+
return container_.insert(p, x);
314+
}
325315

326316
template <class... Args>
327317
CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args)

‎tesseract_command_language/src/poly/instruction_poly.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,11 @@ InstructionPoly& InstructionPoly::operator=(const InstructionPoly& other)
4040

4141
InstructionPoly::InstructionPoly(const InstructionInterface& impl) : impl_(impl.clone()) {}
4242

43+
InstructionPoly::InstructionPoly(const MoveInstructionInterface& impl)
44+
: impl_(std::make_unique<MoveInstructionPoly>(impl))
45+
{
46+
}
47+
4348
const boost::uuids::uuid& InstructionPoly::getUUID() const { return impl_->getUUID(); }
4449

4550
void InstructionPoly::setUUID(const boost::uuids::uuid& uuid) { impl_->setUUID(uuid); }

‎tesseract_command_language/test/command_language_test_program.hpp‎

Lines changed: 25 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -62,19 +62,19 @@ inline CompositeInstruction getTestProgram(std::string profile,
6262
plan_f0.setDescription("from_start_plan");
6363
CompositeInstruction from_start;
6464
from_start.setDescription("from_start");
65-
from_start.appendMoveInstruction(start_instruction);
66-
from_start.appendMoveInstruction(plan_f0);
65+
from_start.push_back(start_instruction);
66+
from_start.push_back(plan_f0);
6767
program.push_back(from_start);
6868

6969
{
7070
CompositeInstruction raster_segment;
7171
raster_segment.setDescription("raster_segment");
72-
raster_segment.appendMoveInstruction(plan_c0);
73-
raster_segment.appendMoveInstruction(plan_c1);
74-
raster_segment.appendMoveInstruction(plan_c2);
75-
raster_segment.appendMoveInstruction(plan_c3);
76-
raster_segment.appendMoveInstruction(plan_c4);
77-
raster_segment.appendMoveInstruction(plan_c5);
72+
raster_segment.push_back(plan_c0);
73+
raster_segment.push_back(plan_c1);
74+
raster_segment.push_back(plan_c2);
75+
raster_segment.push_back(plan_c3);
76+
raster_segment.push_back(plan_c4);
77+
raster_segment.push_back(plan_c5);
7878
program.push_back(raster_segment);
7979
}
8080

@@ -83,10 +83,10 @@ inline CompositeInstruction getTestProgram(std::string profile,
8383
plan_f1.setDescription("transition_from_end_plan");
8484
CompositeInstruction transition_from_end;
8585
transition_from_end.setDescription("transition_from_end");
86-
transition_from_end.appendMoveInstruction(plan_f1);
86+
transition_from_end.push_back(plan_f1);
8787
CompositeInstruction transition_from_start;
8888
transition_from_start.setDescription("transition_from_start");
89-
transition_from_start.appendMoveInstruction(plan_f1);
89+
transition_from_start.push_back(plan_f1);
9090

9191
CompositeInstruction transitions(
9292
DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED);
@@ -99,12 +99,12 @@ inline CompositeInstruction getTestProgram(std::string profile,
9999
{
100100
CompositeInstruction raster_segment;
101101
raster_segment.setDescription("raster_segment");
102-
raster_segment.appendMoveInstruction(plan_c0);
103-
raster_segment.appendMoveInstruction(plan_c1);
104-
raster_segment.appendMoveInstruction(plan_c2);
105-
raster_segment.appendMoveInstruction(plan_c3);
106-
raster_segment.appendMoveInstruction(plan_c4);
107-
raster_segment.appendMoveInstruction(plan_c5);
102+
raster_segment.push_back(plan_c0);
103+
raster_segment.push_back(plan_c1);
104+
raster_segment.push_back(plan_c2);
105+
raster_segment.push_back(plan_c3);
106+
raster_segment.push_back(plan_c4);
107+
raster_segment.push_back(plan_c5);
108108
program.push_back(raster_segment);
109109
}
110110

@@ -113,10 +113,10 @@ inline CompositeInstruction getTestProgram(std::string profile,
113113
plan_f1.setDescription("transition_from_end_plan");
114114
CompositeInstruction transition_from_end;
115115
transition_from_end.setDescription("transition_from_end");
116-
transition_from_end.appendMoveInstruction(plan_f1);
116+
transition_from_end.push_back(plan_f1);
117117
CompositeInstruction transition_from_start;
118118
transition_from_start.setDescription("transition_from_start");
119-
transition_from_start.appendMoveInstruction(plan_f1);
119+
transition_from_start.push_back(plan_f1);
120120

121121
CompositeInstruction transitions(
122122
DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED);
@@ -129,18 +129,18 @@ inline CompositeInstruction getTestProgram(std::string profile,
129129
{
130130
CompositeInstruction raster_segment;
131131
raster_segment.setDescription("raster_segment");
132-
raster_segment.appendMoveInstruction(plan_c0);
133-
raster_segment.appendMoveInstruction(plan_c1);
134-
raster_segment.appendMoveInstruction(plan_c2);
135-
raster_segment.appendMoveInstruction(plan_c3);
136-
raster_segment.appendMoveInstruction(plan_c4);
137-
raster_segment.appendMoveInstruction(plan_c5);
132+
raster_segment.push_back(plan_c0);
133+
raster_segment.push_back(plan_c1);
134+
raster_segment.push_back(plan_c2);
135+
raster_segment.push_back(plan_c3);
136+
raster_segment.push_back(plan_c4);
137+
raster_segment.push_back(plan_c5);
138138
program.push_back(raster_segment);
139139
}
140140

141141
CompositeInstruction to_end;
142142
to_end.setDescription("to_end");
143-
to_end.appendMoveInstruction(end_instruction);
143+
to_end.push_back(end_instruction);
144144
program.push_back(to_end);
145145

146146
// Add a wait instruction

‎tesseract_command_language/test/command_language_unit.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -661,7 +661,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT
661661
auto mis = instr.flatten(moveFilter);
662662
T insert_mi_program(profile, manip_info, order);
663663
for (const auto& mi : mis)
664-
insert_mi_program.insertMoveInstruction(insert_mi_program.end(), mi.get().as<MoveInstructionPoly>());
664+
insert_mi_program.insert(insert_mi_program.end(), mi.get().as<MoveInstructionPoly>());
665665

666666
const auto& nmis = insert_mi_program.getInstructions();
667667
EXPECT_EQ(mis.size(), insert_mi_program.size());

‎tesseract_command_language/test/command_language_utils_unit.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ TEST(TesseractCommandLanguageUtilsUnit, flatten) // NOLINT
6161
MoveInstruction instruction(wp, MoveInstructionType::LINEAR);
6262
instruction.setDescription("instruction_" + std::to_string(i) + "_" + std::to_string(j) + "_" +
6363
std::to_string(k));
64-
sub_sub_composite.appendMoveInstruction(instruction);
64+
sub_sub_composite.push_back(instruction);
6565
}
6666
sub_composite.push_back(sub_sub_composite);
6767
}
@@ -573,17 +573,17 @@ TEST(TesseractCommandLanguageUtilsUnit, toDelimitedFile) // NOLINT
573573
Eigen::VectorXd values = Eigen::VectorXd::Constant(3, 5);
574574
{
575575
JointWaypoint jwp{ joint_names, values };
576-
composite.appendMoveInstruction(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
576+
composite.push_back(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
577577
}
578578
{
579579
values = Eigen::VectorXd::Constant(3, 10);
580580
JointWaypoint jwp{ joint_names, values };
581-
composite.appendMoveInstruction(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
581+
composite.push_back(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
582582
}
583583
{
584584
values = Eigen::VectorXd::Constant(3, 15);
585585
JointWaypoint jwp{ joint_names, values };
586-
composite.appendMoveInstruction(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
586+
composite.push_back(MoveInstruction(jwp, MoveInstructionType::FREESPACE));
587587
}
588588

589589
std::string path = tesseract_common::getTempPath() + "to_delimited_file.csv";

‎tesseract_command_language/test/type_erasure_benchmark.cpp‎

Lines changed: 25 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -87,19 +87,19 @@ CompositeInstruction getProgram()
8787
plan_f0.setDescription("from_start_plan");
8888
CompositeInstruction from_start;
8989
from_start.setDescription("from_start");
90-
from_start.appendMoveInstruction(start_instruction);
91-
from_start.appendMoveInstruction(plan_f0);
90+
from_start.push_back(start_instruction);
91+
from_start.push_back(plan_f0);
9292
program.push_back(from_start);
9393

9494
{
9595
CompositeInstruction raster_segment;
9696
raster_segment.setDescription("raster_segment");
97-
raster_segment.appendMoveInstruction(plan_c0);
98-
raster_segment.appendMoveInstruction(plan_c1);
99-
raster_segment.appendMoveInstruction(plan_c2);
100-
raster_segment.appendMoveInstruction(plan_c3);
101-
raster_segment.appendMoveInstruction(plan_c4);
102-
raster_segment.appendMoveInstruction(plan_c5);
97+
raster_segment.push_back(plan_c0);
98+
raster_segment.push_back(plan_c1);
99+
raster_segment.push_back(plan_c2);
100+
raster_segment.push_back(plan_c3);
101+
raster_segment.push_back(plan_c4);
102+
raster_segment.push_back(plan_c5);
103103
program.push_back(raster_segment);
104104
}
105105

@@ -108,10 +108,10 @@ CompositeInstruction getProgram()
108108
plan_f1.setDescription("transition_from_end_plan");
109109
CompositeInstruction transition_from_end;
110110
transition_from_end.setDescription("transition_from_end");
111-
transition_from_end.appendMoveInstruction(plan_f1);
111+
transition_from_end.push_back(plan_f1);
112112
CompositeInstruction transition_from_start;
113113
transition_from_start.setDescription("transition_from_start");
114-
transition_from_start.appendMoveInstruction(plan_f1);
114+
transition_from_start.push_back(plan_f1);
115115

116116
CompositeInstruction transitions(
117117
DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED);
@@ -124,12 +124,12 @@ CompositeInstruction getProgram()
124124
{
125125
CompositeInstruction raster_segment;
126126
raster_segment.setDescription("raster_segment");
127-
raster_segment.appendMoveInstruction(plan_c0);
128-
raster_segment.appendMoveInstruction(plan_c1);
129-
raster_segment.appendMoveInstruction(plan_c2);
130-
raster_segment.appendMoveInstruction(plan_c3);
131-
raster_segment.appendMoveInstruction(plan_c4);
132-
raster_segment.appendMoveInstruction(plan_c5);
127+
raster_segment.push_back(plan_c0);
128+
raster_segment.push_back(plan_c1);
129+
raster_segment.push_back(plan_c2);
130+
raster_segment.push_back(plan_c3);
131+
raster_segment.push_back(plan_c4);
132+
raster_segment.push_back(plan_c5);
133133
program.push_back(raster_segment);
134134
}
135135

@@ -138,10 +138,10 @@ CompositeInstruction getProgram()
138138
plan_f1.setDescription("transition_from_end_plan");
139139
CompositeInstruction transition_from_end;
140140
transition_from_end.setDescription("transition_from_end");
141-
transition_from_end.appendMoveInstruction(plan_f1);
141+
transition_from_end.push_back(plan_f1);
142142
CompositeInstruction transition_from_start;
143143
transition_from_start.setDescription("transition_from_start");
144-
transition_from_start.appendMoveInstruction(plan_f1);
144+
transition_from_start.push_back(plan_f1);
145145

146146
CompositeInstruction transitions(
147147
DEFAULT_PROFILE_KEY, tesseract_common::ManipulatorInfo(), CompositeInstructionOrder::UNORDERED);
@@ -154,20 +154,20 @@ CompositeInstruction getProgram()
154154
{
155155
CompositeInstruction raster_segment;
156156
raster_segment.setDescription("raster_segment");
157-
raster_segment.appendMoveInstruction(plan_c0);
158-
raster_segment.appendMoveInstruction(plan_c1);
159-
raster_segment.appendMoveInstruction(plan_c2);
160-
raster_segment.appendMoveInstruction(plan_c3);
161-
raster_segment.appendMoveInstruction(plan_c4);
162-
raster_segment.appendMoveInstruction(plan_c5);
157+
raster_segment.push_back(plan_c0);
158+
raster_segment.push_back(plan_c1);
159+
raster_segment.push_back(plan_c2);
160+
raster_segment.push_back(plan_c3);
161+
raster_segment.push_back(plan_c4);
162+
raster_segment.push_back(plan_c5);
163163
program.push_back(raster_segment);
164164
}
165165

166166
MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile");
167167
plan_f2.setDescription("to_end_plan");
168168
CompositeInstruction to_end;
169169
to_end.setDescription("to_end");
170-
to_end.appendMoveInstruction(plan_f2);
170+
to_end.push_back(plan_f2);
171171
program.push_back(to_end);
172172

173173
// Add a wait instruction

‎tesseract_examples/src/basic_cartesian_example.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -209,10 +209,10 @@ bool BasicCartesianExample::run()
209209
plan_f1.setDescription("to_end_plan");
210210

211211
// Add Instructions to program
212-
program.appendMoveInstruction(start_instruction);
213-
program.appendMoveInstruction(plan_f0);
214-
program.appendMoveInstruction(plan_c0);
215-
program.appendMoveInstruction(plan_f1);
212+
program.push_back(start_instruction);
213+
program.push_back(plan_f0);
214+
program.push_back(plan_c0);
215+
program.push_back(plan_f1);
216216

217217
// Print Diagnostics
218218
program.print("Program: ");

‎tesseract_examples/src/car_seat_example.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -369,8 +369,8 @@ bool CarSeatExample::run()
369369
plan_f0.setDescription("Freespace pick seat 1");
370370

371371
// Add Instructions to program
372-
program.appendMoveInstruction(start_instruction);
373-
program.appendMoveInstruction(plan_f0);
372+
program.push_back(start_instruction);
373+
program.push_back(plan_f0);
374374

375375
// Print Diagnostics
376376
program.print("Program: ");
@@ -455,8 +455,8 @@ bool CarSeatExample::run()
455455
plan_f0.setDescription("Freespace pick seat 1");
456456

457457
// Add Instructions to program
458-
program.appendMoveInstruction(start_instruction);
459-
program.appendMoveInstruction(plan_f0);
458+
program.push_back(start_instruction);
459+
program.push_back(plan_f0);
460460

461461
// Print Diagnostics
462462
program.print("Program: ");

0 commit comments

Comments
 (0)