Skip to content

Commit

Permalink
Remove composite start instruction
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jan 12, 2023
1 parent e57aaf2 commit 6c006bf
Show file tree
Hide file tree
Showing 49 changed files with 483 additions and 683 deletions.
138 changes: 72 additions & 66 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 @@ -50,14 +50,10 @@ class CompositeInstruction;
*
* The filter should return true when the instruction passed should be included not throw.
*/
using flattenFilterFn =
std::function<bool(const InstructionPoly&, const CompositeInstruction&, bool parent_is_first_composite)>;
using locateFilterFn =
std::function<bool(const InstructionPoly&, const CompositeInstruction&, bool parent_is_first_composite)>;
using flattenFilterFn = std::function<bool(const InstructionPoly&, const CompositeInstruction&)>;
using locateFilterFn = std::function<bool(const InstructionPoly&, const CompositeInstruction&)>;

bool moveFilter(const InstructionPoly& instruction,
const CompositeInstruction& composite,
bool parent_is_first_composite);
bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& composite);

enum class CompositeInstructionOrder
{
Expand All @@ -73,10 +69,38 @@ class CompositeInstruction
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// LCOV_EXCL_STOP

/** value_type */
using value_type = InstructionPoly;
/** pointer */
using pointer = typename std::vector<value_type>::pointer;
/** const_pointer */
using const_pointer = typename std::vector<value_type>::const_pointer;
/** reference */
using reference = typename std::vector<value_type>::reference;
/** const_reference */
using const_reference = typename std::vector<value_type>::const_reference;
/** size_type */
using size_type = typename std::vector<value_type>::size_type;
/** difference_type */
using difference_type = typename std::vector<value_type>::difference_type;
/** iterator */
using iterator = typename std::vector<value_type>::iterator;
/** const_iterator */
using const_iterator = typename std::vector<value_type>::const_iterator;
/** reverse_iterator */
using reverse_iterator = typename std::vector<value_type>::reverse_iterator;
/** const_reverse_iterator */
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());

template <class InputIt>
CompositeInstruction(InputIt first, InputIt last) : container_(first, last)
{
}

CompositeInstructionOrder getOrder() const;

const boost::uuids::uuid& getUUID() const;
Expand All @@ -100,22 +124,15 @@ class CompositeInstruction
const tesseract_common::ManipulatorInfo& getManipulatorInfo() const;
tesseract_common::ManipulatorInfo& getManipulatorInfo();

void setStartInstruction(MoveInstructionPoly instruction);

void resetStartInstruction();
const MoveInstructionPoly& getStartInstruction() const;
MoveInstructionPoly& getStartInstruction();
bool hasStartInstruction() const;

void setInstructions(std::vector<tesseract_planning::InstructionPoly> instructions);
std::vector<tesseract_planning::InstructionPoly>& getInstructions();
const std::vector<tesseract_planning::InstructionPoly>& getInstructions() const;

void appendMoveInstruction(const MoveInstructionPoly& mi);
void appendMoveInstruction(const MoveInstructionPoly&& mi);

void appendInstruction(const InstructionPoly& i);
void appendInstruction(const InstructionPoly&& i);
iterator insertMoveInstruction(const_iterator p, const MoveInstructionPoly& x);
iterator insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x);

/**
* @brief Get the first Move Instruction in a Composite Instruction
Expand Down Expand Up @@ -228,34 +245,6 @@ class CompositeInstruction

// C++ container support

/** value_type */
using value_type = InstructionPoly;
/** pointer */
using pointer = typename std::vector<value_type>::pointer;
/** const_pointer */
using const_pointer = typename std::vector<value_type>::const_pointer;
/** reference */
using reference = typename std::vector<value_type>::reference;
/** const_reference */
using const_reference = typename std::vector<value_type>::const_reference;
/** size_type */
using size_type = typename std::vector<value_type>::size_type;
/** difference_type */
using difference_type = typename std::vector<value_type>::difference_type;
/** iterator */
using iterator = typename std::vector<value_type>::iterator;
/** const_iterator */
using const_iterator = typename std::vector<value_type>::const_iterator;
/** reverse_iterator */
using reverse_iterator = typename std::vector<value_type>::reverse_iterator;
/** const_reverse_iterator */
using const_reverse_iterator = typename std::vector<value_type>::const_reverse_iterator;

// template <class InputIt>
// CompositeInstruction(InputIt first, InputIt last) : container_(first, last)
// {
// }

