Add simple ompl profile interface

tesseract_motion_planners/ompl/CMakeLists.txt

@@ -14,9 +14,7 @@ set(OMPL_SRC
     src/profile/ompl_default_plan_profile.cpp
     src/utils.cpp
     src/state_collision_validator.cpp
-    src/compound_state_validator.cpp
-    src/serialize.cpp
-    src/deserialize.cpp)
+    src/compound_state_validator.cpp)
 
 # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif()
 

tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h

@@ -94,14 +94,6 @@ class OMPLMotionPlanner : public MotionPlanner
 protected:
   /** @brief OMPL Parallel planner */
   std::shared_ptr<ompl::tools::ParallelPlan> parallel_plan_;
-
-  OMPLProblemConfig createSubProblem(const PlannerRequest& request,
-                                     const tesseract_common::ManipulatorInfo& composite_mi,
-                                     const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                                     const MoveInstructionPoly& start_instruction,
-                                     const MoveInstructionPoly& end_instruction,
-                                     int n_output_states,
-                                     int index) const;
 };
 
 }  // namespace tesseract_planning

tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h

@@ -62,9 +62,7 @@ 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 };
 
   /** @brief Max planning time allowed in seconds */
@@ -104,53 +102,42 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   /** @brief The collision check configuration */
   tesseract_collision::CollisionCheckConfig collision_check_config;
 
+  std::unique_ptr<OMPLProblem> create(const PlannerRequest& request,
+                                      const MoveInstructionPoly& start_instruction,
+                                      const MoveInstructionPoly& end_instruction,
+                                      int n_output_states) const override;
+
+protected:
   /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */
-  StateSamplerAllocator state_sampler_allocator;
+  StateSamplerAllocator state_sampler_allocator_;
 
   /** @brief Set the optimization objective function allocator. Default is to minimize path length */
-  OptimizationObjectiveAllocator optimization_objective_allocator;
+  OptimizationObjectiveAllocator optimization_objective_allocator_;
 
   /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses
    * StateCollisionValidator */
-  StateValidityCheckerAllocator svc_allocator;
+  StateValidityCheckerAllocator svc_allocator_;
 
   /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used
    * ContinuousMotionValidator */
-  MotionValidatorAllocator mv_allocator;
+  MotionValidatorAllocator mv_allocator_;
 
-  void setup(OMPLProblem& prob) const override;
+  void setup(OMPLProblem& prob) const;
 
   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 override;
+                       const tesseract_common::ManipulatorInfo& manip_info) const;
 
-  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 override;
+  void applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const;
 
   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 override;
+                        const tesseract_common::ManipulatorInfo& manip_info) const;
 
-  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 override;
+  void applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const;
 
-  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;

tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h

@@ -34,7 +34,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/fwd.h>
+#include <tesseract_kinematics/core/fwd.h>
+#include <tesseract_environment/fwd.h>
 #include <tesseract_command_language/fwd.h>
+#include <tesseract_motion_planners/core/fwd.h>
+
 #include <tesseract_command_language/profile.h>
 
 namespace tinyxml2
@@ -46,6 +50,7 @@ class XMLDocument;
 namespace tesseract_planning
 {
 struct OMPLProblem;
+
 class OMPLPlanProfile : public Profile
 {
 public:
@@ -60,46 +65,25 @@ 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;
+  /**
+   * @brief Create OMPL Planning Problem
+   * @param request The planning request
+   * @param start_instruction The start instruction
+   * @param end_instruction The goal instruction
+   * @param n_output_states The number of interpolated output states
+   * @return A OMPL Problem
+   */
+  virtual std::unique_ptr<OMPLProblem> create(const PlannerRequest& request,
+                                              const MoveInstructionPoly& start_instruction,
+                                              const MoveInstructionPoly& end_instruction,
+                                              int n_output_states) 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)