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..d535f04297e 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 @@ -83,10 +83,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; @@ -349,7 +349,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 cfe8b286b28..b0e4490a96a 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 1e73232f573..241ca8160f0 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 @@ -132,11 +132,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); @@ -184,16 +184,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; @@ -247,22 +247,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(); } @@ -312,16 +312,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 0f1ae5264d0..863b60f8e79 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -69,13 +69,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 1d818930c63..b71eda772ba 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 567d017085a..4f8a8187f39 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 c7675c2f0b6..e7bae62eaa1 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 @@ -157,7 +158,7 @@ bool BasicCartesianExample::run() // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); + MoveInstruction start_instruction(wp0, MoveInstructionType::START, "freespace_profile"); program.setStartInstruction(start_instruction); // Create cartesian waypoint @@ -193,6 +194,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 87b2736f5a7..b801c3a396d 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() program.setStartInstruction(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 @@ -351,8 +356,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 @@ -366,7 +372,7 @@ bool CarSeatExample::run() program.setStartInstruction(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 7d6908d6327..59f1b251ef8 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() program.setStartInstruction(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 @@ -164,7 +165,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 0b7aebeb65b..953a6d25b84 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() program.setStartInstruction(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 @@ -164,7 +165,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 a1caa91dc40..61c0a1df3e7 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 @@ -170,6 +171,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(); @@ -185,7 +189,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); @@ -193,7 +197,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 { @@ -212,7 +217,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); @@ -221,7 +227,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 3d4306f3795..105e4dbd13e 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 @@ -206,6 +207,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 d18c21af983..71d2a3256b5 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 @@ -219,6 +220,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 6b2dccaef82..9233a6d23ae 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 @@ -209,6 +210,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 821404db2a6..1e1856aac97 100644 --- a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp +++ b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp @@ -196,18 +196,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 14dfbac6784..cc7f8fa1caf 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 3ad79107278..2c3de0a23a6 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 d802d3dff2d..cce3950cece 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"); @@ -139,7 +140,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"); @@ -168,7 +169,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"); @@ -196,7 +197,7 @@ int main(int /*argc*/, char** /*argv*/) program.appendInstruction(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"); @@ -213,15 +214,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 b858fdf51de..7e40e5a4028 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -358,13 +358,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 5e1bfed2722..0de716162ac 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; @@ -634,11 +647,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; @@ -705,11 +720,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 5d027ed2c0a..d15e52eac3c 100644 --- a/tesseract_task_composer/src/nodes/motion_planner_task.cpp +++ b/tesseract_task_composer/src/nodes/motion_planner_task.cpp @@ -92,8 +92,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 c73d69b0aeb..c680a075a29 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 cb4b605c74f..4bd33916188 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 = ci.getStartInstruction(); 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 3884c1f390b..c16b935d6b0 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" @@ -324,12 +315,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; @@ -367,12 +359,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; @@ -410,12 +403,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; @@ -453,12 +447,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; @@ -496,12 +491,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; @@ -539,12 +535,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; @@ -582,12 +579,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;