///////////////
// Iterators //
///////////////
Expand Down Expand Up @@ -330,10 +319,42 @@ class CompositeInstruction
/** @brief clears the contents */
void clear();

/** @brief inserts element */
iterator insert(const_iterator p, const value_type& x);
iterator insert(const_iterator p, value_type&& x);
iterator insert(const_iterator p, std::initializer_list<value_type> l);
template <class InputIt>
void insert(const_iterator pos, InputIt first, InputIt last)
{
container_.insert(pos, first, last);
}

/** @brief constructs element in-place */
template <class... Args>
iterator emplace(const_iterator pos, Args&&... args);

/** @brief erases element */
iterator erase(const_iterator p);
iterator erase(const_iterator first, const_iterator last);

/** @brief adds an element to the end */
void push_back(const value_type& x);
void push_back(const value_type&& x);

/** @brief constructs an element in-place at the end */
template <typename... Args>
#if __cplusplus > 201402L
reference emplace_back(Args&&... args)
{
return container_.emplace_back(std::forward<Args>(args)...);
}
#else
void emplace_back(Args&&... args)
{
container_.emplace_back(std::forward<Args>(args)...);
}
#endif

/** @brief removes the last element */
void pop_back();

Expand Down Expand Up @@ -368,38 +389,25 @@ class CompositeInstruction
/** @brief The order of the composite instruction */
CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };

/**
* @brief The start instruction to use for composite instruction. This should be of type
* MoveInstruction but is stored as type Instruction because it is not required
*
* If not provided, the planner should use the current state of the robot is used and defined as fixed.
*/
InstructionPoly start_instruction_;

const InstructionPoly* getFirstInstructionHelper(const CompositeInstruction& composite_instruction,
const locateFilterFn& locate_filter,
bool process_child_composites,
bool first_composite) const;
bool process_child_composites) const;

InstructionPoly* getFirstInstructionHelper(CompositeInstruction& composite_instruction,
const locateFilterFn& locate_filter,
bool process_child_composites,
bool first_composite);
bool process_child_composites);

const InstructionPoly* getLastInstructionHelper(const CompositeInstruction& composite_instruction,
const locateFilterFn& locate_filter,
bool process_child_composites,
bool first_composite) const;
bool process_child_composites) const;

InstructionPoly* getLastInstructionHelper(CompositeInstruction& composite_instruction,
const locateFilterFn& locate_filter,
bool process_child_composites,
bool first_composite);
bool process_child_composites);

long getInstructionCountHelper(const CompositeInstruction& composite_instruction,
const locateFilterFn& locate_filter,
bool process_child_composites,
bool first_composite) const;
bool process_child_composites) const;

/**
* @brief Helper function used by Flatten. Not intended for direct use
Expand All @@ -410,8 +418,7 @@ class CompositeInstruction
*/
void flattenHelper(std::vector<std::reference_wrapper<InstructionPoly>>& flattened,
CompositeInstruction& composite,
const flattenFilterFn& filter,
bool first_composite);
const flattenFilterFn& filter);

/**
* @brief Helper function used by Flatten. Not intended for direct use
Expand All @@ -422,8 +429,7 @@ class CompositeInstruction
*/
void flattenHelper(std::vector<std::reference_wrapper<const InstructionPoly>>& flattened,
const CompositeInstruction& composite,
const flattenFilterFn& filter,
bool first_composite) const;
const flattenFilterFn& filter) const;

friend class boost::serialization::access;
template <class Archive>
Expand Down
2 changes: 1 addition & 1 deletion tesseract_command_language/include/tesseract_command_language/move_instruction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ class MoveInstruction
boost::uuids::uuid parent_uuid_{};

/** @brief The move instruction type */
MoveInstructionType move_type_{ MoveInstructionType::START };
MoveInstructionType move_type_{ MoveInstructionType::FREESPACE };

/** @brief The description of the instruction */
std::string description_{ "Tesseract Move Instruction" };
Expand Down
3 changes: 0 additions & 3 deletions tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,6 @@ enum class MoveInstructionType : int
LINEAR = 0,
FREESPACE = 1,
CIRCULAR = 2,
START = 3 /**< This indicates it is a start instruction. */
};
} // namespace tesseract_planning

Expand Down Expand Up @@ -345,8 +344,6 @@ struct MoveInstructionPoly : MoveInstructionPolyBase

bool isCircular() const;

bool isStart() const;

bool isChild() const;

private:
Expand Down
Loading

0 comments on commit 6c006bf

Please sign in to comment.