Add simple ompl profile interface

tesseract-robotics · Dec 7, 2024 · e73a0f7 · e73a0f7
1 parent ee11855
commit e73a0f7
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 266 deletions.
diff --git a/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h
@@ -62,7 +62,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   using ConstPtr = std::shared_ptr<const OMPLDefaultPlanProfile>;
 
   OMPLDefaultPlanProfile();
-  OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);
 
   /** @brief The OMPL state space to use when planning */
   OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE };
@@ -104,20 +103,9 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   /** @brief The collision check configuration */
   tesseract_collision::CollisionCheckConfig collision_check_config;
 
-  /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */
-  StateSamplerAllocator state_sampler_allocator;
-
-  /** @brief Set the optimization objective function allocator. Default is to minimize path length */
-  OptimizationObjectiveAllocator optimization_objective_allocator;
-
-  /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses
-   * StateCollisionValidator */
-  StateValidityCheckerAllocator svc_allocator;
-
-  /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used
-   * ContinuousMotionValidator */
-  MotionValidatorAllocator mv_allocator;
+  std::vector<OMPLProblemConfig> create(const PlannerRequest& request) const override;
 
+protected:
   void setup(OMPLProblem& prob) const override;
 
   void applyGoalStates(OMPLProblem& prob,
@@ -148,9 +136,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
                         const std::vector<std::string>& active_links,
                         int index) const override;
 
-  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
-
-protected:
   ompl::base::StateValidityCheckerPtr processStateValidator(OMPLProblem& prob) const;
   void processMotionValidator(OMPLProblem& prob,
                               const ompl::base::StateValidityCheckerPtr& svc_without_collision) const;

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
@@ -35,6 +35,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/fwd.h>
 #include <tesseract_command_language/fwd.h>
+#include <tesseract_environment/fwd.h>
+#include <tesseract_motion_planners/core/fwd.h>
+
 #include <tesseract_command_language/profile.h>
 
 namespace tinyxml2
@@ -46,6 +49,8 @@ class XMLDocument;
 namespace tesseract_planning
 {
 struct OMPLProblem;
+struct OMPLProblemConfig;
+
 class OMPLPlanProfile : public Profile
 {
 public:
@@ -60,46 +65,14 @@ class OMPLPlanProfile : public Profile
    */
   static std::size_t getStaticKey();
 
-  virtual void setup(OMPLProblem& prob) const = 0;
-
-  virtual void applyGoalStates(OMPLProblem& prob,
-                               const Eigen::Isometry3d& cartesian_waypoint,
-                               const MoveInstructionPoly& parent_instruction,
-                               const tesseract_common::ManipulatorInfo& manip_info,
-                               const std::vector<std::string>& active_links,
-                               int index) const = 0;
-
-  virtual void applyGoalStates(OMPLProblem& prob,
-                               const Eigen::VectorXd& joint_waypoint,
-                               const MoveInstructionPoly& parent_instruction,
-                               const tesseract_common::ManipulatorInfo& manip_info,
-                               const std::vector<std::string>& active_links,
-                               int index) const = 0;
-
-  virtual void applyStartStates(OMPLProblem& prob,
-                                const Eigen::Isometry3d& cartesian_waypoint,
-                                const MoveInstructionPoly& parent_instruction,
-                                const tesseract_common::ManipulatorInfo& manip_info,
-                                const std::vector<std::string>& active_links,
-                                int index) const = 0;
-
-  virtual void applyStartStates(OMPLProblem& prob,
-                                const Eigen::VectorXd& joint_waypoint,
-                                const MoveInstructionPoly& parent_instruction,
-                                const tesseract_common::ManipulatorInfo& manip_info,
-                                const std::vector<std::string>& active_links,
-                                int index) const = 0;
-
-  virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
+  virtual std::vector<OMPLProblemConfig> create(const PlannerRequest& request) const = 0;
 
 protected:
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
-/** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */
-
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlanProfile)