Cleanup OMPL's PlanningContextManager's protected API

rhaschke · v4hn · commit c07be63b6cd5 · 2022-04-02T22:48:49.000+02:00
- Calling the factory_selector lambda causes the returned value to be NULL.
  There is no value in having this selector function. We could directly pass the correct factory.
- MotionPlanRequest isn't actually used.
diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
@@ -206,8 +206,6 @@ class PlanningContextManager
   ConfiguredPlannerSelector getPlannerSelector() const;
 
 protected:
-  typedef std::function<const ModelBasedStateSpaceFactoryPtr&(const std::string&)> StateSpaceFactoryTypeSelector;
-
   ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const;
 
   void registerDefaultPlanners();
@@ -218,8 +216,7 @@ class PlanningContextManager
 
   /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */
   ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config,
-                                                  const StateSpaceFactoryTypeSelector& factory_selector,
-                                                  const moveit_msgs::MotionPlanRequest& req) const;
+                                                  const ModelBasedStateSpaceFactoryPtr& factory) const;
 
   const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& factory_type) const;
   const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& group_name,
diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
@@ -310,11 +310,8 @@ void ompl_interface::PlanningContextManager::setPlannerConfigurations(
 }
 
 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextManager::getPlanningContext(
-    const planning_interface::PlannerConfigurationSettings& config,
-    const StateSpaceFactoryTypeSelector& factory_selector, const moveit_msgs::MotionPlanRequest& /*req*/) const
+    const planning_interface::PlannerConfigurationSettings& config, const ModelBasedStateSpaceFactoryPtr& factory) const
 {
-  const ompl_interface::ModelBasedStateSpaceFactoryPtr& factory = factory_selector(config.group);
-
   // Check for a cached planning context
   ModelBasedPlanningContextPtr context;
 
@@ -461,17 +458,15 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
   // However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped,
   // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling
   // in JointModelStateSpace.
-  StateSpaceFactoryTypeSelector factory_selector;
+  ModelBasedStateSpaceFactoryPtr factory;
   auto it = pc->second.config.find("enforce_joint_model_state_space");
 
   if (it != pc->second.config.end() && boost::lexical_cast<bool>(it->second))
-    factory_selector = [this](const std::string&) {
-      return getStateSpaceFactory(JointModelStateSpace::PARAMETERIZATION_TYPE);
-    };
+    factory = getStateSpaceFactory(JointModelStateSpace::PARAMETERIZATION_TYPE);
   else
-    factory_selector = [this, &req](const std::string& group) { return getStateSpaceFactory(group, req); };
+    factory = getStateSpaceFactory(pc->second.group, req);
 
-  ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory_selector, req);
+  ModelBasedPlanningContextPtr context = getPlanningContext(pc->second, factory);
 
   if (context)
   {
