simplify_solution per planning context (#1437)

* Allowing to dynamically change the parameter simplify_solutions

* Delete this configuration because it overrides the configuration loaded

The parameters simplify_solutions is passed to the context trough the configuration of each planner but this function overrides it and seems to be contradictory to rest of the implementation. simplify_solutions shouldn't be considered as the rest of the other parameters, like interpolate or hybridize ?

* Remove simplify_solutions_ from OMPL interface and all its setter/getter

* Clean-up code without ConfigureContext and unneeded code related to simplify_solution

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h

@@ -115,15 +115,6 @@ class OMPLInterface
   {
     return use_constraints_approximations_;
   }
-  bool simplifySolutions() const
-  {
-    return simplify_solutions_;
-  }
-
-  void simplifySolutions(bool flag)
-  {
-    simplify_solutions_ = flag;
-  }
 
   /** @brief Print the status of this node*/
   void printStatus();
@@ -140,8 +131,6 @@ class OMPLInterface
   /** @brief Load the additional plugins for sampling constraints */
   void loadConstraintSamplers();
 
-  void configureContext(const ModelBasedPlanningContextPtr& context) const;
-
   /** \brief Configure the OMPL planning context for a new planning request */
   ModelBasedPlanningContextPtr prepareForSolve(const planning_interface::MotionPlanRequest& req,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
@@ -160,8 +149,6 @@ class OMPLInterface
 
   bool use_constraints_approximations_;
 
-  bool simplify_solutions_;
-
 private:
   constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
 };

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -372,6 +372,14 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
     cfg.erase(it);
   }
 
+  // check whether the path returned by the planner should be simplified
+  it = cfg.find("simplify_solutions");
+  if (it != cfg.end())
+  {
+    simplify_solutions_ = boost::lexical_cast<bool>(it->second);
+    cfg.erase(it);
+  }
+
   // check whether solution paths from parallel planning should be hybridized
   it = cfg.find("hybridize");
   if (it != cfg.end())

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

@@ -52,7 +52,6 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   , constraint_sampler_manager_(std::make_shared<constraint_samplers::ConstraintSamplerManager>())
   , context_manager_(robot_model, constraint_sampler_manager_)
   , use_constraints_approximations_(true)
-  , simplify_solutions_(true)
 {
   RCLCPP_DEBUG(LOGGER, "Initializing OMPL interface using ROS parameters");
   loadPlannerConfigurations();
@@ -68,7 +67,6 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   , constraint_sampler_manager_(std::make_shared<constraint_samplers::ConstraintSamplerManager>())
   , context_manager_(robot_model, constraint_sampler_manager_)
   , use_constraints_approximations_(true)
-  , simplify_solutions_(true)
 {
   RCLCPP_DEBUG(LOGGER, "Initializing OMPL interface using specified configuration");
   setPlannerConfigurations(pconfig);
@@ -110,18 +108,9 @@ OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& p
 {
   ModelBasedPlanningContextPtr ctx =
       context_manager_.getPlanningContext(planning_scene, req, error_code, node_, use_constraints_approximations_);
-  if (ctx)
-  {
-    configureContext(ctx);
-  }
   return ctx;
 }
 
-void OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const
-{
-  context->simplifySolutions(simplify_solutions_);
-}
-
 void OMPLInterface::loadConstraintSamplers()
 {
   constraint_sampler_manager_loader_ =