New ompl profile interface

tesseract_examples/src/freespace_hybrid_example.cpp

@@ -49,7 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/profile_dictionary.h>
 #include <tesseract_command_language/utils.h>
 
-#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
+#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
 #include <tesseract_motion_planners/ompl/ompl_planner_configurator.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/core/utils.h>
@@ -197,11 +197,11 @@ bool FreespaceHybridExample::run()
   auto profiles = std::make_shared<ProfileDictionary>();
 
   // Create OMPL Profile
-  auto ompl_profile = std::make_shared<OMPLDefaultPlanProfile>();
+  auto ompl_profile = std::make_shared<OMPLRealVectorPlanProfile>();
   auto ompl_planner_config = std::make_shared<RRTConnectConfigurator>();
   ompl_planner_config->range = range_;
-  ompl_profile->planning_time = planning_time_;
-  ompl_profile->planners = { ompl_planner_config, ompl_planner_config };
+  ompl_profile->solver_config.planning_time = planning_time_;
+  ompl_profile->solver_config.planners = { ompl_planner_config, ompl_planner_config };
 
   profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile);
 

tesseract_examples/src/freespace_ompl_example.cpp

@@ -48,7 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/move_instruction.h>
 #include <tesseract_command_language/utils.h>
 
-#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
+#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
 #include <tesseract_motion_planners/ompl/ompl_planner_configurator.h>
 #include <tesseract_motion_planners/core/utils.h>
 
@@ -180,11 +180,11 @@ bool FreespaceOMPLExample::run()
   auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
 
   // Create OMPL Profile
-  auto ompl_profile = std::make_shared<OMPLDefaultPlanProfile>();
+  auto ompl_profile = std::make_shared<OMPLRealVectorPlanProfile>();
   auto ompl_planner_config = std::make_shared<RRTConnectConfigurator>();
   ompl_planner_config->range = range_;
-  ompl_profile->planning_time = planning_time_;
-  ompl_profile->planners = { ompl_planner_config, ompl_planner_config };
+  ompl_profile->solver_config.planning_time = planning_time_;
+  ompl_profile->solver_config.planners = { ompl_planner_config, ompl_planner_config };
 
   // Create profile dictionary
   auto profiles = std::make_shared<ProfileDictionary>();

tesseract_motion_planners/examples/freespace_example.cpp

@@ -34,7 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_environment/environment.h>
 
-#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
+#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
 #include <tesseract_motion_planners/ompl/ompl_motion_planner.h>
 
 #include <tesseract_motion_planners/trajopt/trajopt_motion_planner.h>
@@ -114,7 +114,7 @@ int main(int /*argc*/, char** /*argv*/)
     }
 
     // Create Profiles
-    auto ompl_plan_profile = std::make_shared<OMPLDefaultPlanProfile>();
+    auto ompl_plan_profile = std::make_shared<OMPLRealVectorPlanProfile>();
     auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
     auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
     auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();

tesseract_motion_planners/ompl/CMakeLists.txt

@@ -4,19 +4,17 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS})
 
 # OMPL Freespace Planning Interface
 set(OMPL_SRC
-    src/ompl_motion_planner.cpp
+    src/profile/ompl_profile.cpp
+    src/profile/ompl_real_vector_plan_profile.cpp
+    src/compound_state_validator.cpp
     src/continuous_motion_validator.cpp
     src/discrete_motion_validator.cpp
-    src/weighted_real_vector_state_sampler.cpp
+    src/ompl_motion_planner.cpp
     src/ompl_planner_configurator.cpp
-    src/ompl_problem.cpp
-    src/profile/ompl_profile.cpp
-    src/profile/ompl_default_plan_profile.cpp
-    src/utils.cpp
+    src/ompl_solver_config.cpp
     src/state_collision_validator.cpp
-    src/compound_state_validator.cpp
-    src/serialize.cpp
-    src/deserialize.cpp)
+    src/utils.cpp
+    src/weighted_real_vector_state_sampler.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/fwd.h

@@ -33,9 +33,8 @@ struct PRMstarConfigurator;
 struct LazyPRMstarConfigurator;
 struct SPARSConfigurator;
 
-// ompl_problem.h
-enum class OMPLProblemStateSpace;
-struct OMPLProblem;
+// ompl_solver_config.h
+struct OMPLSolverConfig;
 
 // state_collision_validator.h
 class StateCollisionValidator;
@@ -45,7 +44,7 @@ class WeightedRealVectorStateSampler;
 
 // profile
 class OMPLPlanProfile;
-class OMPLDefaultPlanProfile;
+class OMPLRealVectorPlanProfile;
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_MOTION_PLANNERS_OMPL_FWD_H

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

@@ -33,25 +33,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/core/planner.h>
-
 #include <tesseract_common/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
-
-namespace ompl::tools
-{
-class ParallelPlan;
-}  // namespace ompl::tools
+#include <tesseract_common/eigen_types.h>
 
 namespace tesseract_planning
 {
-struct OMPLProblem;
-struct OMPLProblemConfig
-{
-  std::shared_ptr<OMPLProblem> problem;
-  boost::uuids::uuid start_uuid{};
-  boost::uuids::uuid end_uuid{};
-};
-
 /**
  * @brief This planner is intended to provide an easy to use interface to OMPL for freespace planning. It is made to
  * take a start and end point and automate the generation of the OMPL problem.
@@ -89,19 +76,25 @@ class OMPLMotionPlanner : public MotionPlanner
 
   std::unique_ptr<MotionPlanner> clone() const override;
 
-  virtual std::vector<OMPLProblemConfig> createProblems(const PlannerRequest& request) const;
-
 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;
+  /**
+   * @brief This a utility function for assigning the trajectory to the results data structure
+   * @param output The results data structure to update
+   * @param start_uuid The start uuid of the provided trajectory
+   * @param end_uuid The end uuid of the provided trajectory
+   * @param start_index The start index to begin search for start uuid
+   * @param joint_names The joint names
+   * @param traj The provided trajectory
+   * @param format_result_as_input Indicate if the result should be formated as input
+   * @return The start index for the next segment
+   */
+  static long assignTrajectory(tesseract_planning::CompositeInstruction& output,
+                               boost::uuids::uuid start_uuid,
+                               boost::uuids::uuid end_uuid,
+                               long start_index,
+                               const std::vector<std::string>& joint_names,
+                               const tesseract_common::TrajArray& traj,
+                               bool format_result_as_input);
 };
 
 }  // namespace tesseract_planning