Skip to content

Commit

Permalink
Merge branch 'feat/ProfileRefactor' into feature/new-planner-profiles-v2
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 authored Jan 16, 2023
2 parents d0f4a35 + fafbd46 commit 4cd7f79
Show file tree
Hide file tree
Showing 69 changed files with 747 additions and 721 deletions.
2 changes: 2 additions & 0 deletions tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,9 @@ namespace tesseract_planning
class CartesianWaypoint
{
public:
// LCOV_EXCL_START
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// LCOV_EXCL_STOP

CartesianWaypoint() = default;
CartesianWaypoint(const Eigen::Isometry3d& transform);
Expand Down
152 changes: 86 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 @@ -69,14 +65,50 @@ enum class CompositeInstructionOrder
class CompositeInstruction
{
public:
// LCOV_EXCL_START
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;
void regenerateUUID();

const boost::uuids::uuid& getParentUUID() const;
void setParentUUID(const boost::uuids::uuid& uuid);

void setDescription(const std::string& description);
const std::string& getDescription() const;

Expand All @@ -92,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 @@ -220,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 @@ -322,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 All @@ -335,6 +364,12 @@ class CompositeInstruction
private:
std::vector<InstructionPoly> container_;

/** @brief The instructions UUID */
boost::uuids::uuid uuid_{};

/** @brief The parent UUID if created from createChild */
boost::uuids::uuid parent_uuid_{};

/** @brief The description of the instruction */
std::string description_{ "Tesseract Composite Instruction" };

Expand All @@ -354,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 @@ -396,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 @@ -408,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: 2 additions & 0 deletions tesseract_command_language/include/tesseract_command_language/joint_waypoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,9 @@ namespace tesseract_planning
class JointWaypoint
{
public:
// LCOV_EXCL_START
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// LCOV_EXCL_STOP

JointWaypoint() = default;
JointWaypoint(std::vector<std::string> names, const Eigen::VectorXd& position, bool is_constrained = true);
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
28 changes: 28 additions & 0 deletions tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <boost/concept_check.hpp>
#include <boost/uuid/uuid.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/poly/waypoint_poly.h>
Expand Down Expand Up @@ -86,6 +87,15 @@ struct InstructionConcept // NOLINT
UNUSED(eq);
UNUSED(neq);

const auto& uuid = c.getUUID();
UNUSED(uuid);

c.regenerateUUID();
c.setParentUUID(uuid);

const auto& parent_uuid = c.getParentUUID();
UNUSED(parent_uuid);

const std::string& desc = c.getDescription();
UNUSED(desc);

Expand All @@ -100,6 +110,12 @@ struct InstructionConcept // NOLINT

struct InstructionInterface : tesseract_common::TypeErasureInterface
{
virtual const boost::uuids::uuid& getUUID() const = 0;
virtual void regenerateUUID() = 0;

virtual const boost::uuids::uuid& getParentUUID() const = 0;
virtual void setParentUUID(const boost::uuids::uuid& uuid) = 0;

virtual const std::string& getDescription() const = 0;

virtual void setDescription(const std::string& description) = 0;
Expand All @@ -123,6 +139,12 @@ struct InstructionInstance : tesseract_common::TypeErasureInstance<T, Instructio

BOOST_CONCEPT_ASSERT((InstructionConcept<T>));

const boost::uuids::uuid& getUUID() const final { return this->get().getUUID(); }
void regenerateUUID() final { this->get().regenerateUUID(); }

const boost::uuids::uuid& getParentUUID() const final { return this->get().getParentUUID(); }
void setParentUUID(const boost::uuids::uuid& uuid) final { this->get().setParentUUID(uuid); }

const std::string& getDescription() const final { return this->get().getDescription(); }

void setDescription(const std::string& description) final { this->get().setDescription(description); }
Expand All @@ -148,6 +170,12 @@ struct InstructionPoly : InstructionPolyBase
{
using InstructionPolyBase::InstructionPolyBase;

const boost::uuids::uuid& getUUID() const;
void regenerateUUID();

const boost::uuids::uuid& getParentUUID() const;
void setParentUUID(const boost::uuids::uuid& uuid);

const std::string& getDescription() const;

void setDescription(const std::string& description);
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
10 changes: 10 additions & 0 deletions tesseract_command_language/include/tesseract_command_language/set_analog_instruction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,12 @@ class SetAnalogInstruction
SetAnalogInstruction() = default; // Required for boost serialization do not use
SetAnalogInstruction(std::string key, int index, double value);

const boost::uuids::uuid& getUUID() const;
void regenerateUUID();

const boost::uuids::uuid& getParentUUID() const;
void setParentUUID(const boost::uuids::uuid& uuid);

const std::string& getDescription() const;

void setDescription(const std::string& description);
Expand Down Expand Up @@ -71,6 +77,10 @@ class SetAnalogInstruction
bool operator!=(const SetAnalogInstruction& rhs) const;

private:
/** @brief The instructions UUID */
boost::uuids::uuid uuid_{};
/** @brief The parent UUID if created from createChild */
boost::uuids::uuid parent_uuid_{};
/** @brief The description of the instruction */
std::string description_{ "Tesseract Set Analog Instruction" };
/** @brief The key is used to identify which type of analog to set */
Expand Down
Loading

0 comments on commit 4cd7f79

Please sign in to comment.