Draft: OMPL Planner Refactor

tesseract_command_language/include/tesseract_command_language/composite_instruction.h

@@ -201,6 +201,7 @@ class CompositeInstruction
   iterator insert(const_iterator p, const value_type& x);
   iterator insert(const_iterator p, value_type&& x);
   iterator insert(const_iterator p, std::initializer_list<value_type> l);
+  iterator insert(const_iterator p, std::size_t n, const value_type& x);
   template <class InputIt>
   void insert(const_iterator pos, InputIt first, InputIt last)
   {

tesseract_command_language/include/tesseract_command_language/joint_waypoint.h

@@ -89,7 +89,10 @@ class JointWaypoint
   /** @returns norm of vector */
   inline double norm() const { return waypoint.norm(); }
   /** @returns true if two are approximate */
-  inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) { return waypoint.isApprox(other, prec); }
+  inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) const
+  {
+    return waypoint.isApprox(other, prec);
+  }
   /** @returns the transpose of the joint positions */
   inline ConstTransposeReturnType transpose() const { return waypoint.transpose(); }  // NOLINT
 

tesseract_command_language/include/tesseract_command_language/profile_dictionary.h

@@ -29,26 +29,121 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <any>
+#include <boost/serialization/serialization.hpp>
+#include <boost/core/demangle.hpp>
 #include <iostream>
 #include <typeindex>
 #include <unordered_map>
 #include <memory>
 #include <shared_mutex>
+#include <sstream>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #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
+ */
+class WaypointProfile
+{
+public:
+  using Ptr = std::shared_ptr<WaypointProfile>;
+  using ConstPtr = std::shared_ptr<const WaypointProfile>;
+
+  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, const tesseract_environment::Environment& env) const = 0;
+
+private:
+  friend class boost::serialization::access;
+  template <class Archive>
+  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<CompositeProfile>;
+  using ConstPtr = std::shared_ptr<const CompositeProfile>;
+
+  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,
+                          const tesseract_environment::Environment& env) const = 0;
+
+private:
+  friend class boost::serialization::access;
+  template <class Archive>
+  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<PlannerProfile>;
+  using ConstPtr = std::shared_ptr<const PlannerProfile>;
+
+  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 <class Archive>
+  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<T> is a std::unordered_map<std::string, std::shared_ptr<const T>>
  *      - The key is the profile name
  *      - Where std::shared_ptr<const T> is the profile
- *    The ProfleEntry<T> is also stored in std::unordered_map where the key here is the std::type_index(typeid(T))
+ *    The ProfileEntry<T> 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 +305,65 @@ class ProfileDictionary
           .erase(profile_name);
   }
 
+  std::unordered_map<std::string, std::unordered_map<std::string, WaypointProfile::ConstPtr>> waypoint_profiles;
+  std::unordered_map<std::string, std::unordered_map<std::string, CompositeProfile::ConstPtr>> composite_profiles;
+  std::unordered_map<std::string, std::unordered_map<std::string, PlannerProfile::ConstPtr>> planner_profiles;
+
+  WaypointProfile::ConstPtr getWaypointProfile(const std::string& ns, const std::string& profile_name) const
+  {
+    return getProfile<WaypointProfile>(ns, profile_name, waypoint_profiles);
+  }
+
+  CompositeProfile::ConstPtr getCompositeProfile(const std::string& ns, const std::string& profile_name) const
+  {
+    return getProfile<CompositeProfile>(ns, profile_name, composite_profiles);
+  }
+
+  PlannerProfile::ConstPtr getPlannerProfile(const std::string& ns, const std::string& profile_name) const
+  {
+    return getProfile<PlannerProfile>(ns, profile_name, planner_profiles);
+  }
+
 protected:
+  template <typename ProfileT>
+  typename ProfileT::ConstPtr getProfile(
+      const std::string& ns,
+      const std::string& profile_name,
+      const std::unordered_map<std::string, std::unordered_map<std::string, typename ProfileT::ConstPtr>>& map) const
+  {
+    try
+    {
+      return map.at(ns).at(profile_name);
+    }
+    catch (const std::out_of_range&)
+    {
+      std::stringstream ss;
+      ss << "Failed to get " << boost::core::demangle(typeid(ProfileT).name()) << " for '" << ns << "/" << profile_name
+         << "'\n";
+      if (map.find(ns) == map.end())
+      {
+        ss << "Profile namespace '" << ns << "' does not exist";
+      }
+      else
+      {
+        ss << "Entries in profile namespace '" << ns << "' are:";
+        for (auto it = map.at(ns).begin(); it != map.at(ns).end(); ++it)
+        {
+          ss << "\n\t" << it->first;
+        }
+      }
+
+      throw std::out_of_range(ss.str());
+    }
+  }
+
   std::unordered_map<std::string, std::unordered_map<std::type_index, std::any>> 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

tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h

