From 0edd73c2de6d0a7af2ef5d74a2bf86fd3ee4675a Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 14:14:34 -0600 Subject: [PATCH 1/3] Added new interfaces for planner profiles; added maps for waypoint, composite, and planner profiles in profile dictionary --- .../profile_dictionary.h | 97 ++++++++++++++++++- 1 file changed, 96 insertions(+), 1 deletion(-) 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 4973bd039fc..75d459379a1 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -36,19 +36,106 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + #ifdef SWIG %shared_ptr(tesseract_planning::ProfileDictionary) #endif // SWIG namespace tesseract_planning { +/** + * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint. + * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler + */ +class WaypointProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + WaypointProfile() = default; + WaypointProfile(const WaypointProfile&) = delete; + WaypointProfile& operator=(const WaypointProfile&) = delete; + WaypointProfile(WaypointProfile&&) = delete; + WaypointProfile&& operator=(WaypointProfile&&) = delete; + + virtual ~WaypointProfile() = default; + + virtual std::any create(const Instruction& instruction, tesseract_environment::Environment::ConstPtr env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce a planner-specific planning profile to apply to a collection of waypoints defined in a + * composite instruction. + * @details Examples of composite profiles include costs/constraints that apply collectively to a group of waypoints + */ +class CompositeProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CompositeProfile() = default; + CompositeProfile(const CompositeProfile&) = delete; + CompositeProfile& operator=(const CompositeProfile&) = delete; + CompositeProfile(CompositeProfile&&) = delete; + CompositeProfile&& operator=(CompositeProfile&&) = delete; + + virtual ~CompositeProfile() = default; + virtual std::any create(const CompositeInstruction& instruction, + tesseract_environment::Environment::ConstPtr env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce configuration parameters for the motion planner + */ +struct PlannerProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PlannerProfile() = default; + PlannerProfile(const PlannerProfile&) = delete; + PlannerProfile& operator=(const PlannerProfile&) = delete; + PlannerProfile(PlannerProfile&&) = delete; + PlannerProfile&& operator=(PlannerProfile&&) = delete; + + virtual ~PlannerProfile() = default; + + virtual std::any create() const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + /** * @brief This class is used to store profiles for motion planning and process planning * @details This is a thread safe class * A ProfileEntry is a std::unordered_map> * - The key is the profile name * - Where std::shared_ptr is the profile - * The ProfleEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) + * The ProfileEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) * @note When adding a profile entry the T should be the base class type. */ class ProfileDictionary @@ -210,10 +297,18 @@ class ProfileDictionary .erase(profile_name); } + std::unordered_map> waypoint_profiles; + std::unordered_map> composite_profiles; + std::unordered_map> planner_profiles; + protected: std::unordered_map> profiles_; mutable std::shared_mutex mutex_; }; } // namespace tesseract_planning +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::WaypointProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::CompositeProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::PlannerProfile); + #endif // TESSERACT_MOTION_PLANNERS_PROFILE_DICTIONARY_H From 6cced559d7cbcfd70924cdaf8f9943ada06024c1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 14:44:14 -0600 Subject: [PATCH 2/3] Forward declare profile function argument classes --- .../profile_dictionary.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) 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 75d459379a1..01f0fab6d5e 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include #include #include @@ -36,15 +37,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include - #ifdef SWIG %shared_ptr(tesseract_planning::ProfileDictionary) #endif // SWIG +namespace tesseract_environment +{ +class Environment; +} + namespace tesseract_planning { +class Instruction; +class CompositeInstruction; + /** * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint. * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler @@ -63,7 +69,7 @@ class WaypointProfile virtual ~WaypointProfile() = default; - virtual std::any create(const Instruction& instruction, tesseract_environment::Environment::ConstPtr env) const = 0; + virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const = 0; private: friend class boost::serialization::access; @@ -92,7 +98,7 @@ class CompositeProfile virtual ~CompositeProfile() = default; virtual std::any create(const CompositeInstruction& instruction, - tesseract_environment::Environment::ConstPtr env) const = 0; + const tesseract_environment::Environment& env) const = 0; private: friend class boost::serialization::access; From 467d5eae1e0e92e67dda041e7d98fe5f75f31c58 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 15:14:46 -0600 Subject: [PATCH 3/3] Include composite instruction and environment headers where profile dictionary is used --- tesseract_motion_planners/test/profile_dictionary_tests.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 5159c79055e..1ee93399384 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include struct ProfileBase