From 598b25f119ab64a5bd3a18b4c25e82e6eaf4a521 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Dec 2022 19:47:22 -0600 Subject: [PATCH 1/5] Add uuid and parent_uuid to InstructionPoly (#261) --- .../cartesian_waypoint.h | 2 ++ .../composite_instruction.h | 14 ++++++++ .../joint_waypoint.h | 2 ++ .../poly/instruction_poly.h | 28 ++++++++++++++++ .../set_analog_instruction.h | 10 ++++++ .../set_tool_instruction.h | 11 +++++++ .../timer_instruction.h | 10 ++++++ .../wait_instruction.h | 10 ++++++ .../src/composite_instruction.cpp | 16 ++++++++- .../src/poly/instruction_poly.cpp | 14 ++++++++ .../src/set_analog_instruction.cpp | 13 +++++++- .../src/set_tool_instruction.cpp | 13 +++++++- .../src/timer_instruction.cpp | 13 +++++++- .../src/wait_instruction.cpp | 11 +++++++ .../task_composer_input.h | 2 +- .../task_composer_node_info.h | 4 +-- .../src/task_composer_data_storage.cpp | 1 + .../src/task_composer_input.cpp | 9 +++++ .../src/task_composer_node_info.cpp | 33 +++++++++++++++++++ 19 files changed, 209 insertions(+), 7 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h b/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h index 6e879422631..5a444b74e90 100644 --- a/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h +++ b/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h @@ -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); diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index f4d833ae5de..5c04ff289b9 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -69,7 +69,9 @@ enum class CompositeInstructionOrder class CompositeInstruction { public: + // LCOV_EXCL_START EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY, CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED, @@ -77,6 +79,12 @@ class CompositeInstruction 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; @@ -335,6 +343,12 @@ class CompositeInstruction private: std::vector 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" }; diff --git a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h index 48739b60a3b..7a383a2a9df 100644 --- a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h +++ b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h @@ -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 names, const Eigen::VectorXd& position, bool is_constrained = true); diff --git a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h index f785e08ccc9..7cf8796f7d9 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -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); @@ -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; @@ -123,6 +139,12 @@ struct InstructionInstance : tesseract_common::TypeErasureInstance)); + 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); } @@ -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); diff --git a/tesseract_command_language/include/tesseract_command_language/set_analog_instruction.h b/tesseract_command_language/include/tesseract_command_language/set_analog_instruction.h index ab15c154db3..60b6ee36829 100644 --- a/tesseract_command_language/include/tesseract_command_language/set_analog_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/set_analog_instruction.h @@ -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); @@ -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 */ diff --git a/tesseract_command_language/include/tesseract_command_language/set_tool_instruction.h b/tesseract_command_language/include/tesseract_command_language/set_tool_instruction.h index 94ba1056392..3db7844c897 100644 --- a/tesseract_command_language/include/tesseract_command_language/set_tool_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/set_tool_instruction.h @@ -41,6 +41,12 @@ class SetToolInstruction SetToolInstruction() = default; // Required for boost serialization do not use SetToolInstruction(int tool_id); + 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); @@ -68,8 +74,13 @@ class SetToolInstruction bool operator!=(const SetToolInstruction& 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 Tool Instruction" }; + /** @brief The tool ID */ int tool_id_{ -1 }; friend class boost::serialization::access; diff --git a/tesseract_command_language/include/tesseract_command_language/timer_instruction.h b/tesseract_command_language/include/tesseract_command_language/timer_instruction.h index 7fe9928b52c..976e0831893 100644 --- a/tesseract_command_language/include/tesseract_command_language/timer_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/timer_instruction.h @@ -54,6 +54,12 @@ class TimerInstruction TimerInstruction() = default; // Required for boost serialization do not use TimerInstruction(TimerInstructionType type, double time, int io); + 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); @@ -110,6 +116,10 @@ class TimerInstruction bool operator!=(const TimerInstruction& 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 Timer Instruction" }; TimerInstructionType timer_type_{ TimerInstructionType::DIGITAL_OUTPUT_LOW }; diff --git a/tesseract_command_language/include/tesseract_command_language/wait_instruction.h b/tesseract_command_language/include/tesseract_command_language/wait_instruction.h index fd67108238c..d4057f0d67d 100644 --- a/tesseract_command_language/include/tesseract_command_language/wait_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/wait_instruction.h @@ -61,6 +61,12 @@ class WaitInstruction WaitInstruction(double time); WaitInstruction(WaitInstructionType type, int io); + 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); @@ -117,6 +123,10 @@ class WaitInstruction bool operator!=(const WaitInstruction& 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 Wait Instruction" }; WaitInstructionType wait_type_{ WaitInstructionType::TIME }; diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 0f1ae5264d0..e59ae672da1 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -31,6 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -55,10 +58,19 @@ bool moveFilter(const InstructionPoly& instruction, CompositeInstruction::CompositeInstruction(std::string profile, CompositeInstructionOrder order, tesseract_common::ManipulatorInfo manipulator_info) - : manipulator_info_(std::move(manipulator_info)), profile_(std::move(profile)), order_(order) + : uuid_(boost::uuids::random_generator()()) + , manipulator_info_(std::move(manipulator_info)) + , profile_(std::move(profile)) + , order_(order) { } +const boost::uuids::uuid& CompositeInstruction::getUUID() const { return uuid_; } +void CompositeInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); } + +const boost::uuids::uuid& CompositeInstruction::getParentUUID() const { return parent_uuid_; } +void CompositeInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; } + CompositeInstructionOrder CompositeInstruction::getOrder() const { return order_; } const std::string& CompositeInstruction::getDescription() const { return description_; } @@ -545,6 +557,8 @@ void CompositeInstruction::flattenHelper(std::vector void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("uuid", uuid_); + ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_); ar& boost::serialization::make_nvp("description", description_); ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_); ar& boost::serialization::make_nvp("profile", profile_); diff --git a/tesseract_command_language/src/poly/instruction_poly.cpp b/tesseract_command_language/src/poly/instruction_poly.cpp index be4ef8e0cd8..619b356e66d 100644 --- a/tesseract_command_language/src/poly/instruction_poly.cpp +++ b/tesseract_command_language/src/poly/instruction_poly.cpp @@ -15,6 +15,20 @@ void tesseract_planning::detail_instruction::InstructionInterface::serialize(Arc boost::serialization::base_object(*this)); } +const boost::uuids::uuid& tesseract_planning::InstructionPoly::getUUID() const { return getInterface().getUUID(); } + +void tesseract_planning::InstructionPoly::regenerateUUID() { getInterface().regenerateUUID(); } + +const boost::uuids::uuid& tesseract_planning::InstructionPoly::getParentUUID() const +{ + return getInterface().getParentUUID(); +} + +void tesseract_planning::InstructionPoly::setParentUUID(const boost::uuids::uuid& uuid) +{ + getInterface().setParentUUID(uuid); +} + const std::string& tesseract_planning::InstructionPoly::getDescription() const { return getInterface().getDescription(); diff --git a/tesseract_command_language/src/set_analog_instruction.cpp b/tesseract_command_language/src/set_analog_instruction.cpp index 5cd67d3bed1..1613c982206 100644 --- a/tesseract_command_language/src/set_analog_instruction.cpp +++ b/tesseract_command_language/src/set_analog_instruction.cpp @@ -28,6 +28,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,10 +39,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { SetAnalogInstruction::SetAnalogInstruction(std::string key, int index, double value) - : key_(std::move(key)), index_(index), value_(value) + : uuid_(boost::uuids::random_generator()()), key_(std::move(key)), index_(index), value_(value) { } +const boost::uuids::uuid& SetAnalogInstruction::getUUID() const { return uuid_; } +void SetAnalogInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); } + +const boost::uuids::uuid& SetAnalogInstruction::getParentUUID() const { return parent_uuid_; } +void SetAnalogInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; } + const std::string& SetAnalogInstruction::getDescription() const { return description_; } void SetAnalogInstruction::setDescription(const std::string& description) { description_ = description; } @@ -73,6 +82,8 @@ bool SetAnalogInstruction::operator!=(const SetAnalogInstruction& rhs) const { r template void SetAnalogInstruction::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("uuid", uuid_); + ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_); ar& boost::serialization::make_nvp("description", description_); ar& boost::serialization::make_nvp("key", key_); ar& boost::serialization::make_nvp("index", index_); diff --git a/tesseract_command_language/src/set_tool_instruction.cpp b/tesseract_command_language/src/set_tool_instruction.cpp index 4a616fedcb8..d3953bba8bb 100644 --- a/tesseract_command_language/src/set_tool_instruction.cpp +++ b/tesseract_command_language/src/set_tool_instruction.cpp @@ -29,6 +29,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,7 +39,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -SetToolInstruction::SetToolInstruction(int tool_id) : tool_id_(tool_id) {} +SetToolInstruction::SetToolInstruction(int tool_id) : uuid_(boost::uuids::random_generator()()), tool_id_(tool_id) {} + +const boost::uuids::uuid& SetToolInstruction::getUUID() const { return uuid_; } +void SetToolInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); } + +const boost::uuids::uuid& SetToolInstruction::getParentUUID() const { return parent_uuid_; } +void SetToolInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; } const std::string& SetToolInstruction::getDescription() const { return description_; } @@ -63,6 +72,8 @@ bool SetToolInstruction::operator!=(const SetToolInstruction& rhs) const { retur template void SetToolInstruction::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("uuid", uuid_); + ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_); ar& boost::serialization::make_nvp("description", description_); ar& boost::serialization::make_nvp("tool_id", tool_id_); } diff --git a/tesseract_command_language/src/timer_instruction.cpp b/tesseract_command_language/src/timer_instruction.cpp index 1181efc5af7..ff57bd0dff6 100644 --- a/tesseract_command_language/src/timer_instruction.cpp +++ b/tesseract_command_language/src/timer_instruction.cpp @@ -28,6 +28,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,10 +39,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { TimerInstruction::TimerInstruction(TimerInstructionType type, double time, int io) - : timer_type_(type), timer_time_(time), timer_io_(io) + : uuid_(boost::uuids::random_generator()()), timer_type_(type), timer_time_(time), timer_io_(io) { } +const boost::uuids::uuid& TimerInstruction::getUUID() const { return uuid_; } +void TimerInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); } + +const boost::uuids::uuid& TimerInstruction::getParentUUID() const { return parent_uuid_; } +void TimerInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; } + const std::string& TimerInstruction::getDescription() const { return description_; } void TimerInstruction::setDescription(const std::string& description) { description_ = description; } @@ -80,6 +89,8 @@ bool TimerInstruction::operator!=(const TimerInstruction& rhs) const { return !o template void TimerInstruction::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("uuid", uuid_); + ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_); ar& boost::serialization::make_nvp("description", description_); ar& boost::serialization::make_nvp("timer_type", timer_type_); ar& boost::serialization::make_nvp("timer_time", timer_time_); diff --git a/tesseract_command_language/src/wait_instruction.cpp b/tesseract_command_language/src/wait_instruction.cpp index 6f6dad4acb3..225029957a2 100644 --- a/tesseract_command_language/src/wait_instruction.cpp +++ b/tesseract_command_language/src/wait_instruction.cpp @@ -28,6 +28,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -42,6 +45,12 @@ WaitInstruction::WaitInstruction(WaitInstructionType type, int io) : wait_type_( throw std::runtime_error("WaitInstruction: Invalid type 'WaitInstructionType::TIME' for constructor"); } +const boost::uuids::uuid& WaitInstruction::getUUID() const { return uuid_; } +void WaitInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); } + +const boost::uuids::uuid& WaitInstruction::getParentUUID() const { return parent_uuid_; } +void WaitInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; } + const std::string& WaitInstruction::getDescription() const { return description_; } void WaitInstruction::setDescription(const std::string& description) { description_ = description; } @@ -81,6 +90,8 @@ bool WaitInstruction::operator!=(const WaitInstruction& rhs) const { return !ope template void WaitInstruction::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("uuid", uuid_); + ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_); ar& boost::serialization::make_nvp("description", description_); ar& boost::serialization::make_nvp("wait_type", wait_type_); ar& boost::serialization::make_nvp("wait_time", wait_time_); diff --git a/tesseract_task_composer/include/tesseract_task_composer/task_composer_input.h b/tesseract_task_composer/include/tesseract_task_composer/task_composer_input.h index bff16941750..0dceacd2fe2 100644 --- a/tesseract_task_composer/include/tesseract_task_composer/task_composer_input.h +++ b/tesseract_task_composer/include/tesseract_task_composer/task_composer_input.h @@ -53,7 +53,6 @@ struct TaskComposerInput TaskComposerInput(TaskComposerProblem problem, ProfileDictionary::ConstPtr profiles = nullptr); TaskComposerInput& operator=(const TaskComposerInput&) = delete; - TaskComposerInput(TaskComposerInput&&) = delete; TaskComposerInput& operator=(TaskComposerInput&&) = delete; virtual ~TaskComposerInput() = default; @@ -103,6 +102,7 @@ struct TaskComposerInput TaskComposerInput() = default; // Required for serialization TaskComposerInput(const TaskComposerInput&); + TaskComposerInput(TaskComposerInput&&) noexcept; template void serialize(Archive& ar, const unsigned int version); // NOLINT diff --git a/tesseract_task_composer/include/tesseract_task_composer/task_composer_node_info.h b/tesseract_task_composer/include/tesseract_task_composer/task_composer_node_info.h index e2b15c7d7d1..6c93f8be4c2 100644 --- a/tesseract_task_composer/include/tesseract_task_composer/task_composer_node_info.h +++ b/tesseract_task_composer/include/tesseract_task_composer/task_composer_node_info.h @@ -116,8 +116,8 @@ struct TaskComposerNodeInfoContainer ~TaskComposerNodeInfoContainer() = default; TaskComposerNodeInfoContainer(const TaskComposerNodeInfoContainer&); TaskComposerNodeInfoContainer& operator=(const TaskComposerNodeInfoContainer&); - TaskComposerNodeInfoContainer(TaskComposerNodeInfoContainer&&) = delete; - TaskComposerNodeInfoContainer& operator=(TaskComposerNodeInfoContainer&&) = delete; + TaskComposerNodeInfoContainer(TaskComposerNodeInfoContainer&&) noexcept; + TaskComposerNodeInfoContainer& operator=(TaskComposerNodeInfoContainer&&) noexcept; /** * @brief Add info to the container diff --git a/tesseract_task_composer/src/task_composer_data_storage.cpp b/tesseract_task_composer/src/task_composer_data_storage.cpp index 6e83260bd40..2024681ab0a 100644 --- a/tesseract_task_composer/src/task_composer_data_storage.cpp +++ b/tesseract_task_composer/src/task_composer_data_storage.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #endif #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include diff --git a/tesseract_task_composer/src/task_composer_input.cpp b/tesseract_task_composer/src/task_composer_input.cpp index d30ae46aa81..1d55e745960 100644 --- a/tesseract_task_composer/src/task_composer_input.cpp +++ b/tesseract_task_composer/src/task_composer_input.cpp @@ -78,6 +78,15 @@ TaskComposerInput::TaskComposerInput(const TaskComposerInput& rhs) { } +TaskComposerInput::TaskComposerInput(TaskComposerInput&& rhs) noexcept + : problem(std::move(rhs.problem)) + , profiles(std::move(rhs.profiles)) + , data_storage(std::move(rhs.data_storage)) + , task_infos(std::move(rhs.task_infos)) + , aborted_(rhs.aborted_.load()) +{ +} + template void TaskComposerInput::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_task_composer/src/task_composer_node_info.cpp b/tesseract_task_composer/src/task_composer_node_info.cpp index d0cd7171f14..4fbcd5c9f3f 100644 --- a/tesseract_task_composer/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/src/task_composer_node_info.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -86,6 +87,10 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(const TaskComposerNodeInfoContainer& other) { + std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + *this = other; } TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const TaskComposerNodeInfoContainer& other) @@ -100,6 +105,25 @@ TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const Ta return *this; } +TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(TaskComposerNodeInfoContainer&& other) noexcept +{ + std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + + *this = std::move(other); +} +TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(TaskComposerNodeInfoContainer&& other) noexcept +{ + std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(other.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + + info_map_ = std::move(other.info_map_); + + return *this; +} + void TaskComposerNodeInfoContainer::addInfo(TaskComposerNodeInfo::UPtr info) { std::unique_lock lock(mutex_); @@ -135,6 +159,10 @@ std::map TaskComposerNodeInfoCon bool TaskComposerNodeInfoContainer::operator==(const TaskComposerNodeInfoContainer& rhs) const { + std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(rhs.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + bool equal = true; auto equality = [](const TaskComposerNodeInfo::UPtr& p1, const TaskComposerNodeInfo::UPtr& p2) { return (p1 && p2 && *p1 == *p2) || (!p1 && !p2); @@ -146,12 +174,17 @@ bool TaskComposerNodeInfoContainer::operator==(const TaskComposerNodeInfoContain bool TaskComposerNodeInfoContainer::operator!=(const TaskComposerNodeInfoContainer& rhs) const { + std::shared_lock lhs_lock(mutex_, std::defer_lock); + std::shared_lock rhs_lock(rhs.mutex_, std::defer_lock); + std::scoped_lock lock{ lhs_lock, rhs_lock }; + return !operator==(rhs); } template void TaskComposerNodeInfoContainer::serialize(Archive& ar, const unsigned int /*version*/) { + std::unique_lock lock(mutex_); ar& BOOST_SERIALIZATION_NVP(info_map_); } From 7dbc8e8c246bee82c318772aea02afb13499e71b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 8 Jan 2023 21:28:21 -0600 Subject: [PATCH 2/5] Add toJointTrajectory overload for InstructionPoly --- .../include/tesseract_command_language/utils.h | 8 ++++++++ tesseract_command_language/src/utils.cpp | 12 ++++++++++++ 2 files changed, 20 insertions(+) diff --git a/tesseract_command_language/include/tesseract_command_language/utils.h b/tesseract_command_language/include/tesseract_command_language/utils.h index 2d650450b62..e5269d6ce2d 100644 --- a/tesseract_command_language/include/tesseract_command_language/utils.h +++ b/tesseract_command_language/include/tesseract_command_language/utils.h @@ -37,6 +37,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +/** + * @brief Convert instruction to a joint trajectory + * @details This searches for both move instructions. If it contains a Cartesian waypoint it is skipped. + * @param instruction The instruction to convert + * @return A joint trajectory + */ +tesseract_common::JointTrajectory toJointTrajectory(const InstructionPoly& instruction); + /** * @brief Convert composite instruction to a joint trajectory * @details This searches for both move and plan instruction to support converting both input and results to planning diff --git a/tesseract_command_language/src/utils.cpp b/tesseract_command_language/src/utils.cpp index 704414134e1..e591d4282a9 100644 --- a/tesseract_command_language/src/utils.cpp +++ b/tesseract_command_language/src/utils.cpp @@ -51,6 +51,18 @@ static const tesseract_planning::locateFilterFn toJointTrajectoryInstructionFilt return false; }; +tesseract_common::JointTrajectory toJointTrajectory(const InstructionPoly& instruction) +{ + using namespace tesseract_planning; + if (instruction.isCompositeInstruction()) + { + const auto& ci = instruction.as(); + return toJointTrajectory(ci); + } + + throw std::runtime_error("toJointTrajectory: Unsupported Instruction Type!"); +} + tesseract_common::JointTrajectory toJointTrajectory(const CompositeInstruction& composite_instructions) { tesseract_common::JointTrajectory trajectory; From e57aaf21229249ec2edb66103033d8b3e09d235b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 10 Jan 2023 18:10:51 -0600 Subject: [PATCH 3/5] Fix descartes collision edge evaluator --- .../descartes_collision_edge_evaluator.hpp | 67 ++++++++++--------- 1 file changed, 36 insertions(+), 31 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp index 446570f30ff..67e0e284d0b 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp @@ -52,11 +52,29 @@ DescartesCollisionEdgeEvaluator::DescartesCollisionEdgeEvaluator( , allow_collision_(allow_collision) , debug_(debug) { - discrete_contact_manager_->setActiveCollisionObjects(active_link_names_); - discrete_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + if (discrete_contact_manager_ != nullptr) + { + discrete_contact_manager_->setActiveCollisionObjects(active_link_names_); + discrete_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + } + else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || + collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) + { + throw std::runtime_error("Evaluator type is DISCRETE or LVS_DISCRETE, but discrete contact manager is not " + "available"); + } - continuous_contact_manager_->setActiveCollisionObjects(active_link_names_); - continuous_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + if (continuous_contact_manager_ != nullptr) + { + continuous_contact_manager_->setActiveCollisionObjects(active_link_names_); + continuous_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + } + else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || + collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) + { + throw std::runtime_error("Evaluator type is CONTINUOUS or LVS_CONTINUOUS, but continuous contact manager is not " + "available"); + } } template @@ -75,30 +93,27 @@ DescartesCollisionEdgeEvaluator::evaluate(const descartes_light::Stat segment(1, i) = end[i]; } - std::vector discrete_results; - std::vector continuous_results; - bool discrete_in_contact = discreteCollisionCheck(discrete_results, segment, allow_collision_); - bool continuous_in_contact = continuousCollisionCheck(continuous_results, segment, allow_collision_); + std::vector contact_results; + bool in_contact{ true }; + if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || + collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) + { + in_contact = continuousCollisionCheck(contact_results, segment, allow_collision_); + } + else + { + in_contact = discreteCollisionCheck(contact_results, segment, allow_collision_); + } - if (!discrete_in_contact && !continuous_in_contact) + if (!in_contact) return std::make_pair(true, 0); // TODO: Update this to consider link pairs auto collision_safety_margin_ = static_cast(collision_check_config_.contact_manager_config.margin_data.getMaxCollisionMargin()); - if (!discrete_in_contact && continuous_in_contact && allow_collision_) - return std::make_pair(true, collision_safety_margin_ - continuous_results.begin()->begin()->second[0].distance); - - if (discrete_in_contact && !continuous_in_contact && allow_collision_) - return std::make_pair(true, collision_safety_margin_ - discrete_results.begin()->begin()->second[0].distance); - - if (discrete_in_contact && continuous_in_contact && allow_collision_) - { - double d = collision_safety_margin_ - discrete_results.begin()->begin()->second[0].distance; - double c = collision_safety_margin_ - continuous_results.begin()->begin()->second[0].distance; - return std::make_pair(true, std::max(d, c)); - } + if (in_contact && allow_collision_) + return std::make_pair(true, collision_safety_margin_ - contact_results.begin()->begin()->second[0].distance); return std::make_pair(false, 0); } @@ -125,11 +140,6 @@ bool DescartesCollisionEdgeEvaluator::continuousCollisionCheck( } mutex_.unlock(); tesseract_collision::CollisionCheckConfig config = collision_check_config_; - if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE || - config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - else - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; config.contact_request.type = (find_best) ? tesseract_collision::ContactTestType::CLOSEST : tesseract_collision::ContactTestType::FIRST; @@ -159,11 +169,6 @@ bool DescartesCollisionEdgeEvaluator::discreteCollisionCheck( mutex_.unlock(); tesseract_collision::CollisionCheckConfig config = collision_check_config_; - if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE || - config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - else - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; config.contact_request.type = (find_best) ? tesseract_collision::ContactTestType::CLOSEST : tesseract_collision::ContactTestType::FIRST; From 6c006bf2f6cae03ffda122fbf528a0a969b554cf Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 12 Jan 2023 11:04:00 -0600 Subject: [PATCH 4/5] Remove composite start instruction --- .../composite_instruction.h | 138 ++++++------ .../move_instruction.h | 2 +- .../poly/move_instruction_poly.h | 3 - .../src/composite_instruction.cpp | 204 +++++++----------- .../src/poly/move_instruction_poly.cpp | 5 - tesseract_command_language/src/utils.cpp | 14 +- .../test/move_instruction_unit.cpp | 28 +-- .../test/serialize_test.cpp | 37 ++-- .../test/type_erasure_benchmark.cpp | 39 ++-- .../test/utils_test.cpp | 6 +- .../src/basic_cartesian_example.cpp | 5 +- tesseract_examples/src/car_seat_example.cpp | 10 +- .../src/freespace_hybrid_example.cpp | 5 +- .../src/freespace_ompl_example.cpp | 5 +- .../src/glass_upright_example.cpp | 5 +- .../src/pick_and_place_example.cpp | 10 +- .../puzzle_piece_auxillary_axes_example.cpp | 7 +- .../src/puzzle_piece_example.cpp | 7 +- .../simple/simple_motion_planner.h | 4 - .../core/src/core/utils.cpp | 35 --- .../core/src/simple/simple_motion_planner.cpp | 72 +++---- .../examples/chain_example.cpp | 4 +- .../examples/freespace_example.cpp | 4 +- .../examples/raster_example.cpp | 27 +-- .../ompl/src/ompl_motion_planner.cpp | 10 - .../test/descartes_planner_tests.cpp | 12 +- .../test/ompl_planner_tests.cpp | 25 +-- ...ple_planner_fixed_size_assign_position.cpp | 6 +- ...imple_planner_fixed_size_interpolation.cpp | 8 +- .../test/simple_planner_lvs_interpolation.cpp | 16 +- .../test/trajopt_planner_tests.cpp | 51 ++--- .../examples/freespace_example_program.h | 10 +- .../examples/raster_example_program.h | 30 ++- .../src/nodes/motion_planner_task.cpp | 3 - .../src/nodes/raster_ct_motion_task.cpp | 68 +++--- .../src/nodes/raster_ct_only_motion_task.cpp | 39 ++-- .../src/nodes/raster_ft_motion_task.cpp | 68 +++--- .../src/nodes/raster_ft_only_motion_task.cpp | 39 ++-- .../time_optimal_parameterization_task.cpp | 5 +- .../nodes/update_start_and_end_state_task.cpp | 3 +- .../src/nodes/update_start_state_task.cpp | 3 +- .../src/nodes/upsample_trajectory_task.cpp | 14 +- .../test/fix_state_bounds_task_unit.cpp | 5 +- .../test/tesseract_task_composer_unit.cpp | 11 - .../src/instructions_trajectory.cpp | 12 +- .../time_optimal_trajectory_generation.cpp | 22 +- .../test/iterative_spline_tests.cpp | 15 +- .../ruckig_trajectory_smoothing_tests.cpp | 10 +- ...me_optimal_trajectory_generation_tests.cpp | 5 +- 49 files changed, 483 insertions(+), 683 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index 5c04ff289b9..544d8764452 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -50,14 +50,10 @@ class CompositeInstruction; * * The filter should return true when the instruction passed should be included not throw. */ -using flattenFilterFn = - std::function; -using locateFilterFn = - std::function; +using flattenFilterFn = std::function; +using locateFilterFn = std::function; -bool moveFilter(const InstructionPoly& instruction, - const CompositeInstruction& composite, - bool parent_is_first_composite); +bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& composite); enum class CompositeInstructionOrder { @@ -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::pointer; + /** const_pointer */ + using const_pointer = typename std::vector::const_pointer; + /** reference */ + using reference = typename std::vector::reference; + /** const_reference */ + using const_reference = typename std::vector::const_reference; + /** size_type */ + using size_type = typename std::vector::size_type; + /** difference_type */ + using difference_type = typename std::vector::difference_type; + /** iterator */ + using iterator = typename std::vector::iterator; + /** const_iterator */ + using const_iterator = typename std::vector::const_iterator; + /** reverse_iterator */ + using reverse_iterator = typename std::vector::reverse_iterator; + /** const_reverse_iterator */ + using const_reverse_iterator = typename std::vector::const_reverse_iterator; + CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY, CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED, tesseract_common::ManipulatorInfo manipulator_info = tesseract_common::ManipulatorInfo()); + template + CompositeInstruction(InputIt first, InputIt last) : container_(first, last) + { + } + CompositeInstructionOrder getOrder() const; const boost::uuids::uuid& getUUID() const; @@ -100,13 +124,6 @@ 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 instructions); std::vector& getInstructions(); const std::vector& getInstructions() const; @@ -114,8 +131,8 @@ class CompositeInstruction 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 @@ -228,34 +245,6 @@ class CompositeInstruction // C++ container support - /** value_type */ - using value_type = InstructionPoly; - /** pointer */ - using pointer = typename std::vector::pointer; - /** const_pointer */ - using const_pointer = typename std::vector::const_pointer; - /** reference */ - using reference = typename std::vector::reference; - /** const_reference */ - using const_reference = typename std::vector::const_reference; - /** size_type */ - using size_type = typename std::vector::size_type; - /** difference_type */ - using difference_type = typename std::vector::difference_type; - /** iterator */ - using iterator = typename std::vector::iterator; - /** const_iterator */ - using const_iterator = typename std::vector::const_iterator; - /** reverse_iterator */ - using reverse_iterator = typename std::vector::reverse_iterator; - /** const_reverse_iterator */ - using const_reverse_iterator = typename std::vector::const_reverse_iterator; - - // template - // CompositeInstruction(InputIt first, InputIt last) : container_(first, last) - // { - // } - /////////////// // Iterators // /////////////// @@ -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 l); + template + void insert(const_iterator pos, InputIt first, InputIt last) + { + container_.insert(pos, first, last); + } + + /** @brief constructs element in-place */ + template + 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 +#if __cplusplus > 201402L + reference emplace_back(Args&&... args) + { + return container_.emplace_back(std::forward(args)...); + } +#else + void emplace_back(Args&&... args) + { + container_.emplace_back(std::forward(args)...); + } +#endif + /** @brief removes the last element */ void pop_back(); @@ -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 @@ -410,8 +418,7 @@ class CompositeInstruction */ void flattenHelper(std::vector>& flattened, CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite); + const flattenFilterFn& filter); /** * @brief Helper function used by Flatten. Not intended for direct use @@ -422,8 +429,7 @@ class CompositeInstruction */ void flattenHelper(std::vector>& flattened, const CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) const; + const flattenFilterFn& filter) const; friend class boost::serialization::access; template diff --git a/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract_command_language/include/tesseract_command_language/move_instruction.h index cfe8b286b28..11d4556368a 100644 --- a/tesseract_command_language/include/tesseract_command_language/move_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/move_instruction.h @@ -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" }; diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h index 1e73232f573..a1de2a2abf4 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h @@ -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 @@ -345,8 +344,6 @@ struct MoveInstructionPoly : MoveInstructionPolyBase bool isCircular() const; - bool isStart() const; - bool isChild() const; private: diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index e59ae672da1..a41fb84c87b 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -41,18 +41,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include /** @todo Remove after refactor is complete */ namespace tesseract_planning { -bool moveFilter(const InstructionPoly& instruction, - const CompositeInstruction& /*composite*/, - bool parent_is_first_composite) +bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& /*composite*/) { - if (instruction.isMoveInstruction()) - { - if (instruction.as().isStart()) - return (parent_is_first_composite); - - return true; - } - return false; + return instruction.isMoveInstruction(); } CompositeInstruction::CompositeInstruction(std::string profile, @@ -96,25 +87,6 @@ void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo const tesseract_common::ManipulatorInfo& CompositeInstruction::getManipulatorInfo() const { return manipulator_info_; } tesseract_common::ManipulatorInfo& CompositeInstruction::getManipulatorInfo() { return manipulator_info_; } -void CompositeInstruction::setStartInstruction(MoveInstructionPoly instruction) -{ - start_instruction_ = std::move(instruction); -} - -void CompositeInstruction::resetStartInstruction() { start_instruction_ = InstructionPoly(); } - -const MoveInstructionPoly& CompositeInstruction::getStartInstruction() const -{ - return start_instruction_.as(); -} - -MoveInstructionPoly& CompositeInstruction::getStartInstruction() -{ - return start_instruction_.as(); -} - -bool CompositeInstruction::hasStartInstruction() const { return (!start_instruction_.isNull()); } - void CompositeInstruction::setInstructions(std::vector instructions) { container_.swap(instructions); } std::vector& CompositeInstruction::getInstructions() { return container_; } @@ -125,9 +97,15 @@ void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly& mi) void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly&& mi) { container_.emplace_back(mi); } -void CompositeInstruction::appendInstruction(const InstructionPoly& i) { container_.push_back(i); } - -void CompositeInstruction::appendInstruction(const InstructionPoly&& i) { container_.push_back(i); } +CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p, + const MoveInstructionPoly& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x) +{ + return container_.insert(p, x); +} MoveInstructionPoly* CompositeInstruction::getFirstMoveInstruction() { @@ -170,36 +148,36 @@ long CompositeInstruction::getMoveInstructionCount() const { return getInstructi const InstructionPoly* CompositeInstruction::getFirstInstruction(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getFirstInstructionHelper(*this, locate_filter, process_child_composites, true); + return getFirstInstructionHelper(*this, locate_filter, process_child_composites); } InstructionPoly* CompositeInstruction::getFirstInstruction(const locateFilterFn& locate_filter, bool process_child_composites) { - return getFirstInstructionHelper(*this, locate_filter, process_child_composites, true); + return getFirstInstructionHelper(*this, locate_filter, process_child_composites); } const InstructionPoly* CompositeInstruction::getLastInstruction(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getLastInstructionHelper(*this, locate_filter, process_child_composites, true); + return getLastInstructionHelper(*this, locate_filter, process_child_composites); } InstructionPoly* CompositeInstruction::getLastInstruction(const locateFilterFn& locate_filter, bool process_child_composites) { - return getLastInstructionHelper(*this, locate_filter, process_child_composites, true); + return getLastInstructionHelper(*this, locate_filter, process_child_composites); } long CompositeInstruction::getInstructionCount(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getInstructionCountHelper(*this, locate_filter, process_child_composites, true); + return getInstructionCountHelper(*this, locate_filter, process_child_composites); } std::vector> CompositeInstruction::flatten(const flattenFilterFn& filter) { std::vector> flattened; - flattenHelper(flattened, *this, filter, true); + flattenHelper(flattened, *this, filter); return flattened; } @@ -207,17 +185,13 @@ std::vector> CompositeInstruction::flatten(const flattenFilterFn& filter) const { std::vector> flattened; - flattenHelper(flattened, *this, filter, true); + flattenHelper(flattened, *this, filter); return flattened; } void CompositeInstruction::print(const std::string& prefix) const { std::cout << prefix + "Composite Instruction, Description: " << getDescription() << std::endl; - if (!start_instruction_.isNull()) - std::cout << prefix + "--- Start Instruction, Description: " << start_instruction_.getDescription() << std::endl; - else - std::cout << prefix + "--- Start Instruction, Description: Null Instruction" << std::endl; std::cout << prefix + "{" << std::endl; for (const auto& i : container_) { @@ -235,7 +209,6 @@ bool CompositeInstruction::operator==(const CompositeInstruction& rhs) const equal &= (static_cast(order_) == static_cast(rhs.order_)); equal &= (profile_ == rhs.profile_); // NOLINT equal &= (manipulator_info_ == rhs.manipulator_info_); - equal &= (start_instruction_ == rhs.start_instruction_); equal &= (container_.size() == rhs.container_.size()); if (equal) { @@ -290,19 +263,41 @@ CompositeInstruction::const_reference CompositeInstruction::at(size_type n) cons CompositeInstruction::pointer CompositeInstruction::data() { return container_.data(); } CompositeInstruction::const_pointer CompositeInstruction::data() const { return container_.data(); } CompositeInstruction::reference CompositeInstruction::operator[](size_type pos) { return container_[pos]; } -CompositeInstruction::const_reference CompositeInstruction::operator[](size_type pos) const { return container_[pos]; }; +CompositeInstruction::const_reference CompositeInstruction::operator[](size_type pos) const { return container_[pos]; } /////////////// // Modifiers // /////////////// void CompositeInstruction::clear() { container_.clear(); } +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, const value_type& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, value_type&& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::initializer_list l) +{ + return container_.insert(p, l); +} + +template +CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args) +{ + return container_.emplace(pos, std::forward(args)...); +} + CompositeInstruction::iterator CompositeInstruction::erase(const_iterator p) { return container_.erase(p); } CompositeInstruction::iterator CompositeInstruction::erase(const_iterator first, const_iterator last) { return container_.erase(first, last); } +void CompositeInstruction::push_back(const value_type& x) { container_.push_back(x); } +void CompositeInstruction::push_back(const value_type&& x) { container_.push_back(x); } + void CompositeInstruction::pop_back() { container_.pop_back(); } void CompositeInstruction::swap(std::vector& other) { container_.swap(other); } @@ -313,25 +308,19 @@ void CompositeInstruction::swap(std::vector& other) { container_.swa const InstructionPoly* CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - if (process_child_composites) { for (const auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; if (instruction.isCompositeInstruction()) { - const InstructionPoly* result = getFirstInstructionHelper( - instruction.as(), locate_filter, process_child_composites, false); + const InstructionPoly* result = + getFirstInstructionHelper(instruction.as(), locate_filter, process_child_composites); if (result != nullptr) return result; } @@ -341,7 +330,7 @@ CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& comp } for (const auto& instruction : composite_instruction.container_) - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; return nullptr; @@ -349,25 +338,19 @@ CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& comp InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) + bool process_child_composites) { - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - if (process_child_composites) { for (auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; if (instruction.isCompositeInstruction()) { - InstructionPoly* result = getFirstInstructionHelper( - instruction.as(), locate_filter, process_child_composites, false); + InstructionPoly* result = + getFirstInstructionHelper(instruction.as(), locate_filter, process_child_composites); if (result != nullptr) return result; } @@ -377,7 +360,7 @@ InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstru } for (auto& instruction : composite_instruction.container_) - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; return nullptr; @@ -385,128 +368,95 @@ InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstru const InstructionPoly* CompositeInstruction::getLastInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { if (process_child_composites) { for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) { - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); if (it->isCompositeInstruction()) { const InstructionPoly* result = - getLastInstructionHelper(it->as(), locate_filter, process_child_composites, false); + getLastInstructionHelper(it->as(), locate_filter, process_child_composites); if (result != nullptr) return result; } } - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } InstructionPoly* CompositeInstruction::getLastInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) + bool process_child_composites) { if (process_child_composites) { for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) { - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); if (it->isCompositeInstruction()) { InstructionPoly* result = - getLastInstructionHelper(it->as(), locate_filter, process_child_composites, false); + getLastInstructionHelper(it->as(), locate_filter, process_child_composites); if (result != nullptr) return result; } } - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } long CompositeInstruction::getInstructionCountHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { long cnt = 0; - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - ++cnt; if (process_child_composites) { for (const auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) ++cnt; if (instruction.isCompositeInstruction()) - cnt += getInstructionCountHelper( - instruction.as(), locate_filter, process_child_composites, false); + cnt += + getInstructionCountHelper(instruction.as(), locate_filter, process_child_composites); } return cnt; } - cnt += std::count_if( - composite_instruction.container_.begin(), composite_instruction.container_.end(), [=](const auto& i) { - return (!locate_filter || locate_filter(i, composite_instruction, first_composite)); - }); + cnt += std::count_if(composite_instruction.container_.begin(), + composite_instruction.container_.end(), + [=](const auto& i) { return (!locate_filter || locate_filter(i, composite_instruction)); }); return cnt; } void CompositeInstruction::flattenHelper(std::vector>& flattened, CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) + const flattenFilterFn& filter) { - if (composite.hasStartInstruction()) - if (!filter || filter(composite.start_instruction_, composite, first_composite)) - flattened.emplace_back(composite.start_instruction_); - for (auto& i : composite.container_) { if (i.isCompositeInstruction()) @@ -514,12 +464,12 @@ void CompositeInstruction::flattenHelper(std::vector(), filter, false); + flattenHelper(flattened, i.as(), filter); } - else if (!filter || (filter && filter(i, composite, first_composite))) + else if (!filter || (filter && filter(i, composite))) { flattened.emplace_back(i); } @@ -528,13 +478,8 @@ void CompositeInstruction::flattenHelper(std::vector>& flattened, const CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) const + const flattenFilterFn& filter) const { - if (composite.hasStartInstruction()) - if (!filter || filter(composite.start_instruction_, composite, first_composite)) - flattened.emplace_back(composite.start_instruction_); - for (const auto& i : composite.container_) { if (i.isCompositeInstruction()) @@ -542,12 +487,12 @@ void CompositeInstruction::flattenHelper(std::vector(), filter, false); + flattenHelper(flattened, i.as(), filter); } - else if (!filter || filter(i, composite, first_composite)) + else if (!filter || filter(i, composite)) { flattened.emplace_back(i); } @@ -563,7 +508,6 @@ void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/ ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_); ar& boost::serialization::make_nvp("profile", profile_); ar& boost::serialization::make_nvp("order", order_); - ar& boost::serialization::make_nvp("start_instruction", start_instruction_); ar& boost::serialization::make_nvp("container", container_); } diff --git a/tesseract_command_language/src/poly/move_instruction_poly.cpp b/tesseract_command_language/src/poly/move_instruction_poly.cpp index 1d818930c63..b11cd0addda 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -154,11 +154,6 @@ bool tesseract_planning::MoveInstructionPoly::isCircular() const return (getInterface().getMoveType() == MoveInstructionType::CIRCULAR); } -bool tesseract_planning::MoveInstructionPoly::isStart() const -{ - return (getInterface().getMoveType() == MoveInstructionType::START); -} - bool tesseract_planning::MoveInstructionPoly::isChild() const { return (!getInterface().getParentUUID().is_nil()); } template diff --git a/tesseract_command_language/src/utils.cpp b/tesseract_command_language/src/utils.cpp index e591d4282a9..8cb341f6503 100644 --- a/tesseract_command_language/src/utils.cpp +++ b/tesseract_command_language/src/utils.cpp @@ -37,18 +37,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { static const tesseract_planning::locateFilterFn toJointTrajectoryInstructionFilter = - [](const tesseract_planning::InstructionPoly& i, - const tesseract_planning::CompositeInstruction& /*composite*/, - bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; + [](const tesseract_planning::InstructionPoly& i, const tesseract_planning::CompositeInstruction& /*composite*/) { + return i.isMoveInstruction(); }; tesseract_common::JointTrajectory toJointTrajectory(const InstructionPoly& instruction) diff --git a/tesseract_command_language/test/move_instruction_unit.cpp b/tesseract_command_language/test/move_instruction_unit.cpp index 567d017085a..967fec8762e 100644 --- a/tesseract_command_language/test/move_instruction_unit.cpp +++ b/tesseract_command_language/test/move_instruction_unit.cpp @@ -45,11 +45,11 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // Minimum arguments { - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); - EXPECT_TRUE(instr.getPathProfile().empty()); + EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -79,11 +79,11 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // With plan profile { - MoveInstruction instr(swp, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR, "TEST_PROFILE"); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); - EXPECT_TRUE(instr.getPathProfile().empty()); + EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE"); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -113,9 +113,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // With plan and path profile { - MoveInstruction instr(swp, MoveInstructionType::START, "TEST_PROFILE", "TEST_PATH_PROFILE"); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR, "TEST_PROFILE", "TEST_PATH_PROFILE"); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); EXPECT_FALSE(instr.getDescription().empty()); @@ -152,9 +152,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, setters) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_TRUE(instr.getPathProfile().empty()); EXPECT_FALSE(instr.getDescription().empty()); @@ -192,9 +192,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, UUID) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_TRUE(instr.getPathProfile().empty()); EXPECT_FALSE(instr.getDescription().empty()); @@ -219,7 +219,7 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, boostSerialization) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); instr.setMoveType(MoveInstructionType::LINEAR); instr.setProfile("TEST_PROFILE"); instr.setPathProfile("TEST_PATH_PROFILE"); @@ -245,7 +245,7 @@ TEST(TesseractCommandLanguageMoveInstructionPolyUnit, boostSerialization) // NO std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); instr.setMoveType(MoveInstructionType::LINEAR); instr.setProfile("TEST_PROFILE"); instr.setPathProfile("TEST_PATH_PROFILE"); diff --git a/tesseract_command_language/test/serialize_test.cpp b/tesseract_command_language/test/serialize_test.cpp index dfdf1512357..310fceede1b 100644 --- a/tesseract_command_language/test/serialize_test.cpp +++ b/tesseract_command_language/test/serialize_test.cpp @@ -55,8 +55,8 @@ CompositeInstruction getProgram() // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp0{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -85,8 +85,9 @@ CompositeInstruction getProgram() plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -97,7 +98,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -112,9 +113,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -126,7 +127,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -141,9 +142,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -155,7 +156,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -163,26 +164,26 @@ CompositeInstruction getProgram() CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Add a wait instruction WaitInstruction wait_instruction_time(1.5); - program.appendInstruction(wait_instruction_time); + program.push_back(wait_instruction_time); WaitInstruction wait_instruction_io(WaitInstructionType::DIGITAL_INPUT_LOW, 10); - program.appendInstruction(wait_instruction_io); + program.push_back(wait_instruction_io); // Add a timer instruction TimerInstruction timer_instruction(TimerInstructionType::DIGITAL_OUTPUT_LOW, 3.1, 5); - program.appendInstruction(timer_instruction); + program.push_back(timer_instruction); // Add a set tool instruction SetToolInstruction set_tool_instruction(5); - program.appendInstruction(set_tool_instruction); + program.push_back(set_tool_instruction); // Add a set tool instruction SetAnalogInstruction set_analog_instruction("R", 0, 1.5); - program.appendInstruction(set_analog_instruction); + program.push_back(set_analog_instruction); return program; } diff --git a/tesseract_command_language/test/type_erasure_benchmark.cpp b/tesseract_command_language/test/type_erasure_benchmark.cpp index 8d99079254f..0f4568ae206 100644 --- a/tesseract_command_language/test/type_erasure_benchmark.cpp +++ b/tesseract_command_language/test/type_erasure_benchmark.cpp @@ -57,8 +57,8 @@ CompositeInstruction getProgram() // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp0{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -87,8 +87,9 @@ CompositeInstruction getProgram() plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -99,7 +100,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -114,9 +115,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -128,7 +129,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -143,9 +144,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -157,7 +158,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -165,26 +166,26 @@ CompositeInstruction getProgram() CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Add a wait instruction WaitInstruction wait_instruction_time(1.5); - program.appendInstruction(wait_instruction_time); + program.push_back(wait_instruction_time); WaitInstruction wait_instruction_io(WaitInstructionType::DIGITAL_INPUT_LOW, 10); - program.appendInstruction(wait_instruction_io); + program.push_back(wait_instruction_io); // Add a timer instruction TimerInstruction timer_instruction(TimerInstructionType::DIGITAL_OUTPUT_LOW, 3.1, 5); - program.appendInstruction(timer_instruction); + program.push_back(timer_instruction); // Add a set tool instruction SetToolInstruction set_tool_instruction(5); - program.appendInstruction(set_tool_instruction); + program.push_back(set_tool_instruction); // Add a set tool instruction SetAnalogInstruction set_analog_instruction("R", 0, 1.5); - program.appendInstruction(set_analog_instruction); + program.push_back(set_analog_instruction); return program; } @@ -236,7 +237,7 @@ static void BM_MoveInstructionCreation(benchmark::State& state) { CartesianWaypointPoly w{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; for (auto _ : state) - MoveInstruction i(w, MoveInstructionType::START); + MoveInstruction i(w, MoveInstructionType::FREESPACE); } BENCHMARK(BM_MoveInstructionCreation); diff --git a/tesseract_command_language/test/utils_test.cpp b/tesseract_command_language/test/utils_test.cpp index 67a5a7522c7..296a07fc73c 100644 --- a/tesseract_command_language/test/utils_test.cpp +++ b/tesseract_command_language/test/utils_test.cpp @@ -63,9 +63,9 @@ TEST(TesseractCommandLanguageUtilsUnit, flatten) // NOLINT std::to_string(k)); sub_sub_composite.appendMoveInstruction(instruction); } - sub_composite.appendInstruction(sub_sub_composite); + sub_composite.push_back(sub_sub_composite); } - composite.appendInstruction(sub_composite); + composite.push_back(sub_composite); } // flatten(composite); @@ -112,7 +112,7 @@ TEST(TesseractCommandLanguageUtilsUnit, flatten) // NOLINT composite.print(); // flatten the composite keeping the composite instructions - flattenFilterFn filter = [](const InstructionPoly&, const CompositeInstruction&, bool) { return true; }; + flattenFilterFn filter = [](const InstructionPoly&, const CompositeInstruction&) { return true; }; std::vector> flattened = composite.flatten(filter); EXPECT_EQ(flattened.size(), i_max * j_max * k_max + 16); // Add 16 for the composite instructions diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index c7675c2f0b6..b271c919811 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -157,8 +157,8 @@ bool BasicCartesianExample::run() // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Create cartesian waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.5, -0.2, 0.62) * @@ -179,6 +179,7 @@ bool BasicCartesianExample::run() plan_f1.setDescription("to_end_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); program.appendMoveInstruction(plan_c0); program.appendMoveInstruction(plan_f1); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 87b2736f5a7..16f5b0fe7c6 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -278,14 +278,15 @@ bool CarSeatExample::run() StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) }; // Start Joint Position for the program - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics @@ -362,14 +363,15 @@ bool CarSeatExample::run() StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) }; // Start Joint Position for the program - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 7d6908d6327..52de24e0638 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -137,14 +137,15 @@ bool FreespaceHybridExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 0b7aebeb65b..446a3d13e26 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -137,14 +137,15 @@ bool FreespaceOMPLExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index a1caa91dc40..03483a27e60 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -151,8 +151,8 @@ bool GlassUprightExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::LINEAR, "UPRIGHT"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start // Assign a linear motion so cartesian is defined as the target @@ -160,6 +160,7 @@ bool GlassUprightExample::run() plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 3d4306f3795..330d0fa9eb7 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -152,8 +152,8 @@ bool PickAndPlaceExample::run() ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); StateWaypointPoly pick_swp{ StateWaypoint(joint_names, joint_pos) }; - MoveInstruction start_instruction(pick_swp, MoveInstructionType::START); - pick_program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(pick_swp, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Define the final pose (on top of the box) Eigen::Isometry3d pick_final_pose; @@ -176,6 +176,7 @@ bool PickAndPlaceExample::run() pick_plan_a1.setDescription("Pick Approach"); // Add Instructions to program + pick_program.appendMoveInstruction(start_instruction); pick_program.appendMoveInstruction(pick_plan_a0); pick_program.appendMoveInstruction(pick_plan_a1); @@ -305,10 +306,6 @@ bool PickAndPlaceExample::run() CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); - MoveInstructionPoly place_start_instruction(*pick_final_state); - place_start_instruction.setMoveType(MoveInstructionType::START); - place_program.setStartInstruction(place_start_instruction); - // Define the approach pose CartesianWaypointPoly place_wp0{ CartesianWaypoint(retreat_pose) }; @@ -331,6 +328,7 @@ bool PickAndPlaceExample::run() place_plan_a2.setDescription("Place approach"); // Add Instructions to program + place_program.appendMoveInstruction(*pick_final_state); place_program.appendMoveInstruction(place_plan_a0); place_program.appendMoveInstruction(place_plan_a1); place_program.appendMoveInstruction(place_plan_a2); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index d18c21af983..fc3101ad88c 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -172,12 +172,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); // Create cartesian waypoint - CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[0]) }; - MoveInstruction plan_instruction(wp, MoveInstructionType::START, "CARTESIAN"); - plan_instruction.setDescription("from_start_plan"); - program.setStartInstruction(plan_instruction); - - for (std::size_t i = 1; i < tool_poses.size(); ++i) + for (std::size_t i = 0; i < tool_poses.size(); ++i) { CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[i]) }; MoveInstruction plan_instruction(wp, MoveInstructionType::LINEAR, "CARTESIAN"); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 6b2dccaef82..941bac6c22f 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -167,12 +167,7 @@ bool PuzzlePieceExample::run() CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); // Create cartesian waypoint - CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[0]) }; - MoveInstruction plan_instruction(wp, MoveInstructionType::START, "CARTESIAN"); - plan_instruction.setDescription("from_start_plan"); - program.setStartInstruction(plan_instruction); - - for (std::size_t i = 1; i < tool_poses.size(); ++i) + for (std::size_t i = 0; i < tool_poses.size(); ++i) { CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[i]) }; MoveInstruction plan_instruction(wp, MoveInstructionType::LINEAR, "CARTESIAN"); diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h index 9b31e7120a4..36a5a26bbe9 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -69,10 +69,6 @@ class SimpleMotionPlanner : public MotionPlanner MotionPlanner::Ptr clone() const override; protected: - static MoveInstructionPoly getStartInstruction(const PlannerRequest& request, - const tesseract_scene_graph::SceneState& current_state, - const tesseract_kinematics::JointGroup& manip); - CompositeInstruction processCompositeInstruction(const CompositeInstruction& instructions, MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, diff --git a/tesseract_motion_planners/core/src/core/utils.cpp b/tesseract_motion_planners/core/src/core/utils.cpp index 1a5f128fbe7..78a3c444a4e 100644 --- a/tesseract_motion_planners/core/src/core/utils.cpp +++ b/tesseract_motion_planners/core/src/core/utils.cpp @@ -164,9 +164,6 @@ tesseract_common::Toolpath toToolpath(const MoveInstructionPoly& mi, const tesse void assignCurrentStateAsSeed(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env) { - if (!composite_instructions.hasStartInstruction()) - throw std::runtime_error("Top most composite instruction is missing start instruction!"); - std::unordered_map manip_joint_state; tesseract_scene_graph::SceneState state = env.getState(); const tesseract_common::ManipulatorInfo& global_mi = composite_instructions.getManipulatorInfo(); @@ -240,42 +237,10 @@ bool formatProgramHelper(CompositeInstruction& composite_instructions, bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env) { - if (!composite_instructions.hasStartInstruction()) - throw std::runtime_error("Top most composite instruction is missing start instruction!"); - std::unordered_map> manip_joint_names; bool format_required = false; tesseract_common::ManipulatorInfo mi = composite_instructions.getManipulatorInfo(); - std::unordered_map manipulators; - - if (composite_instructions.hasStartInstruction()) - { - auto& pi = composite_instructions.getStartInstruction(); - - tesseract_common::ManipulatorInfo start_mi = mi.getCombined(pi.getManipulatorInfo()); - - std::vector joint_names; - auto it = manip_joint_names.find(start_mi.manipulator); - if (it == manip_joint_names.end()) - { - joint_names = env.getGroupJointNames(start_mi.manipulator); - manip_joint_names[start_mi.manipulator] = joint_names; - } - else - { - joint_names = it->second; - } - - if (pi.getWaypoint().isStateWaypoint() || pi.getWaypoint().isJointWaypoint()) - { - if (formatJointPosition(joint_names, pi.getWaypoint())) - format_required = true; - } - } - else - throw std::runtime_error("Top most composite instruction start instruction has invalid waypoint type!"); - if (formatProgramHelper(composite_instructions, env, mi, manip_joint_names)) format_required = true; diff --git a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp index 821404db2a6..a894e202a61 100644 --- a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp +++ b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp @@ -77,18 +77,14 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const // Create seed CompositeInstruction seed; - // Get the start waypoint/instruction - MoveInstructionPoly start_instruction = getStartInstruction(request, request.env_state, *manip); - - // Set start instruction - MoveInstructionPoly start_instruction_seed = start_instruction; - start_instruction_seed.setMoveType(MoveInstructionType::START); + // Place holder for start instruction + MoveInstructionPoly null_instruction; // Process the instructions into the seed try { - MoveInstructionPoly start_instruction_copy = start_instruction; - MoveInstructionPoly start_instruction_seed_copy = start_instruction_seed; + MoveInstructionPoly start_instruction_copy = null_instruction; + MoveInstructionPoly start_instruction_seed_copy = null_instruction; seed = processCompositeInstruction(request.instructions, start_instruction_copy, start_instruction_seed_copy, request); } @@ -100,9 +96,6 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } - // Set seed start state - seed.setStartInstruction(start_instruction_seed); - // Fill out the response response.results = seed; @@ -135,31 +128,6 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } -MoveInstructionPoly SimpleMotionPlanner::getStartInstruction(const PlannerRequest& request, - const tesseract_scene_graph::SceneState& current_state, - const tesseract_kinematics::JointGroup& manip) -{ - // Get start instruction - MoveInstructionPoly start_instruction = request.instructions.getStartInstruction(); - assert(start_instruction.isStart()); - auto& start_waypoint = start_instruction.getWaypoint(); - if (start_waypoint.isJointWaypoint() || start_waypoint.isStateWaypoint()) - { - assert(checkJointPositionFormat(manip.getJointNames(), start_waypoint)); - return start_instruction; - } - - if (start_waypoint.isCartesianWaypoint()) - { - /** @todo Update to run IK to find solution closest to start */ - start_waypoint.as().setSeed( - tesseract_common::JointState(manip.getJointNames(), current_state.getJointValues(manip.getJointNames()))); - return start_instruction; - } - - throw std::runtime_error("Unsupported waypoint type!"); -} - CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const CompositeInstruction& instructions, MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, @@ -174,12 +142,38 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (instruction.isCompositeInstruction()) { - seed.appendInstruction( + seed.push_back( processCompositeInstruction(instruction.as(), prev_instruction, prev_seed, request)); } else if (instruction.isMoveInstruction()) { const auto& base_instruction = instruction.as(); + if (prev_instruction.isNull()) + { + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; + tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + + prev_instruction = base_instruction; + auto& start_waypoint = prev_instruction.getWaypoint(); + if (start_waypoint.isJointWaypoint() || start_waypoint.isStateWaypoint()) + { + assert(checkJointPositionFormat(manip->getJointNames(), start_waypoint)); + } + else if (start_waypoint.isCartesianWaypoint()) + { + /** @todo Update to run IK to find solution closest to start */ + start_waypoint.as().setSeed(tesseract_common::JointState( + manip->getJointNames(), request.env_state.getJointValues(manip->getJointNames()))); + } + else + { + throw std::runtime_error("Unsupported waypoint type!"); + } + prev_seed = prev_instruction; + seed.push_back(prev_instruction); + continue; + } // Get the next plan instruction if it exists InstructionPoly next_instruction; @@ -222,14 +216,14 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp request.instructions.getManipulatorInfo()); for (const auto& instr : instruction_seed) - seed.appendInstruction(instr); + seed.push_back(instr); prev_instruction = base_instruction; prev_seed = instruction_seed.back(); } else { - seed.appendInstruction(instruction); + seed.push_back(instruction); } } // for (const auto& instruction : instructions) return seed; diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index 14dfbac6784..a334068cce8 100644 --- a/tesseract_motion_planners/examples/chain_example.cpp +++ b/tesseract_motion_planners/examples/chain_example.cpp @@ -111,7 +111,7 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Plan Instructions - MoveInstruction start_instruction(wp0, MoveInstructionType::START); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_c1(wp2, MoveInstructionType::LINEAR, "DEFAULT"); MoveInstruction plan_c2(wp3, MoveInstructionType::LINEAR, "DEFAULT"); @@ -122,8 +122,8 @@ int main(int /*argc*/, char** /*argv*/) // Create program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); program.appendMoveInstruction(plan_c1); program.appendMoveInstruction(plan_c2); diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 3ad79107278..fa47e8e4694 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -94,13 +94,13 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Plan Instructions - MoveInstruction start_instruction(wp0, MoveInstructionType::START); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "DEFAULT"); // Create program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Plot Program diff --git a/tesseract_motion_planners/examples/raster_example.cpp b/tesseract_motion_planners/examples/raster_example.cpp index d802d3dff2d..f2999dbc589 100644 --- a/tesseract_motion_planners/examples/raster_example.cpp +++ b/tesseract_motion_planners/examples/raster_example.cpp @@ -92,8 +92,8 @@ int main(int /*argc*/, char** /*argv*/) // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(kin_group->getJointNames(), Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE); + start_instruction.setDescription("Start"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -123,8 +123,9 @@ int main(int /*argc*/, char** /*argv*/) plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -135,7 +136,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -150,9 +151,9 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -164,7 +165,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -179,9 +180,9 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -193,7 +194,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -201,7 +202,7 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Plot Program auto state_solver = env->getStateSolver(); diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index b858fdf51de..e76cf679eed 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -248,16 +248,6 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const bool found{ false }; Eigen::Index row{ 0 }; - if (start_index == 0) - { - MoveInstructionPoly& mi = response.results.getStartInstruction(); - if (mi.getUUID() == pc.start_uuid) - { - assignSolution(mi, joint_names, traj.row(row++), request.format_result_as_input); - found = true; - } - } - auto& ci = response.results.getInstructions(); for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) { diff --git a/tesseract_motion_planners/test/descartes_planner_tests.cpp b/tesseract_motion_planners/test/descartes_planner_tests.cpp index 2062db24095..c0785857b75 100644 --- a/tesseract_motion_planners/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/test/descartes_planner_tests.cpp @@ -99,15 +99,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -211,15 +211,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -314,15 +314,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 260beb09d78..c7ce701a0c7 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -161,7 +161,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT Eigen::Map(end_state.data(), static_cast(end_state.size()))) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); @@ -169,8 +169,8 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); program.appendMoveInstruction(plan_f2); @@ -211,9 +211,8 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 21); // 10 per segment + start a instruction - EXPECT_EQ(planner_response.results.size(), 20); + EXPECT_EQ(planner_response.results.size(), 21); EXPECT_TRUE(wp1.getPosition().isApprox( getJointPosition(planner_response.results.getFirstMoveInstruction()->getWaypoint()), 1e-5)); @@ -237,12 +236,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT wp1.setPosition(Eigen::Map(swp.data(), static_cast(swp.size()))); wp1.setNames(joint_group->getJointNames()); - start_instruction = MoveInstruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + start_instruction = MoveInstruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a new program program = CompositeInstruction(); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a new seed @@ -267,15 +266,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT wp2.setNames(joint_group->getJointNames()); // Define Start Instruction - start_instruction = MoveInstruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + start_instruction = MoveInstruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions plan_f1 = MoveInstruction(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a new program program = CompositeInstruction(); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a new seed @@ -326,15 +325,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -373,7 +372,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT CONSOLE_BRIDGE_logError("CI Error: %s", planner_response.message.c_str()); } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 11); EXPECT_TRUE(wp1.getPosition().isApprox( getJointPosition(planner_response.results.getFirstMoveInstruction()->getWaypoint()), 1e-5)); @@ -420,15 +418,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT Eigen::Map(end_state.data(), static_cast(end_state.size()))) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -468,7 +466,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 11); EXPECT_TRUE(wp2.getPosition().isApprox( getJointPosition(planner_response.results.getLastMoveInstruction()->getWaypoint()), 1e-5)); diff --git a/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp index d7d01de3c47..93162603c98 100644 --- a/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp @@ -69,7 +69,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -111,7 +111,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint request.env = env_; request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -152,7 +152,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte request.env = env_; request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp index 244d11a04d0..47853570df1 100644 --- a/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp @@ -69,7 +69,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -111,7 +111,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -158,7 +158,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -201,7 +201,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp index 7fd88aa4a7e..5d4a0af16af 100644 --- a/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp @@ -70,7 +70,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -128,7 +128,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -197,7 +197,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -254,7 +254,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo auto joint_group = request.env->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -325,7 +325,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -382,7 +382,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -451,7 +451,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -510,7 +510,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/test/trajopt_planner_tests.cpp index 5e1bfed2722..05e03259462 100644 --- a/tesseract_motion_planners/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/test/trajopt_planner_tests.cpp @@ -117,15 +117,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N wp2.getPosition() << 0, 0, 0, 1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -192,15 +192,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT wp2.getPosition() << 0, 0, 0, 1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -272,15 +272,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -355,7 +355,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT wp2.getPosition() << 0, 0, 0, -1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -363,8 +363,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -438,7 +438,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -447,8 +447,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -522,7 +522,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -531,8 +531,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -613,18 +613,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Specify a Joint Waypoint as the finish JointWaypointPoly wp{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; wp.getPosition() << 0, 0, 0, -1.57 + ind * 0.1, 0, 0, 0; - if (ind == 0) - { - // Define Start Instruction - MoveInstruction start_instruction(wp, MoveInstructionType::START, "TEST_PROFILE"); - program.setStartInstruction(start_instruction); - } - else - { - wp.setNames(joint_names); - MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); - program.appendMoveInstruction(plan_f); - } + MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); + program.appendMoveInstruction(plan_f); } // Create a seed @@ -683,17 +673,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT // Specify a Joint Waypoint as the finish JointWaypointPoly wp{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; wp.getPosition() << 0, 0, 0, -1.57 + ind * 0.1, 0, 0, 0; - if (ind == 0) - { - // Define Start Instruction - MoveInstruction start_instruction(wp, MoveInstructionType::START, "TEST_PROFILE"); - program.setStartInstruction(start_instruction); - } - else - { - MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); - program.appendMoveInstruction(plan_f); - } + MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); + program.appendMoveInstruction(plan_f); } // Create a seed diff --git a/tesseract_task_composer/examples/freespace_example_program.h b/tesseract_task_composer/examples/freespace_example_program.h index a275b3e3879..9d61ba02318 100644 --- a/tesseract_task_composer/examples/freespace_example_program.h +++ b/tesseract_task_composer/examples/freespace_example_program.h @@ -27,13 +27,14 @@ inline CompositeInstruction freespaceExampleProgramIIWA( std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7" }; StateWaypointPoly wp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start Instruction"); // Define target pose CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE, freespace_profile); plan_f0.setDescription("freespace_motion"); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); JointWaypointPoly wp3{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -54,13 +55,14 @@ inline CompositeInstruction freespaceExampleProgramABB( // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start Instruction"); // Define target pose CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE, freespace_profile); plan_f0.setDescription("freespace_motion"); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); JointWaypointPoly wp3{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; diff --git a/tesseract_task_composer/examples/raster_example_program.h b/tesseract_task_composer/examples/raster_example_program.h index 0535e21aabf..8a800a837e3 100644 --- a/tesseract_task_composer/examples/raster_example_program.h +++ b/tesseract_task_composer/examples/raster_example_program.h @@ -47,8 +47,8 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly swp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(swp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(swp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start"); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -58,8 +58,9 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); // for (int i = 0; i < 4; ++i) @@ -100,7 +101,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); } - program.appendInstruction(raster_segment); + program.push_back(raster_segment); // Add transition if (i == 0 || i == 2) @@ -115,7 +116,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } else if (i == 1) { @@ -129,7 +130,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } } @@ -138,7 +139,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction to_end(freespace_profile); to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); return program; } @@ -149,14 +150,6 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction program( DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); - CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * - Eigen::Quaterniond(0, 0, -1.0, 0)); - - // Define start instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - start_instruction.setDescription("Start Instruction"); - program.setStartInstruction(start_instruction); - for (int i = 0; i < 4; ++i) { double x = 0.8 + (i * 0.1); @@ -179,6 +172,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { + raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp3, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp4, MoveInstructionType::LINEAR, process_profile)); @@ -195,7 +189,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); } - program.appendInstruction(raster_segment); + program.push_back(raster_segment); // Add transition if (i == 0 || i == 2) @@ -210,7 +204,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } else if (i == 1) { @@ -224,7 +218,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } } diff --git a/tesseract_task_composer/src/nodes/motion_planner_task.cpp b/tesseract_task_composer/src/nodes/motion_planner_task.cpp index 5d027ed2c0a..76b14474409 100644 --- a/tesseract_task_composer/src/nodes/motion_planner_task.cpp +++ b/tesseract_task_composer/src/nodes/motion_planner_task.cpp @@ -81,9 +81,6 @@ TaskComposerNodeInfo::UPtr MotionPlannerTask::runImpl(TaskComposerInput& input, assert(!(input.problem.manip_info.empty() && instructions.getManipulatorInfo().empty())); instructions.setManipulatorInfo(instructions.getManipulatorInfo().getCombined(input.problem.manip_info)); - // It should always have a start instruction which required by the motion planners - assert(instructions.hasStartInstruction()); - // -------------------- // Fill out request // -------------------- diff --git a/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp index 242fbcf5605..e1cbeb89952 100644 --- a/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp @@ -109,29 +109,12 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 1) - { - const InstructionPoly& from_start_input_instruction = program[0]; - assert(from_start_input_instruction.isCompositeInstruction()); - const auto& ci = from_start_input_instruction.as(); - const auto* li = ci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - else - { - const InstructionPoly& pre_input_instruction = program[idx - 1]; - assert(pre_input_instruction.isCompositeInstruction()); - const auto& tci = pre_input_instruction.as(); - const auto* li = tci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + raster_input.insertMoveInstruction(raster_input.begin(), *li); auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); @@ -157,6 +140,14 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -181,8 +172,6 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, // Plan from_start - preceded by the first raster auto from_start_input = program[0].as(); - - from_start_input.setStartInstruction(program.getStartInstruction()); from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info)); auto from_start_pipeline_task = @@ -205,6 +194,14 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[program.size() - 2]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + to_end_input.insertMoveInstruction(to_end_input.begin(), *li); + auto to_end_pipeline_task = std::make_unique("To End: " + to_end_input.getDescription()); std::string to_end_pipeline_key = to_end_pipeline_task->getUUIDString(); auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_pipeline_task)); @@ -237,14 +234,23 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, } program.clear(); - program.appendInstruction(input.data_storage.getData(from_start_pipeline_key).as()); + program.emplace_back(input.data_storage.getData(from_start_pipeline_key).as()); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + segment.erase(segment.begin()); + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } - program.appendInstruction(input.data_storage.getData(to_end_pipeline_key).as()); + CompositeInstruction to_end = input.data_storage.getData(to_end_pipeline_key).as(); + to_end.erase(to_end.begin()); + program.emplace_back(to_end); input.data_storage.setData(output_keys_[0], program); @@ -267,10 +273,6 @@ void RasterCtMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input) const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterCtMotionTask, input should have a start instruction"); - // Check from_start if (!composite.at(0).isCompositeInstruction()) throw std::runtime_error("RasterCtMotionTask, from_start should be a composite"); diff --git a/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp index 1609fb3a39d..1600a84dbb7 100644 --- a/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp @@ -107,26 +107,17 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); - // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 0) - { - start_instruction = program.getStartInstruction(); - } - else + // Insert Start Plan Instruction + if (idx > 0) { const InstructionPoly& pre_input_instruction = program[idx - 1]; assert(pre_input_instruction.isCompositeInstruction()); const auto& tci = pre_input_instruction.as(); const auto* li = tci.getLastMoveInstruction(); assert(li != nullptr); - start_instruction = *li; + raster_input.insertMoveInstruction(raster_input.begin(), *li); } - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); - auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); std::string raster_pipeline_key = raster_pipeline_task->getUUIDString(); @@ -151,6 +142,13 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -193,9 +191,18 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in program.clear(); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + if (i != 0) + segment.erase(segment.begin()); + + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } input.data_storage.setData(output_keys_[0], program); @@ -219,10 +226,6 @@ void RasterCtOnlyMotionTask::checkTaskInput(const tesseract_common::AnyPoly& inp const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterCtOnlyMotionTask, input should have a start instruction"); - // Check rasters and transitions for (const auto& i : composite) { diff --git a/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp index 6b39340d0f9..c03f93432a7 100644 --- a/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp @@ -109,29 +109,12 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 1) - { - const InstructionPoly& from_start_input_instruction = program[0]; - assert(from_start_input_instruction.isCompositeInstruction()); - const auto& ci = from_start_input_instruction.as(); - const auto* li = ci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - else - { - const InstructionPoly& pre_input_instruction = program[idx - 1]; - assert(pre_input_instruction.isCompositeInstruction()); - const auto& tci = pre_input_instruction.as(); - const auto* li = tci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + raster_input.insertMoveInstruction(raster_input.begin(), *li); auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); @@ -157,6 +140,14 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -181,8 +172,6 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, // Plan from_start - preceded by the first raster auto from_start_input = program[0].as(); - - from_start_input.setStartInstruction(program.getStartInstruction()); from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info)); auto from_start_pipeline_task = @@ -205,6 +194,14 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[program.size() - 2]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + to_end_input.insertMoveInstruction(to_end_input.begin(), *li); + auto to_end_pipeline_task = std::make_unique("To End: " + to_end_input.getDescription()); std::string to_end_pipeline_key = to_end_pipeline_task->getUUIDString(); auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_pipeline_task)); @@ -237,14 +234,23 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, } program.clear(); - program.appendInstruction(input.data_storage.getData(from_start_pipeline_key).as()); + program.emplace_back(input.data_storage.getData(from_start_pipeline_key).as()); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + segment.erase(segment.begin()); + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } - program.appendInstruction(input.data_storage.getData(to_end_pipeline_key).as()); + CompositeInstruction to_end = input.data_storage.getData(to_end_pipeline_key).as(); + to_end.erase(to_end.begin()); + program.emplace_back(to_end); input.data_storage.setData(output_keys_[0], program); @@ -267,10 +273,6 @@ void RasterFtMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input) const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterFtMotionTask, input should have a start instruction"); - // Check from_start if (!composite.at(0).isCompositeInstruction()) throw std::runtime_error("RasterFtMotionTask, from_start should be a composite"); diff --git a/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp index 7f5c2b46bda..7ffb17ffbf5 100644 --- a/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp @@ -108,26 +108,17 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); - // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 0) - { - start_instruction = program.getStartInstruction(); - } - else + // Insert Start Plan Instruction + if (idx > 0) { const InstructionPoly& pre_input_instruction = program[idx - 1]; assert(pre_input_instruction.isCompositeInstruction()); const auto& tci = pre_input_instruction.as(); const auto* li = tci.getLastMoveInstruction(); assert(li != nullptr); - start_instruction = *li; + raster_input.insertMoveInstruction(raster_input.begin(), *li); } - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); - auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); std::string raster_pipeline_key = raster_pipeline_task->getUUIDString(); @@ -152,6 +143,13 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -194,9 +192,18 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in program.clear(); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + if (i != 0) + segment.erase(segment.begin()); + + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } input.data_storage.setData(output_keys_[0], program); @@ -220,10 +227,6 @@ void RasterFtOnlyMotionTask::checkTaskInput(const tesseract_common::AnyPoly& inp const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterFtOnlyMotionTask, input should have a start instruction"); - // Check rasters and transitions for (const auto& i : composite) { diff --git a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp index c73d69b0aeb..2897b0d2aab 100644 --- a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp @@ -166,7 +166,6 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer RescaleTimings(unflattened, scaling_factors); } - ci.setStartInstruction(unflattened.getStartInstruction()); for (std::size_t idx = 0; idx < ci.size(); idx++) ci[idx] = unflattened[idx]; } @@ -176,7 +175,6 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer CONSOLE_BRIDGE_logWarn("TOTG Move Profile specified but unflatten is not set in the composite profile. Move " "Profile will be ignored"); - ci.setStartInstruction(resampled.getStartInstruction()); for (std::size_t idx = 0; idx < ci.size(); idx++) ci[idx] = resampled[idx]; } @@ -194,7 +192,6 @@ CompositeInstruction TimeOptimalParameterizationTask::unflatten(const CompositeI double tolerance) { CompositeInstruction unflattened(pattern); - unflattened.setStartInstruction(flattened_input.getStartInstruction()); for (auto& instr : unflattened) instr.as().clear(); @@ -242,7 +239,7 @@ CompositeInstruction TimeOptimalParameterizationTask::unflatten(const CompositeI } // Add flattened point to the subcomposite - unflattened[original_idx].as().appendInstruction(flattened_input.at(resample_idx)); + unflattened[original_idx].as().push_back(flattened_input.at(resample_idx)); // Correct the meta information, taking information from the last element of each composite in the original if (unflattened[original_idx].as().back().isMoveInstruction()) diff --git a/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp b/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp index dd8a545bb4a..fc8b51c800f 100644 --- a/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp +++ b/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp @@ -118,8 +118,7 @@ TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerInput const auto* next_start_move = input_next_data_poly.as().getFirstMoveInstruction(); // Update start instruction - instructions.setStartInstruction(*prev_last_move); - instructions.getStartInstruction().setMoveType(MoveInstructionType::START); + instructions.at(0) = (*prev_last_move); // Update end instruction if (next_start_move->getWaypoint().isCartesianWaypoint()) diff --git a/tesseract_task_composer/src/nodes/update_start_state_task.cpp b/tesseract_task_composer/src/nodes/update_start_state_task.cpp index ada18f45d81..c3296fdbba5 100644 --- a/tesseract_task_composer/src/nodes/update_start_state_task.cpp +++ b/tesseract_task_composer/src/nodes/update_start_state_task.cpp @@ -101,8 +101,7 @@ TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerInput& inpu const auto* prev_last_move = input_prev_data_poly.as().getLastMoveInstruction(); // Update start instruction - instructions.setStartInstruction(*prev_last_move); - instructions.getStartInstruction().setMoveType(MoveInstructionType::START); + instructions.at(0) = (*prev_last_move); // Store results input.data_storage.setData(output_keys_[0], input_data_poly); diff --git a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp index cb4b605c74f..f6a239bdd35 100644 --- a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp @@ -85,7 +85,7 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& in cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); assert(cur_composite_profile->longest_valid_segment_length > 0); - InstructionPoly start_instruction = ci.getStartInstruction(); + InstructionPoly start_instruction; CompositeInstruction new_results{ ci }; new_results.clear(); @@ -113,10 +113,16 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, new_cc.clear(); upsample(new_cc, cc, start_instruction, longest_valid_segment_length); - composite.appendInstruction(new_cc); + composite.push_back(new_cc); } else if (i.isMoveInstruction()) { + if (start_instruction.isNull()) + { + start_instruction = i.as(); + continue; + } + assert(start_instruction.isMoveInstruction()); const auto& mi0 = start_instruction.as(); const auto& mi1 = i.as(); @@ -145,7 +151,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, } else { - composite.appendInstruction(i); + composite.push_back(i); } start_instruction = i; @@ -153,7 +159,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, else { assert(!i.isMoveInstruction()); - composite.appendInstruction(i); + composite.push_back(i); } } } diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 2511324700e..115d980d4a3 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -49,8 +49,9 @@ CompositeInstruction createProgram(const Eigen::VectorXd& start_state, std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; JointWaypointPoly wp1{ JointWaypoint(joint_names, start_state) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE); + start_instruction.setDescription("Start Instruction"); + program.appendMoveInstruction(start_instruction); JointWaypointPoly wp2{ JointWaypoint(joint_names, start_state + ((goal_state - start_state) / 2)) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE); diff --git a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp index 3884c1f390b..40f3dbc4cfe 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp @@ -75,7 +75,6 @@ TEST_F(TesseractTaskComposerUnit, MinLengthTaskTest) // NOLINT EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Define the Process Input @@ -129,7 +128,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlanPr EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -157,7 +155,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlanPr // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -170,7 +167,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -197,7 +193,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(98, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -210,7 +205,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanPro EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -237,7 +231,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanPro // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(83, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -247,7 +240,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerFixedSizeAssignPla EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); auto interpolator = std::make_shared(); @@ -273,7 +265,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerFixedSizeAssignPla // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -283,7 +274,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerDefaultLVSPlanProf EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -308,7 +298,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerDefaultLVSPlanProf // The first plan instruction is the start instruction and every other plan instruction should be converted into // 32 move instruction. EXPECT_EQ(37, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } diff --git a/tesseract_time_parameterization/src/instructions_trajectory.cpp b/tesseract_time_parameterization/src/instructions_trajectory.cpp index b2cc657c343..49f7445479b 100644 --- a/tesseract_time_parameterization/src/instructions_trajectory.cpp +++ b/tesseract_time_parameterization/src/instructions_trajectory.cpp @@ -31,17 +31,7 @@ namespace tesseract_planning { static const flattenFilterFn programFlattenMoveInstructionFilter = - [](const InstructionPoly& i, const CompositeInstruction& /*composite*/, bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; - }; + [](const InstructionPoly& i, const CompositeInstruction& /*composite*/) { return i.isMoveInstruction(); }; InstructionsTrajectory::InstructionsTrajectory(std::vector> trajectory) : trajectory_(std::move(trajectory)) diff --git a/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp b/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp index 20d98f9659e..a92a6c262c3 100644 --- a/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp +++ b/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp @@ -55,17 +55,7 @@ constexpr double EPS = 0.000001; namespace tesseract_planning { static const flattenFilterFn programFlattenMoveInstructionFilter = - [](const InstructionPoly& i, const CompositeInstruction& /*composite*/, bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; - }; + [](const InstructionPoly& i, const CompositeInstruction& /*composite*/) { return i.isMoveInstruction(); }; TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(double path_tolerance, double resample_dt, @@ -199,16 +189,6 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(CompositeInstruction& pr CompositeInstruction new_program(program); new_program.clear(); - if (new_program.hasStartInstruction()) - { - if (new_program.getStartInstruction().getWaypoint().isStateWaypoint()) - { - auto& waypoint = new_program.getStartInstruction().getWaypoint().as(); - waypoint.setVelocity(Eigen::VectorXd::Zero(num_joints)); - waypoint.setAcceleration(Eigen::VectorXd::Zero(num_joints)); - } - } - /// @todo Figure out how to maintain the original seed structure of subcomposites for (size_t sample = 0; sample <= sample_count; ++sample) { diff --git a/tesseract_time_parameterization/test/iterative_spline_tests.cpp b/tesseract_time_parameterization/test/iterative_spline_tests.cpp index 79a63ec5e16..e6218d0b158 100644 --- a/tesseract_time_parameterization/test/iterative_spline_tests.cpp +++ b/tesseract_time_parameterization/test/iterative_spline_tests.cpp @@ -58,10 +58,7 @@ CompositeInstruction createRepeatedPointTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = 1; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } return program; @@ -80,10 +77,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset @@ -129,11 +123,10 @@ TEST(TestTimeParameterization, TestIterativeSplineDynamicParams) // NOLINT max_velocity << 2.088, 2.082, 3.27, 3.6, 3.3, 3.078; Eigen::VectorXd max_acceleration(6); max_acceleration << 1, 1, 1, 1, 1, 1; - Eigen::VectorXd max_velocity_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size() + 1)); + Eigen::VectorXd max_velocity_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size())); // +1 for start instruction - Eigen::VectorXd max_acceleration_scaling_factors = - Eigen::VectorXd::Ones(static_cast(program.size() + 1)); + Eigen::VectorXd max_acceleration_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size())); TrajectoryContainer::Ptr trajectory = std::make_shared(program); EXPECT_TRUE(time_parameterization.compute( diff --git a/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp index 1f09789baf1..af4d80bde34 100644 --- a/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp +++ b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp @@ -27,10 +27,7 @@ CompositeInstruction createRepeatedPointTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = 1; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } return program; @@ -49,10 +46,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset diff --git a/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp b/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp index 8d5395f0612..bc1e56cb648 100644 --- a/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp +++ b/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp @@ -327,10 +327,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset From fafbd46a5b746a52e429637a4e87fa8d8e5307fb Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 4 Nov 2022 16:51:14 -0500 Subject: [PATCH 5/5] Profile Dictionary Update (#255) --- .../composite_instruction.h | 10 +- .../move_instruction.h | 18 ++-- .../poly/move_instruction_poly.h | 42 ++++---- .../profile_dictionary.h | 83 +++++++++------- .../tesseract_command_language/types.h | 10 ++ .../src/composite_instruction.cpp | 12 ++- .../src/move_instruction.cpp | 24 +++-- .../src/poly/move_instruction_poly.cpp | 17 ++-- .../test/move_instruction_unit.cpp | 34 +++++++ .../src/basic_cartesian_example.cpp | 4 + tesseract_examples/src/car_seat_example.cpp | 22 +++-- .../src/freespace_hybrid_example.cpp | 10 +- .../src/freespace_ompl_example.cpp | 9 +- .../src/glass_upright_example.cpp | 18 ++-- .../src/glass_upright_ompl_example.cpp | 5 - .../src/pick_and_place_example.cpp | 4 + .../puzzle_piece_auxillary_axes_example.cpp | 4 + .../src/puzzle_piece_example.cpp | 4 + .../tesseract_motion_planners/core/types.h | 22 +---- .../tesseract_motion_planners/planner_utils.h | 87 ---------------- .../core/src/simple/simple_motion_planner.cpp | 13 +-- .../impl/descartes_motion_planner.hpp | 10 +- .../examples/chain_example.cpp | 3 + .../examples/freespace_example.cpp | 3 + .../examples/raster_example.cpp | 35 ++++--- .../ompl/src/ompl_motion_planner.cpp | 8 +- tesseract_motion_planners/test/CMakeLists.txt | 25 ----- .../test/profile_dictionary_tests.cpp | 46 ++++----- .../test/trajopt_planner_tests.cpp | 17 ++++ tesseract_motion_planners/test/utils_test.cpp | 99 ------------------- .../trajopt/src/trajopt_motion_planner.cpp | 29 ++---- .../src/trajopt_ifopt_motion_planner.cpp | 29 ++---- .../examples/task_composer_raster_example.cpp | 3 + .../task_composer_trajopt_example.cpp | 3 + .../task_composer_problem.h | 25 ----- .../task_composer_utils.h | 11 ++- .../src/nodes/check_input_task.cpp | 21 ++-- .../nodes/continuous_contact_check_task.cpp | 6 +- .../src/nodes/discrete_contact_check_task.cpp | 6 +- .../src/nodes/fix_state_bounds_task.cpp | 7 +- .../src/nodes/fix_state_collision_task.cpp | 6 +- ...iterative_spline_parameterization_task.cpp | 21 ++-- .../src/nodes/min_length_task.cpp | 6 +- .../src/nodes/motion_planner_task.cpp | 2 - .../src/nodes/profile_switch_task.cpp | 6 +- .../ruckig_trajectory_smoothing_task.cpp | 21 ++-- .../time_optimal_parameterization_task.cpp | 21 ++-- .../src/nodes/upsample_trajectory_task.cpp | 6 +- .../src/task_composer_problem.cpp | 32 ------ .../src/task_composer_utils.cpp | 70 ++++++++++++- .../test/tesseract_task_composer_unit.cpp | 92 +++++++++-------- 51 files changed, 476 insertions(+), 645 deletions(-) delete mode 100644 tesseract_motion_planners/test/utils_test.cpp diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index 544d8764452..66a5abdc15d 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include namespace tesseract_planning @@ -115,10 +115,10 @@ class CompositeInstruction void print(const std::string& prefix = "") const; void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; void setManipulatorInfo(tesseract_common::ManipulatorInfo info); const tesseract_common::ManipulatorInfo& getManipulatorInfo() const; @@ -384,7 +384,7 @@ class CompositeInstruction std::string profile_{ DEFAULT_PROFILE_KEY }; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr profile_overrides_; + ProfileOverrides profile_overrides_; /** @brief The order of the composite instruction */ CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED }; diff --git a/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract_command_language/include/tesseract_command_language/move_instruction.h index 11d4556368a..72c2f1db5c0 100644 --- a/tesseract_command_language/include/tesseract_command_language/move_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/move_instruction.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include namespace tesseract_planning @@ -127,16 +127,16 @@ class MoveInstruction tesseract_common::ManipulatorInfo& getManipulatorInfo(); void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; void setPathProfile(const std::string& profile); - const std::string& getPathProfile() const; + const std::string& getPathProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getPathProfileOverrides() const; const std::string& getDescription() const; @@ -171,10 +171,10 @@ class MoveInstruction std::string path_profile_; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr profile_overrides_; + ProfileOverrides profile_overrides_; /** @brief Dictionary of path profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr path_profile_overrides_; + ProfileOverrides path_profile_overrides_; /** @brief The assigned waypoint (Cartesian, Joint or State) */ WaypointPoly waypoint_; diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h index a1de2a2abf4..069a1cbe722 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h @@ -40,7 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -131,11 +131,11 @@ struct MoveInstructionConcept // NOLINT const std::string& path_profile_const = c.getPathProfile(); UNUSED(path_profile_const); - c.setProfileOverrides(nullptr); + c.setProfileOverrides(ProfileOverrides{}); auto profile_overrides = c.getProfileOverrides(); UNUSED(profile_overrides); - c.setPathProfileOverrides(nullptr); + c.setPathProfileOverrides(ProfileOverrides{}); auto path_profile_overrides = c.getPathProfileOverrides(); UNUSED(path_profile_overrides); @@ -183,16 +183,16 @@ struct MoveInstructionInterface : tesseract_common::TypeErasureInterface virtual tesseract_common::ManipulatorInfo& getManipulatorInfo() = 0; virtual void setProfile(const std::string& profile) = 0; - virtual const std::string& getProfile() const = 0; + virtual const std::string& getProfile(const std::string& ns = "") const = 0; virtual void setPathProfile(const std::string& profile) = 0; - virtual const std::string& getPathProfile() const = 0; + virtual const std::string& getPathProfile(const std::string& ns = "") const = 0; - virtual void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0; - virtual ProfileDictionary::ConstPtr getProfileOverrides() const = 0; + virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual ProfileOverrides getProfileOverrides() const = 0; - virtual void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0; - virtual ProfileDictionary::ConstPtr getPathProfileOverrides() const = 0; + virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual ProfileOverrides getPathProfileOverrides() const = 0; virtual void setMoveType(MoveInstructionType move_type) = 0; virtual MoveInstructionType getMoveType() const = 0; @@ -246,22 +246,22 @@ struct MoveInstructionInstance : tesseract_common::TypeErasureInstanceget().getManipulatorInfo(); } void setProfile(const std::string& profile) final { this->get().setProfile(profile); } - const std::string& getProfile() const final { return this->get().getProfile(); } + const std::string& getProfile(const std::string& ns = "") const final { return this->get().getProfile(ns); } void setPathProfile(const std::string& profile) final { this->get().setPathProfile(profile); } - const std::string& getPathProfile() const final { return this->get().getPathProfile(); } + const std::string& getPathProfile(const std::string& ns = "") const final { return this->get().getPathProfile(ns); } - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final + void setProfileOverrides(ProfileOverrides profile_overrides) final { this->get().setProfileOverrides(profile_overrides); } - ProfileDictionary::ConstPtr getProfileOverrides() const final { return this->get().getProfileOverrides(); } + ProfileOverrides getProfileOverrides() const final { return this->get().getProfileOverrides(); } - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final + void setPathProfileOverrides(ProfileOverrides profile_overrides) final { this->get().setPathProfileOverrides(profile_overrides); } - ProfileDictionary::ConstPtr getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); } + ProfileOverrides getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); } void setMoveType(MoveInstructionType move_type) final { this->get().setMoveType(move_type); } MoveInstructionType getMoveType() const final { return this->get().getMoveType(); } @@ -311,16 +311,16 @@ struct MoveInstructionPoly : MoveInstructionPolyBase tesseract_common::ManipulatorInfo& getManipulatorInfo(); void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; void setPathProfile(const std::string& profile); - const std::string& getPathProfile() const; + const std::string& getPathProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getPathProfileOverrides() const; void setMoveType(MoveInstructionType move_type); MoveInstructionType getMoveType() const; diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index 9e04f04f581..86c8cf5ac44 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -35,19 +35,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -/** - * This used to store specific profile mapping with the request - * - * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles - * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to - * say Raster2 for the specific planner Descartes by Map>. - */ -using ProfileRemapping = std::unordered_map>; - /** * @brief This class is used to store profiles for motion planning and process planning * @details This is a thread safe class @@ -156,31 +149,6 @@ class ProfileDictionary } } - /** - * @brief Check if a profile exists - * @details If profile entry does not exist it also returns false - * @return True if profile exists, otherwise false - */ - template - bool hasProfile(const std::string& ns, const std::string& profile_name) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return false; - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - { - const auto& profile_map = - std::any_cast>&>(it2->second); - auto it3 = profile_map.find(profile_name); - if (it3 != profile_map.end()) - return true; - } - return false; - } - /** * @brief Get a profile by name * @details Check if the profile exist before calling this function, if missing an exception is thrown @@ -191,11 +159,50 @@ class ProfileDictionary std::shared_ptr getProfile(const std::string& ns, const std::string& profile_name) const { std::shared_lock lock(mutex_); - const auto& it = profiles_.at(ns); - const auto& it2 = it.at(std::type_index(typeid(ProfileType))); + + const std::unordered_map* it = nullptr; + try + { + it = &(profiles_.at(ns)); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "Failed to find entries for namespace '" << ns << "'. Available namespaces are: "; + for (auto it = profiles_.begin(); it != profiles_.end(); ++it) + { + ss << "'" << it->first << "', "; + } + std::throw_with_nested(std::runtime_error(ss.str())); + } + + const std::any* it2 = nullptr; + try + { + it2 = &(it->at(std::type_index(typeid(ProfileType)))); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "No entries for profile base class type '" << boost::core::demangle(typeid(ProfileType).name()) << "'"; + std::throw_with_nested(std::runtime_error(ss.str())); + } + const auto& profile_map = - std::any_cast>&>(it2); - return profile_map.at(profile_name); + std::any_cast>&>(*it2); + try + { + return profile_map.at(profile_name); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "No entries for profile '" << profile_name << "' in namespace '" << ns << "'. Available profiles are: "; + for (auto it = profile_map.begin(); it != profile_map.end(); ++it) + ss << "'" << it->first << "', "; + + std::throw_with_nested(std::runtime_error(ss.str())); + } } /** diff --git a/tesseract_command_language/include/tesseract_command_language/types.h b/tesseract_command_language/include/tesseract_command_language/types.h index 1d362720db3..a253361ff06 100644 --- a/tesseract_command_language/include/tesseract_command_language/types.h +++ b/tesseract_command_language/include/tesseract_command_language/types.h @@ -31,6 +31,16 @@ namespace tesseract_planning { using ManipulatorInfo = tesseract_common::ManipulatorInfo; + +/** + * @brief Map that associates for override profile names (values) with specified task namespaces (keys) + * @details For example, a profile "default" might have the following override profiles names for various specific task + * namespaces + * - ["ompl", "custom_profile_1"] + * - ["time_parameterization", "custom_profile_2"] + */ +using ProfileOverrides = std::unordered_map; + } // namespace tesseract_planning #endif // TESSERACT_COMMAND_LANGUAGE_TYPES_H diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index a41fb84c87b..f8e6d5a4cc0 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -72,13 +72,19 @@ void CompositeInstruction::setProfile(const std::string& profile) { profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile; } -const std::string& CompositeInstruction::getProfile() const { return profile_; } +const std::string& CompositeInstruction::getProfile(const std::string& ns) const +{ + if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) + return profile_; + + return profile_overrides_.at(ns); +} -void CompositeInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr CompositeInstruction::getProfileOverrides() const { return profile_overrides_; } +ProfileOverrides CompositeInstruction::getProfileOverrides() const { return profile_overrides_; } void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo info) { diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp index b694cc03e15..b8920563963 100644 --- a/tesseract_command_language/src/move_instruction.cpp +++ b/tesseract_command_language/src/move_instruction.cpp @@ -148,22 +148,34 @@ const tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() c tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() { return manipulator_info_; } void MoveInstruction::setProfile(const std::string& profile) { profile_ = profile; } -const std::string& MoveInstruction::getProfile() const { return profile_; } +const std::string& MoveInstruction::getProfile(const std::string& ns) const +{ + if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) + return profile_; + + return profile_overrides_.at(ns); +} void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; } -const std::string& MoveInstruction::getPathProfile() const { return path_profile_; } +const std::string& MoveInstruction::getPathProfile(const std::string& ns) const +{ + if (ns.empty() || (path_profile_overrides_.find(ns) == path_profile_overrides_.end())) + return path_profile_; + + return path_profile_overrides_.at(ns); +} -void MoveInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr MoveInstruction::getProfileOverrides() const { return profile_overrides_; } +ProfileOverrides MoveInstruction::getProfileOverrides() const { return profile_overrides_; } -void MoveInstruction::setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides) { path_profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; } +ProfileOverrides MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; } const std::string& MoveInstruction::getDescription() const { return description_; } diff --git a/tesseract_command_language/src/poly/move_instruction_poly.cpp b/tesseract_command_language/src/poly/move_instruction_poly.cpp index b11cd0addda..59d29e22b90 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -66,33 +66,36 @@ void tesseract_planning::MoveInstructionPoly::setProfile(const std::string& prof { getInterface().setProfile(profile); } -const std::string& tesseract_planning::MoveInstructionPoly::getProfile() const { return getInterface().getProfile(); } +const std::string& tesseract_planning::MoveInstructionPoly::getProfile(const std::string& ns) const +{ + return getInterface().getProfile(ns); +} void tesseract_planning::MoveInstructionPoly::setPathProfile(const std::string& profile) { getInterface().setPathProfile(profile); } -const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile() const +const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile(const std::string& ns) const { - return getInterface().getPathProfile(); + return getInterface().getPathProfile(ns); } void tesseract_planning::MoveInstructionPoly::setProfileOverrides( - tesseract_planning::ProfileDictionary::ConstPtr profile_overrides) + tesseract_planning::ProfileOverrides profile_overrides) { getInterface().setProfileOverrides(std::move(profile_overrides)); } -tesseract_planning::ProfileDictionary::ConstPtr tesseract_planning::MoveInstructionPoly::getProfileOverrides() const +tesseract_planning::ProfileOverrides tesseract_planning::MoveInstructionPoly::getProfileOverrides() const { return getInterface().getProfileOverrides(); } void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides( - tesseract_planning::ProfileDictionary::ConstPtr profile_overrides) + tesseract_planning::ProfileOverrides profile_overrides) { getInterface().setPathProfileOverrides(std::move(profile_overrides)); } -tesseract_planning::ProfileDictionary::ConstPtr tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const +tesseract_planning::ProfileOverrides tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const { return getInterface().getPathProfileOverrides(); } diff --git a/tesseract_command_language/test/move_instruction_unit.cpp b/tesseract_command_language/test/move_instruction_unit.cpp index 967fec8762e..ca11d150d1e 100644 --- a/tesseract_command_language/test/move_instruction_unit.cpp +++ b/tesseract_command_language/test/move_instruction_unit.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_planning; @@ -290,6 +291,39 @@ TEST(TesseractCommandLanguageMoveInstructionPolyUnit, boostSerialization) // NO EXPECT_FALSE(child_ninstr.getParentUUID().is_nil()); } +TEST(TesseractCommandLanguageMoveInstructionPolyUnit, ProfileOverrides) // NOLINT +{ + Eigen::VectorXd jv = Eigen::VectorXd::Ones(6); + std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; + StateWaypointPoly swp(StateWaypoint(jn, jv)); + MoveInstruction instr(swp, MoveInstructionType::START, DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY); + + // Create arbitrary profile overrides under arbitrary namespaces + const std::string ns1 = "ns1"; + const std::string ns1_profile = "profile1"; + const std::string ns2 = "ns2"; + const std::string ns2_profile = "profile2"; + { + ProfileOverrides overrides; + overrides[ns1] = ns1_profile; + overrides[ns2] = ns2_profile; + instr.setProfileOverrides(overrides); + instr.setPathProfileOverrides(overrides); + } + + // Profile Overrides + EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); + EXPECT_EQ(instr.getProfile(ns1), ns1_profile); + EXPECT_EQ(instr.getProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getProfile("nonexistent_ns"), DEFAULT_PROFILE_KEY); + + // Path Profile Overrides + EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); + EXPECT_EQ(instr.getPathProfile(ns1), ns1_profile); + EXPECT_EQ(instr.getPathProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getPathProfile("nonexistent_ns"), DEFAULT_PROFILE_KEY); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index b271c919811..f272de63024 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -51,6 +51,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -194,6 +195,9 @@ bool BasicCartesianExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "RASTER", "freespace_profile", "cartesian_program" }); + addDefaultTaskComposerProfiles(*profiles, { "RASTER", "freespace_profile", "cartesian_program" }); + if (ifopt_) { auto composite_profile = std::make_shared(); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 16f5b0fe7c6..b25cf42b971 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -258,17 +259,21 @@ bool CarSeatExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + profiles->addProfile( - profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_composite_profile); profiles->addProfile( - profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_solver_profile); // Solve Trajectory 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(DEFAULT_PROFILE_KEY, + CompositeInstructionOrder::ORDERED, + ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Pick up the first seat!"); // Start and End Joint Position for the program @@ -282,7 +287,7 @@ bool CarSeatExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program @@ -352,8 +357,9 @@ bool CarSeatExample::run() return false; { // Create Program to place first seat - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "end_effector")); + CompositeInstruction program(DEFAULT_PROFILE_KEY, + CompositeInstructionOrder::ORDERED, + ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Place the first seat!"); // Start and End Joint Position for the program @@ -367,7 +373,7 @@ bool CarSeatExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 52de24e0638..4bad9696e8d 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -131,7 +132,7 @@ bool FreespaceHybridExample::run() // Create Program CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -141,7 +142,7 @@ bool FreespaceHybridExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -165,7 +166,10 @@ bool FreespaceHybridExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(profile_ns::OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + + profiles->addProfile(profile_ns::OMPL_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, ompl_profile); // Create Task Input Data TaskComposerDataStorage input_data; diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 446a3d13e26..423f26f1b17 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -131,7 +132,7 @@ bool FreespaceOMPLExample::run() // Create Program CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -141,7 +142,7 @@ bool FreespaceOMPLExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -165,7 +166,9 @@ bool FreespaceOMPLExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(profile_ns::OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + profiles->addProfile(profile_ns::OMPL_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, ompl_profile); // Create Task Input Data TaskComposerDataStorage input_data; diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 03483a27e60..a1585acfc25 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -41,6 +41,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -145,7 +146,7 @@ bool GlassUprightExample::run() // Create Program CompositeInstruction program( - "UPRIGHT", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -156,7 +157,7 @@ bool GlassUprightExample::run() // Plan freespace from start // Assign a linear motion so cartesian is defined as the target - MoveInstruction plan_f0(wp1, MoveInstructionType::LINEAR, "UPRIGHT"); + MoveInstruction plan_f0(wp1, MoveInstructionType::LINEAR); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -171,6 +172,9 @@ bool GlassUprightExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + if (ifopt_) { auto composite_profile = std::make_shared(); @@ -186,7 +190,7 @@ bool GlassUprightExample::run() composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); profiles->addProfile( - profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, composite_profile); auto plan_profile = std::make_shared(); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); @@ -194,7 +198,8 @@ bool GlassUprightExample::run() plan_profile->cartesian_coeff(0) = 0; plan_profile->cartesian_coeff(1) = 0; plan_profile->cartesian_coeff(2) = 0; - profiles->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile( + profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, plan_profile); } else { @@ -213,7 +218,8 @@ bool GlassUprightExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); @@ -222,7 +228,7 @@ bool GlassUprightExample::run() plan_profile->cartesian_coeff(2) = 0; // Add profile to Dictionary - profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, plan_profile); } // Create Task Input Data diff --git a/tesseract_examples/src/glass_upright_ompl_example.cpp b/tesseract_examples/src/glass_upright_ompl_example.cpp index 2395de26d2d..a07d780196e 100644 --- a/tesseract_examples/src/glass_upright_ompl_example.cpp +++ b/tesseract_examples/src/glass_upright_ompl_example.cpp @@ -40,11 +40,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include -#include -#include -#include using namespace trajopt; using namespace tesseract; diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 330d0fa9eb7..1715b76e09d 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -207,6 +208,9 @@ bool PickAndPlaceExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "DEFAULT", "CARTESIAN", "FREESPACE" }); + addDefaultTaskComposerProfiles(*profiles, { "DEFAULT", "CARTESIAN", "FREESPACE" }); + profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); profiles->addProfile( profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index fc3101ad88c..697bbc9e8d1 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -45,6 +45,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -214,6 +215,9 @@ bool PuzzlePieceAuxillaryAxesExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "CARTESIAN", "DEFAULT" }); + addDefaultTaskComposerProfiles(*profiles, { "CARTESIAN", "DEFAULT" }); + profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); profiles->addProfile( profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 941bac6c22f..2f690dca803 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -45,6 +45,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -204,6 +205,9 @@ bool PuzzlePieceExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "DEFAULT", "CARTESIAN" }); + addDefaultTaskComposerProfiles(*profiles, { "DEFAULT", "CARTESIAN" }); + profiles->addProfile(profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); profiles->addProfile( profile_ns::TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h index 2d1934e2f65..46d08885c7d 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h @@ -30,18 +30,10 @@ #include #include #include +#include namespace tesseract_planning { -/** - * This used to store planner specific profile mapping with the request - * - * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles - * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to - * say Raster2 for the specific planner Descartes by Map>. - */ -using PlannerProfileRemapping = std::unordered_map>; - struct PlannerRequest { // LCOV_EXCL_START @@ -66,18 +58,6 @@ struct PlannerRequest */ CompositeInstruction instructions; - /** - * @brief This allows the remapping of the Plan Profile identified in the command language to a specific profile for a - * given motion planner. - */ - PlannerProfileRemapping plan_profile_remapping{}; - - /** - * @brief This allows the remapping of the Composite Profile identified in the command language to a specific profile - * for a given motion planner. - */ - PlannerProfileRemapping composite_profile_remapping{}; - /** @brief Indicate if output should be verbose */ bool verbose{ false }; diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h index 0ff12e5a1a4..88aaee9def4 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h @@ -92,93 +92,6 @@ bool isValidState(const tesseract_kinematics::ForwardKinematics::ConstPtr& robot return !(robot_config != RobotConfig::FUT && robot_config != RobotConfig::NUT); } -/** - * @brief Get the profile string taking into account defaults and profile remapping - * @param ns Used to look up if there are remappings available - * @param profile The requested profile name in the instructions - * @param profile_remapping Remapping used to remap a profile name based on the planner name - * @param default_profile Default = DEFAULT. This is set if profile.empty() - * @return The profile string taking into account defaults and profile remapping - */ -inline std::string getProfileString(const std::string& ns, - const std::string& profile, - const PlannerProfileRemapping& profile_remapping, - std::string default_profile = DEFAULT_PROFILE_KEY) -{ - std::string results = profile; - if (profile.empty()) - results = std::move(default_profile); - - // Check for remapping of profile - auto remap = profile_remapping.find(ns); - if (remap != profile_remapping.end()) - { - auto p = remap->second.find(profile); - if (p != remap->second.end()) - results = p->second; - } - return results; -} - -/** - * @brief Gets the profile specified from the profile map - * @param ns The namespace to search for requested profile - * @param profile The requested profile - * @param profile_map map that contains the profiles - * @param default_profile Profile that is returned if the requested profile is not found in the map. Default = nullptr - * @return The profile requested if found. Otherwise the default_profile - */ -template -std::shared_ptr getProfile(const std::string& ns, - const std::string& profile, - const ProfileDictionary& profile_dictionary, - std::shared_ptr default_profile = nullptr) -{ - if (profile_dictionary.hasProfile(ns, profile)) - return profile_dictionary.getProfile(ns, profile); - - CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. " - "Available " - "profiles:", - profile.c_str(), - ns.c_str(), - typeid(ProfileType).name()); - - if (profile_dictionary.hasProfileEntry(ns)) - { - for (const auto& pair : profile_dictionary.getProfileEntry(ns)) - { - CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str()); - } - } - - return default_profile; -} - -/** - * @brief Returns either nominal_profile or override based on task name passed in - * @param ns The namespace to search for requested profile - * @param profile The name used to look up if there is a profile override - * @param nominal_profile Profile that will be used if no override is present - * @param overrides Dictionary of profile overrides that will override the nominal profile if present for this task. - * Default = nullptr - * @return The nominal_profile or override based on the task name passed in - */ -template -std::shared_ptr applyProfileOverrides(const std::string& ns, - const std::string& profile, - const std::shared_ptr& nominal_profile, - const ProfileDictionary::ConstPtr& overrides = nullptr) -{ - if (!overrides) - return nominal_profile; - - if (overrides->hasProfile(ns, profile)) - return overrides->getProfile(ns, profile); - - return nominal_profile; -} - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_PLANNER_UTILS_H diff --git a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp index a894e202a61..da02432603a 100644 --- a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp +++ b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp @@ -190,18 +190,13 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp SimplePlannerPlanProfile::ConstPtr plan_profile; if (base_instruction.getPathProfile().empty()) { - std::string profile = getProfileString(name_, base_instruction.getProfile(), request.plan_profile_remapping); - plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + plan_profile = + request.profiles->getProfile(name_, base_instruction.getProfile(name_)); } else { - std::string profile = - getProfileString(name_, base_instruction.getPathProfile(), request.plan_profile_remapping); - plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + plan_profile = + request.profiles->getProfile(name_, base_instruction.getPathProfile(name_)); } if (!plan_profile) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index df76a9cf6ef..2475a1493cf 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -281,14 +281,8 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) throw std::runtime_error("Descartes, working_frame is empty!"); // Get Plan Profile - std::string profile = plan_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - auto cur_plan_profile = getProfile>( - name_, profile, *request.profiles, std::make_shared>()); - // cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, - // plan_instruction.profile_overrides); - if (!cur_plan_profile) - throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); + auto cur_plan_profile = + request.profiles->getProfile>(name_, plan_instruction.getProfile(name_)); if (plan_instruction.getWaypoint().isCartesianWaypoint()) { diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index a334068cce8..353f9910e8c 100644 --- a/tesseract_motion_planners/examples/chain_example.cpp +++ b/tesseract_motion_planners/examples/chain_example.cpp @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -141,6 +142,7 @@ int main(int /*argc*/, char** /*argv*/) auto descartes_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); @@ -150,6 +152,7 @@ int main(int /*argc*/, char** /*argv*/) profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "DEFAULT", descartes_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index fa47e8e4694..41c0becece1 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -112,6 +113,7 @@ int main(int /*argc*/, char** /*argv*/) auto ompl_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); @@ -121,6 +123,7 @@ int main(int /*argc*/, char** /*argv*/) profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/examples/raster_example.cpp b/tesseract_motion_planners/examples/raster_example.cpp index f2999dbc589..6d939942696 100644 --- a/tesseract_motion_planners/examples/raster_example.cpp +++ b/tesseract_motion_planners/examples/raster_example.cpp @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -88,7 +89,7 @@ int main(int /*argc*/, char** /*argv*/) auto kin_group = env->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); auto cur_state = env->getState(); - CompositeInstruction program("raster_program", CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(kin_group->getJointNames(), Eigen::VectorXd::Zero(6)) }; @@ -112,14 +113,14 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)); // Define raster move instruction - MoveInstruction plan_c0(wp2, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c1(wp3, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c2(wp4, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c3(wp5, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c4(wp6, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c5(wp7, MoveInstructionType::LINEAR, "RASTER"); - - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_c0(wp2, MoveInstructionType::LINEAR); + MoveInstruction plan_c1(wp3, MoveInstructionType::LINEAR); + MoveInstruction plan_c2(wp4, MoveInstructionType::LINEAR); + MoveInstruction plan_c3(wp5, MoveInstructionType::LINEAR); + MoveInstruction plan_c4(wp6, MoveInstructionType::LINEAR); + MoveInstruction plan_c5(wp7, MoveInstructionType::LINEAR); + + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); @@ -140,7 +141,7 @@ int main(int /*argc*/, char** /*argv*/) } { - MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE); plan_f1.setDescription("transition_from_end_plan"); CompositeInstruction transition_from_end; transition_from_end.setDescription("transition_from_end"); @@ -169,7 +170,7 @@ int main(int /*argc*/, char** /*argv*/) } { - MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE); plan_f1.setDescription("transition_from_end_plan"); CompositeInstruction transition_from_end; transition_from_end.setDescription("transition_from_end"); @@ -197,7 +198,7 @@ int main(int /*argc*/, char** /*argv*/) program.push_back(raster_segment); } - MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE); plan_f2.setDescription("to_end_plan"); CompositeInstruction to_end; to_end.setDescription("to_end"); @@ -214,15 +215,19 @@ int main(int /*argc*/, char** /*argv*/) auto descartes_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "DEFAULT", descartes_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile>( + DESCARTES_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, descartes_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_plan_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_solver_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index e76cf679eed..c09ce68ebd7 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -348,13 +348,7 @@ OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& requ std::vector active_link_names = manip->getActiveLinkNames(); // Get Plan Profile - std::string profile = end_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - auto cur_plan_profile = - getProfile(name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, end_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); + auto cur_plan_profile = request.profiles->getProfile(name_, end_instruction.getProfile(name_)); /** @todo Should check that the joint names match the order of the manipulator */ OMPLProblemConfig config; diff --git a/tesseract_motion_planners/test/CMakeLists.txt b/tesseract_motion_planners/test/CMakeLists.txt index 903dce100ce..2579d6bc2e8 100644 --- a/tesseract_motion_planners/test/CMakeLists.txt +++ b/tesseract_motion_planners/test/CMakeLists.txt @@ -196,31 +196,6 @@ if(TESSERACT_BUILD_DESCARTES) add_dependencies(run_tests ${PROJECT_NAME}_descartes_unit) endif() -# Utils Tests -add_executable(${PROJECT_NAME}_utils_unit utils_test.cpp) -target_link_libraries( - ${PROJECT_NAME}_utils_unit - PRIVATE GTest::GTest - GTest::Main - tesseract::tesseract_support - tesseract::tesseract_command_language - ${PROJECT_NAME}_core - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_utils_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} - ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_utils_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_utils_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_utils_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_utils_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_utils_unit) -add_dependencies(${PROJECT_NAME}_utils_unit ${PROJECT_NAME}_core) -add_dependencies(run_tests ${PROJECT_NAME}_utils_unit) - # Serialize Tests if(TESSERACT_BUILD_OMPL AND TESSERACT_BUILD_TRAJOPT AND TESSERACT_BUILD_DESCARTES) add_executable(${PROJECT_NAME}_serialize_unit serialize_test.cpp) diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 5159c79055e..172f1f0f1f8 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP struct ProfileBase { + using ConstPtr = std::shared_ptr; int a{ 0 }; }; @@ -43,6 +44,7 @@ struct ProfileTest : public ProfileBase struct ProfileBase2 { + using ConstPtr = std::shared_ptr; int b{ 0 }; }; @@ -58,30 +60,28 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_FALSE(profiles.hasProfileEntry("ns")); + EXPECT_THROW(profiles.getProfile("ns", "key"), std::runtime_error); // NOLINT profiles.addProfile("ns", "key", std::make_shared()); - EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); + ProfileBase::ConstPtr profile; + ASSERT_NO_THROW(profile = profiles.getProfile("ns", "key")); // NOLINT - auto profile = profiles.getProfile("ns", "key"); - - EXPECT_TRUE(profile != nullptr); + ASSERT_TRUE(profile != nullptr); EXPECT_EQ(profile->a, 0); // Check add same profile with different key profiles.addProfile("ns", "key2", profile); - EXPECT_TRUE(profiles.hasProfile("ns", "key2")); - auto profile2 = profiles.getProfile("ns", "key2"); - EXPECT_TRUE(profile2 != nullptr); + ProfileBase::ConstPtr profile2; + ASSERT_NO_THROW(profile2 = profiles.getProfile("ns", "key2")); // NOLINT + ASSERT_TRUE(profile2 != nullptr); EXPECT_EQ(profile2->a, 0); // Check replacing a profile profiles.addProfile("ns", "key", std::make_shared(10)); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); - auto profile_check = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check != nullptr); + ProfileBase::ConstPtr profile_check; + ASSERT_NO_THROW(profile_check = profiles.getProfile("ns", "key")); // NOLINT + ASSERT_TRUE(profile_check != nullptr); EXPECT_EQ(profile_check->a, 10); auto profile_map = profiles.getProfileEntry("ns"); @@ -90,12 +90,13 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT EXPECT_EQ(it->second->a, 10); profiles.addProfile("ns", "key", std::make_shared(20)); - auto profile_check2 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check2 != nullptr); + ProfileBase::ConstPtr profile_check2; + ASSERT_NO_THROW(profile_check2 = profiles.getProfile("ns", "key")); // NOLINT + ASSERT_TRUE(profile_check2 != nullptr); EXPECT_EQ(profile_check2->a, 20); // Request a profile entry namespace that does not exist - EXPECT_ANY_THROW(profiles.getProfileEntry("DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "DoesNotExist")); // NOLINT // Request a profile that does not exist EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "key")); // NOLINT @@ -104,10 +105,10 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT EXPECT_ANY_THROW(profiles.getProfile("ns", "DoesNotExist")); // NOLINT // Check adding a empty namespace - EXPECT_ANY_THROW(profiles.addProfile("", "key3", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("", "key3", nullptr)); // NOLINT // Check adding a empty key - EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "", nullptr)); // NOLINT // Check adding a nullptr profile EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // NOLINT @@ -115,13 +116,14 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT // Add different profile entry profiles.addProfile("ns", "key", std::make_shared(5)); EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); - auto profile_check3 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check3 != nullptr); + ProfileBase2::ConstPtr profile_check3; + ASSERT_NO_THROW(profile_check3 = profiles.getProfile("ns", "key")); // NOLINT + ASSERT_TRUE(profile_check3 != nullptr); EXPECT_EQ(profile_check3->b, 5); // Check that other profile entry with same key is not affected - auto profile_check4 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check4 != nullptr); + ProfileBase::ConstPtr profile_check4; + ASSERT_NO_THROW(profile_check4 = profiles.getProfile("ns", "key")); // NOLINT + ASSERT_TRUE(profile_check4 != nullptr); EXPECT_EQ(profile_check4->a, 20); } diff --git a/tesseract_motion_planners/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/test/trajopt_planner_tests.cpp index 05e03259462..db036e36798 100644 --- a/tesseract_motion_planners/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/test/trajopt_planner_tests.cpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -135,11 +136,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -210,11 +213,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -290,11 +295,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -374,11 +381,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -458,11 +467,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -542,11 +553,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -624,11 +637,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; @@ -686,11 +701,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT plan_profile->term_type = trajopt::TermType::TT_COST; // Everything associated with profile is now added as a cost auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner; diff --git a/tesseract_motion_planners/test/utils_test.cpp b/tesseract_motion_planners/test/utils_test.cpp deleted file mode 100644 index 6d2a99f3427..00000000000 --- a/tesseract_motion_planners/test/utils_test.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/** - * @file utils_test.cpp - * @brief - * - * @author Matthew Powelson - * @date June 15, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include - -using namespace tesseract_planning; -using namespace tesseract_environment; - -class TesseractPlanningUtilsUnit : public ::testing::Test -{ -protected: - Environment::Ptr env_; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - } -}; - -TEST_F(TesseractPlanningUtilsUnit, GenerateSeed) // NOLINT -{ - EXPECT_TRUE(true); -} - -TEST_F(TesseractPlanningUtilsUnit, GetProfileStringTest) // NOLINT -{ - std::string input_profile; - std::string planner_name = "Planner_1"; - std::string default_planner = "TEST_DEFAULT"; - - std::unordered_map remap; - remap["profile_1"] = "profile_1_remapped"; - PlannerProfileRemapping remapping; - remapping["Planner_2"] = remap; - - // Empty input profile - std::string output_profile = getProfileString(planner_name, input_profile, remapping); - EXPECT_EQ(output_profile, "DEFAULT"); - output_profile = getProfileString(planner_name, input_profile, remapping, default_planner); - EXPECT_EQ(output_profile, default_planner); - - // Planner name doesn't match - input_profile = "profile_1"; - output_profile = getProfileString(planner_name, input_profile, remapping, default_planner); - EXPECT_EQ(input_profile, output_profile); - - // Profile name doesn't match - input_profile = "doesnt_match"; - output_profile = getProfileString("Planner_2", input_profile, remapping, default_planner); - EXPECT_EQ(input_profile, output_profile); - - // Successful remap - input_profile = "profile_1"; - output_profile = getProfileString("Planner_2", input_profile, remapping, default_planner); - EXPECT_EQ(output_profile, "profile_1_remapped"); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 124d8fd8743..08ae60558e9 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -205,15 +205,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const } // Apply Solver parameters - std::string profile = request.instructions.getProfile(); - ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - TrajOptSolverProfile::ConstPtr solver_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); - if (!solver_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - + TrajOptSolverProfile::ConstPtr solver_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); solver_profile->apply(*pci); // Get kinematics information @@ -245,12 +238,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); + TrajOptPlanProfile::ConstPtr cur_plan_profile = + request.profiles->getProfile(name_, move_instruction.getProfile(name_)); if (move_instruction.getWaypoint().isCartesianWaypoint()) { @@ -328,14 +317,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const for (long i = 0; i < pci->basic_info.n_steps; ++i) pci->init_info.data.row(i) = seed_states[static_cast(i)]; - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - TrajOptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); - if (!cur_composite_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - + TrajOptCompositeProfile::ConstPtr cur_composite_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); cur_composite_profile->apply(*pci, 0, pci->basic_info.n_steps - 1, composite_mi, active_links, fixed_steps); return pci; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 42b0e7f859d..23aafe69592 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -215,16 +215,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co } // Apply Solver parameters - std::string profile = request.instructions.getProfile(); - ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, PlannerProfileRemapping()); - // TrajOptSolverProfile::ConstPtr solver_profile = - // getProfile(profile, solver_profiles, std::make_shared()); - // solver_profile = applyProfileOverrides(name, solver_profile, profile_overrides); - // if (!solver_profile) - // throw std::runtime_error("TrajOptSolverConfig: Invalid profile"); - - // solver_profile->apply(*pci); + // TrajOptSolverProfile::ConstPtr solver_profile = request.profiles->getProfile(name_, + // request.instructions.getProfile(name_)); solver_profile->apply(*pci); // Get kinematics information tesseract_environment::Environment::ConstPtr env = request.env; @@ -255,12 +247,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); + TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = + request.profiles->getProfile(name_, move_instruction.getProfile(name_)); if (move_instruction.getWaypoint().isCartesianWaypoint()) { @@ -343,13 +331,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // ---------------- // Translate TCL for CompositeInstructions // ---------------- - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); - if (!cur_composite_profile) - throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); + TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); cur_composite_profile->apply( *problem, 0, static_cast(problem->vars.size()) - 1, composite_mi, active_links, fixed_steps); diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index ec5f6fc34a5..f9797efe87b 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -45,6 +46,8 @@ int main() // Define profiles auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); // Define the program CompositeInstruction program = rasterExampleProgram(); diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp index 792e5487da9..b4991c2c40d 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -7,6 +7,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -44,6 +45,8 @@ int main() // Define profiles auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); // Define the program CompositeInstruction program = freespaceExampleProgramIIWA(); diff --git a/tesseract_task_composer/include/tesseract_task_composer/task_composer_problem.h b/tesseract_task_composer/include/tesseract_task_composer/task_composer_problem.h index 34be5839380..a5b78692ceb 100644 --- a/tesseract_task_composer/include/tesseract_task_composer/task_composer_problem.h +++ b/tesseract_task_composer/include/tesseract_task_composer/task_composer_problem.h @@ -52,19 +52,6 @@ struct TaskComposerProblem TaskComposerDataStorage input_data, std::string name = "unset"); - TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - tesseract_common::ManipulatorInfo manip_info, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - std::string name = "unset"); - - TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - std::string name = "unset"); - TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, TaskComposerDataStorage input_data, std::string name = "unset"); @@ -84,18 +71,6 @@ struct TaskComposerProblem /** @brief Global Manipulator Information */ tesseract_common::ManipulatorInfo manip_info; - /** - * @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a - * given motion planner. - */ - ProfileRemapping move_profile_remapping; - - /** - * @brief This allows the remapping of the Composite Profile identified in the command language to a specific profile - * for a given motion planner. - */ - ProfileRemapping composite_profile_remapping; - /** @brief The location data is stored and retrieved during execution */ TaskComposerDataStorage input_data; diff --git a/tesseract_task_composer/include/tesseract_task_composer/task_composer_utils.h b/tesseract_task_composer/include/tesseract_task_composer/task_composer_utils.h index b18c713dbcf..d4166d7a1e1 100644 --- a/tesseract_task_composer/include/tesseract_task_composer/task_composer_utils.h +++ b/tesseract_task_composer/include/tesseract_task_composer/task_composer_utils.h @@ -26,13 +26,20 @@ #ifndef TESSERACT_TASK_COMPOSER_TASK_COMPOSER_UTILS_H #define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_UTILS_H -#include +#include +#include namespace tesseract_planning { +class TaskComposerServer; +class ProfileDictionary; + void loadDefaultTaskComposerNodes(TaskComposerServer& server, const std::string& input_key, const std::string& output_key); -} + +void addDefaultTaskComposerProfiles(ProfileDictionary& profiles, const std::vector& profile_names); +void addDefaultPlannerProfiles(ProfileDictionary& profiles, const std::vector& profile_names); +} // namespace tesseract_planning #endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_UTILS_H diff --git a/tesseract_task_composer/src/nodes/check_input_task.cpp b/tesseract_task_composer/src/nodes/check_input_task.cpp index 494283313c2..173f5e917f9 100644 --- a/tesseract_task_composer/src/nodes/check_input_task.cpp +++ b/tesseract_task_composer/src/nodes/check_input_task.cpp @@ -72,16 +72,19 @@ TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input, return info; } - const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); - - if (!cur_composite_profile->isValid(input)) + try + { + const auto& ci = input_data_poly.as(); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); + if (!cur_composite_profile->isValid(input)) + { + info->message = "Validator failed"; + return info; + } + } + catch (const std::exception& ex) { - info->message = "Validator failed"; + info->message = ex.what(); return info; } } diff --git a/tesseract_task_composer/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/src/nodes/continuous_contact_check_task.cpp index b248035d38d..769e727d4d9 100644 --- a/tesseract_task_composer/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/src/nodes/continuous_contact_check_task.cpp @@ -78,11 +78,7 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input.problem.manip_info); diff --git a/tesseract_task_composer/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/src/nodes/discrete_contact_check_task.cpp index 8cd24b6a9ee..f42f007feb7 100644 --- a/tesseract_task_composer/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/src/nodes/discrete_contact_check_task.cpp @@ -76,11 +76,7 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input.problem.manip_info); diff --git a/tesseract_task_composer/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/src/nodes/fix_state_bounds_task.cpp index 575cb82bea7..05f25b0b363 100644 --- a/tesseract_task_composer/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/src/nodes/fix_state_bounds_task.cpp @@ -84,11 +84,8 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + std::string profile; + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); if (cur_composite_profile->mode == FixStateBoundsProfile::Settings::DISABLED) { diff --git a/tesseract_task_composer/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/src/nodes/fix_state_collision_task.cpp index 09f51b96046..2dc7ff95011 100644 --- a/tesseract_task_composer/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/src/nodes/fix_state_collision_task.cpp @@ -346,11 +346,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp auto& ci = input_data_poly.as(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); switch (cur_composite_profile->mode) { diff --git a/tesseract_task_composer/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/src/nodes/iterative_spline_parameterization_task.cpp index 5344617d2d6..881cea5db9e 100644 --- a/tesseract_task_composer/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/src/nodes/iterative_spline_parameterization_task.cpp @@ -86,11 +86,8 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + input.profiles->getProfile(name_, ci.getProfile(name_)); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); @@ -112,20 +109,18 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) { const auto& mi = flattened[static_cast(idx)].get().as(); - std::string move_profile = mi.getProfile(); - - // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, input.problem.move_profile_remapping); - auto cur_move_profile = getProfile( - name_, move_profile, *input.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters - if (cur_move_profile) + try { + auto cur_move_profile = + input.profiles->getProfile(name_, mi.getProfile(name_)); velocity_scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; acceleration_scaling_factors[idx] = cur_move_profile->max_acceleration_scaling_factor; } + catch (const std::exception&) + { + } } // Solve using parameters diff --git a/tesseract_task_composer/src/nodes/min_length_task.cpp b/tesseract_task_composer/src/nodes/min_length_task.cpp index 1880ddf3c46..78f5e292986 100644 --- a/tesseract_task_composer/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/src/nodes/min_length_task.cpp @@ -80,11 +80,7 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerInput& input, // Get Composite Profile const auto& ci = input_data_poly.as(); long cnt = ci.getMoveInstructionCount(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); if (cnt < cur_composite_profile->min_length) { diff --git a/tesseract_task_composer/src/nodes/motion_planner_task.cpp b/tesseract_task_composer/src/nodes/motion_planner_task.cpp index 76b14474409..44f0238d60e 100644 --- a/tesseract_task_composer/src/nodes/motion_planner_task.cpp +++ b/tesseract_task_composer/src/nodes/motion_planner_task.cpp @@ -89,8 +89,6 @@ TaskComposerNodeInfo::UPtr MotionPlannerTask::runImpl(TaskComposerInput& input, request.env = input.problem.env; request.instructions = instructions; request.profiles = input.profiles; - request.plan_profile_remapping = input.problem.move_profile_remapping; - request.composite_profile_remapping = input.problem.composite_profile_remapping; request.format_result_as_input = format_result_as_input_; // -------------------- diff --git a/tesseract_task_composer/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/src/nodes/profile_switch_task.cpp index 1b568ec7f25..3b57b36fd04 100644 --- a/tesseract_task_composer/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/src/nodes/profile_switch_task.cpp @@ -73,11 +73,7 @@ TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerInput& input, // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); // Return the value specified in the profile CONSOLE_BRIDGE_logDebug("ProfileSwitchProfile returning %d", cur_composite_profile->return_value); diff --git a/tesseract_task_composer/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/src/nodes/ruckig_trajectory_smoothing_task.cpp index b0eebc5a2ee..10cc26733f9 100644 --- a/tesseract_task_composer/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -83,11 +83,8 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + input.profiles->getProfile(name_, ci.getProfile(name_)); RuckigTrajectorySmoothing solver(cur_composite_profile->duration_extension_fraction, cur_composite_profile->max_duration_extension_factor); @@ -114,21 +111,19 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) { const auto& mi = flattened[static_cast(idx)].get().as(); - std::string move_profile = mi.getProfile(); - - // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, input.problem.move_profile_remapping); - auto cur_move_profile = getProfile( - name_, move_profile, *input.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters - if (cur_move_profile) + try { + auto cur_move_profile = + input.profiles->getProfile(name_, mi.getProfile(name_)); velocity_scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; acceleration_scaling_factors[idx] = cur_move_profile->max_acceleration_scaling_factor; jerk_scaling_factors[idx] = cur_move_profile->max_jerk_scaling_factor; } + catch (const std::exception&) + { + } } // Solve using parameters diff --git a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp index 2897b0d2aab..4d76d7f4f7b 100644 --- a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp @@ -87,11 +87,8 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + input.profiles->getProfile(name_, ci.getProfile(name_)); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); @@ -113,20 +110,18 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer for (std::size_t idx = 0; idx < ci.size(); idx++) { const auto& mi = ci.at(idx).as(); - profile = mi.getProfile(); - - // Check for remapping of the plan profile - std::string remap = getProfileString(name_, profile, input.problem.move_profile_remapping); - auto cur_move_profile = getProfile( - name_, remap, *input.profiles, std::make_shared()); - cur_move_profile = applyProfileOverrides(name_, remap, cur_move_profile, mi.getProfileOverrides()); // If there is a move profile associated with it, override the parameters - if (cur_move_profile) + try { + auto cur_move_profile = + input.profiles->getProfile(name_, mi.getProfile(name_)); use_move_profile = true; scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; } + catch (const std::exception&) + { + } } if (use_move_profile) diff --git a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp index f6a239bdd35..8457f379895 100644 --- a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp @@ -78,11 +78,7 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& in // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, input.problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *input.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = input.profiles->getProfile(name_, ci.getProfile(name_)); assert(cur_composite_profile->longest_valid_segment_length > 0); InstructionPoly start_instruction; diff --git a/tesseract_task_composer/src/task_composer_problem.cpp b/tesseract_task_composer/src/task_composer_problem.cpp index 9a7c2c2ea9a..3c7195aff3a 100644 --- a/tesseract_task_composer/src/task_composer_problem.cpp +++ b/tesseract_task_composer/src/task_composer_problem.cpp @@ -52,34 +52,6 @@ TaskComposerProblem::TaskComposerProblem(tesseract_environment::Environment::Con { } -TaskComposerProblem::TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - tesseract_common::ManipulatorInfo manip_info, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - std::string name) - : name(std::move(name)) - , env(std::move(env)) - , manip_info(std::move(manip_info)) - , move_profile_remapping(std::move(move_profile_remapping)) - , composite_profile_remapping(std::move(composite_profile_remapping)) - , input_data(std::move(input_data)) -{ -} - -TaskComposerProblem::TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - std::string name) - : name(std::move(name)) - , env(std::move(env)) - , move_profile_remapping(std::move(move_profile_remapping)) - , composite_profile_remapping(std::move(composite_profile_remapping)) - , input_data(std::move(input_data)) -{ -} - TaskComposerProblem::TaskComposerProblem(tesseract_environment::Environment::ConstPtr env, TaskComposerDataStorage input_data, std::string name) @@ -93,8 +65,6 @@ bool TaskComposerProblem::operator==(const TaskComposerProblem& rhs) const equal &= name == rhs.name; equal &= tesseract_common::pointersEqual(env, rhs.env); equal &= manip_info == rhs.manip_info; - equal &= move_profile_remapping == rhs.move_profile_remapping; - equal &= composite_profile_remapping == rhs.composite_profile_remapping; equal &= input_data == rhs.input_data; return equal; } @@ -107,8 +77,6 @@ void TaskComposerProblem::serialize(Archive& ar, const unsigned int /*version*/) ar& boost::serialization::make_nvp("name", name); ar& boost::serialization::make_nvp("environment", env); ar& boost::serialization::make_nvp("manip_info", manip_info); - ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping); - ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping); ar& boost::serialization::make_nvp("input_data", input_data); } diff --git a/tesseract_task_composer/src/task_composer_utils.cpp b/tesseract_task_composer/src/task_composer_utils.cpp index 85c5c2eb46e..977a0fb62af 100644 --- a/tesseract_task_composer/src/task_composer_utils.cpp +++ b/tesseract_task_composer/src/task_composer_utils.cpp @@ -29,8 +29,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include +// Nodes #include #include #include @@ -44,10 +46,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include - #ifdef TESSERACT_TASK_COMPOSER_HAS_TRAJOPT_IFOPT #include #endif +// Task Profiles +#include +#include +#include +#include +#include +#include +#include +#include +// Planner Profiles +#include +#include +#include +#include +#include +#include +#include namespace tesseract_planning { @@ -73,4 +91,54 @@ void loadDefaultTaskComposerNodes(TaskComposerServer& server, server.addTask(std::make_unique(input_key, output_key)); server.addTask(std::make_unique(input_key, output_key)); } + +void addDefaultTaskComposerProfiles(ProfileDictionary& profiles, const std::vector& profile_names) +{ + auto check_input_profile = std::make_shared(); + auto contact_check_profile = std::make_shared(); + auto fix_state_bounds_profile = std::make_shared(); + auto fix_state_collision_profile = std::make_shared(); + auto isp_profile = std::make_shared(); + auto min_length_profile = std::make_shared(); + auto profile_switch_profile = std::make_shared(); + auto upsample_trajectory_profile = std::make_shared(); + + for (const std::string& profile_name : profile_names) + { + using namespace node_names; + profiles.addProfile(CHECK_INPUT_TASK_NAME, profile_name, check_input_profile); + profiles.addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, profile_name, contact_check_profile); + profiles.addProfile(FIX_STATE_BOUNDS_TASK_NAME, profile_name, fix_state_bounds_profile); + profiles.addProfile( + FIX_STATE_COLLISION_TASK_NAME, profile_name, fix_state_collision_profile); + profiles.addProfile( + ITERATIVE_SPLINE_PARAMETERIZATION_TASK_NAME, profile_name, isp_profile); + profiles.addProfile(MIN_LENGTH_TASK_NAME, profile_name, min_length_profile); + profiles.addProfile(PROFILE_SWITCH_TASK_NAME, profile_name, profile_switch_profile); + profiles.addProfile( + UPSAMPLE_TRAJECTORY_TASK_NAME, profile_name, upsample_trajectory_profile); + } +} + +void addDefaultPlannerProfiles(ProfileDictionary& profiles, const std::vector& profile_names) +{ + auto simple_plan_profile = std::make_shared(); + auto descartes_profile = std::make_shared(); + auto trajopt_plan_profile = std::make_shared(); + auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); + auto ompl_profile = std::make_shared(); + + for (const std::string& profile_name : profile_names) + { + using namespace profile_ns; + profiles.addProfile(SIMPLE_DEFAULT_NAMESPACE, profile_name, simple_plan_profile); + profiles.addProfile>(DESCARTES_DEFAULT_NAMESPACE, profile_name, descartes_profile); + profiles.addProfile(TRAJOPT_DEFAULT_NAMESPACE, profile_name, trajopt_plan_profile); + profiles.addProfile(TRAJOPT_DEFAULT_NAMESPACE, profile_name, trajopt_composite_profile); + profiles.addProfile(TRAJOPT_DEFAULT_NAMESPACE, profile_name, trajopt_solver_profile); + profiles.addProfile(OMPL_DEFAULT_NAMESPACE, profile_name, ompl_profile); + } +} + } // namespace tesseract_planning diff --git a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp index 40f3dbc4cfe..5ebea74db6f 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp @@ -11,30 +11,21 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include + #include #include #include #include +#include #include -#include #include #include #include #include +#include #include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include #include #include "raster_example_program.h" @@ -313,12 +304,13 @@ TEST_F(TesseractTaskComposerUnit, RasterProcessManagerDefaultPlanProfileTest) / CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -356,12 +348,13 @@ TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultPlanProfileTe CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -399,12 +392,13 @@ TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultLVSPlanProfil CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -442,12 +436,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultPlanProfileTest CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -485,12 +480,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfileT CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -528,12 +524,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultPlanProfi CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -571,12 +568,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlanPr CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data;