@@ -35,6 +35,7 @@ namespace tesseract_planning
 {
 /**
  * @brief Get the first Instruction in a Composite Instruction that is identified by the filter
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @param locate_filter The filter to indicate if an instruction should be considered
  * @param process_child_composites Indicate if child Composite Instructions should be searched
@@ -46,6 +47,7 @@ const Instruction* getFirstInstruction(const CompositeInstruction& composite_ins
 
 /**
  * @brief Get the first Instruction in a Composite Instruction that is identified by the filter
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @param locate_filter The filter to indicate if an instruction should be considered
  * @param process_child_composites Indicate if child Composite Instructions should be searched
@@ -57,6 +59,7 @@ Instruction* getFirstInstruction(CompositeInstruction& composite_instruction,
 
 /**
  * @brief Get the last Instruction in a Composite Instruction that is identified by the filter
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @param locate_filter The filter to indicate if an instruction should be considered
  * @param process_child_composites Indicate if child Composite Instructions should be searched
@@ -68,6 +71,7 @@ const Instruction* getLastInstruction(const CompositeInstruction& composite_inst
 
 /**
  * @brief Get the last Instruction in a Composite Instruction that is identified by the filter
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @param locate_filter The filter to indicate if an instruction should be considered
  * @param process_child_composites Indicate if child Composite Instructions should be searched
@@ -79,7 +83,7 @@ Instruction* getLastInstruction(CompositeInstruction& composite_instruction,
 
 /**
  * @brief Get the first Move Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The first Move Instruction (Non-Const)
  */
@@ -94,7 +98,7 @@ inline MoveInstruction* getFirstMoveInstruction(CompositeInstruction& composite_
 
 /**
  * @brief Get the first Move Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The first Move Instruction (Const)
  */
@@ -109,7 +113,7 @@ inline const MoveInstruction* getFirstMoveInstruction(const CompositeInstruction
 
 /**
  * @brief Get the first Plan Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The first Plan Instruction (Non-Const)
  */
@@ -124,7 +128,7 @@ inline PlanInstruction* getFirstPlanInstruction(CompositeInstruction& composite_
 
 /**
  * @brief Get the first Plan Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The first Plan Instruction (Const)
  */
@@ -139,7 +143,7 @@ inline const PlanInstruction* getFirstPlanInstruction(const CompositeInstruction
 
 /**
  * @brief Get the last Move Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The last Move Instruction (Non-Const)
  */
@@ -154,7 +158,7 @@ inline MoveInstruction* getLastMoveInstruction(CompositeInstruction& composite_i
 
 /**
  * @brief Get the last Move Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The last Move Instruction (Const)
  */
@@ -169,7 +173,7 @@ inline const MoveInstruction* getLastMoveInstruction(const CompositeInstruction&
 
 /**
  * @brief Get the last Plan Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The last Plan Instruction (Non-Const)
  */
@@ -184,7 +188,7 @@ inline PlanInstruction* getLastPlanInstruction(CompositeInstruction& composite_i
 
 /**
  * @brief Get the last Plan Instruction in a Composite Instruction
- * This does not consider the start instruction in child composite instruction
+ * @detials This does not consider the start instruction of nested composite instructions
  * @param composite_instruction Composite Instruction to search
  * @return The last Plan Instruction (Const)
  */
@@ -199,6 +203,7 @@ inline const PlanInstruction* getLastPlanInstruction(const CompositeInstruction&
 
 /**
  * @brief Get number of Instruction in a Composite Instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction The Composite Instruction to process
  * @param locate_filter The filter to indicate if an instruction should be considered
  * @param process_child_composites Indicate if child Composite Instructions should be searched
@@ -210,7 +215,7 @@ long getInstructionCount(const CompositeInstruction& composite_instruction,
 
 /**
  * @brief Get number of Move Instruction in a Composite Instruction
- * This does not consider the start instruction in the child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction The Composite Instruction to process
  * @return The number of Move Instructions
  */
@@ -221,7 +226,7 @@ inline long getMoveInstructionCount(const CompositeInstruction& composite_instru
 
 /**
  * @brief Get number of Plan Instruction in a Composite Instruction
- * This does not consider the start instruction in the child composite instruction
+ * @details This does not consider the start instruction of nested composite instructions
  * @param composite_instruction The Composite Instruction to process
  * @return The number of Plan Instructions
  */

tesseract_command_language/src/composite_instruction.cpp

@@ -166,6 +166,10 @@ CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, st
 {
   return container_.insert(p, l);
 }
+CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::size_t n, const value_type& x)
+{
+  return container_.insert(p, n, x);
+}
 
 template <class... Args>
 CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args)

tesseract_command_language/src/utils/flatten_utils.cpp

@@ -49,7 +49,7 @@ void flattenHelper(std::vector<std::reference_wrapper<Instruction>>& flattened,
                    const flattenFilterFn& filter,
                    bool first_composite)
 {
-  if (composite.hasStartInstruction())
+  if (composite.hasStartInstruction() && first_composite)
     if (!filter || filter(composite.getStartInstruction(), composite, first_composite))
       flattened.emplace_back(composite.getStartInstruction());
 
@@ -92,7 +92,7 @@ void flattenHelper(std::vector<std::reference_wrapper<const Instruction>>& flatt
                    const flattenFilterFn& filter,
                    bool first_composite)
 {
-  if (composite.hasStartInstruction())
+  if (composite.hasStartInstruction() && first_composite)
     if (!filter || filter(composite.getStartInstruction(), composite, first_composite))
       flattened.emplace_back(composite.getStartInstruction());
 
@@ -143,7 +143,7 @@ void flattenToPatternHelper(std::vector<std::reference_wrapper<Instruction>>& fl
     return;
   }
 
-  if (composite.hasStartInstruction())
+  if (composite.hasStartInstruction() && first_composite)
     if (!filter || filter(composite.getStartInstruction(), composite, first_composite))
       flattened.emplace_back(composite.getStartInstruction());
 
@@ -196,7 +196,7 @@ void flattenToPatternHelper(std::vector<std::reference_wrapper<const Instruction
     return;
   }
 
-  if (composite.hasStartInstruction())
+  if (composite.hasStartInstruction() && first_composite)
     if (!filter || filter(composite.getStartInstruction(), composite, first_composite))
       flattened.emplace_back(composite.getStartInstruction());