Loading OMPL's optimization objectives at launch in order to reduce t…

…he number of call to PluginLib

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

@@ -46,6 +46,9 @@
 #include <ompl/base/StateStorage.h>
 #include <ompl/base/spaces/constraint/ConstrainedStateSpace.h>
 
+#include <moveit/ompl_interface/ompl_optimization_objective_loader.h>
+#include <pluginlib/class_loader.hpp>
+
 namespace ompl_interface
 {
 namespace ob = ompl::base;
@@ -390,6 +393,10 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext
   /** \brief Convert OMPL PlannerStatus to moveit_msgs::msg::MoveItErrorCode */
   int32_t logPlannerStatus(og::SimpleSetupPtr ompl_simple_setup);
 
+  /** \brief Construct the map with all the declared optimization objective plugin
+   */
+  void constructOptimizationObjectives();
+
   ModelBasedPlanningContextSpecification spec_;
 
   moveit::core::RobotState complete_initial_robot_state_;
@@ -452,5 +459,8 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext
 
   // if false parallel plan returns the first solution found
   bool hybridize_;
+
+  /// map of all the optimization objectives plugins
+  std::map<std::string, std::shared_ptr<ompl_optimization_loader::OptimizationObjectiveLoader>> optimization_objectives_;
 };
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -67,9 +67,6 @@
 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
 #include <ompl/geometric/planners/prm/LazyPRM.h>
 
-#include <moveit/ompl_interface/ompl_optimization_objective_loader.h>
-#include <pluginlib/class_loader.hpp>
-
 
 namespace ompl_interface
 {
@@ -102,6 +99,8 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::
   complete_initial_robot_state_.update();
 
   constraints_library_ = std::make_shared<ConstraintsLibrary>(this);
+
+  constructOptimizationObjectives();
 }
 
 void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::SharedPtr& node,
@@ -352,27 +351,18 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
       objective =
           std::make_shared<ompl::base::MaximizeMinClearanceObjective>(ompl_simple_setup_->getSpaceInformation());
     }
+    else if (optimization_objectives_.find(optimizer) != optimization_objectives_.end())
+    {
+      objective =
+              optimization_objectives_[optimizer]->getOptimizationObjective(ompl_simple_setup_->getSpaceInformation());
+    }
     else
     {
-        pluginlib::ClassLoader<ompl_optimization_loader::OptimizationObjectiveLoader> poly_loader("moveit_core", "ompl_optimization_loader::OptimizationObjectiveLoader");
-
-        try
-        {
-            RCLCPP_DEBUG(LOGGER, "Using optimization objective : %s", optimizer.c_str());
-            std::shared_ptr<ompl_optimization_loader::OptimizationObjectiveLoader> obj = poly_loader.createSharedInstance(optimizer);
+      RCLCPP_ERROR(LOGGER, "Unknown optimization objective for OMPL planner");
+      RCLCPP_ERROR(LOGGER, "Fall back to the default optimizer : PathLengthOptimizationObjective");
 
-            objective = obj->getOptimizationObjective(ompl_simple_setup_->getSpaceInformation());
-
-        }
-        catch(pluginlib::PluginlibException& ex)
-        {
-            RCLCPP_ERROR(LOGGER, "The plugin failed to load for some reason. Error: %s",  ex.what());
-            RCLCPP_ERROR(LOGGER, "Fall back to the default optimizer");
-
-            objective =
-                    std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
-
-        }
+      objective =
+              std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
 
     }
 
@@ -1074,3 +1064,23 @@ bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(con
   }
   return false;
 }
+
+void ompl_interface::ModelBasedPlanningContext::constructOptimizationObjectives()
+{
+  auto optimization_objective_loader = pluginlib::ClassLoader<ompl_optimization_loader::OptimizationObjectiveLoader>("moveit_core", "ompl_optimization_loader::OptimizationObjectiveLoader");
+
+  for (const auto& plugin_name : optimization_objective_loader.getDeclaredClasses())
+  {
+    try
+    {
+      RCLCPP_DEBUG(LOGGER, "Loading optimization objective : %s", plugin_name.c_str());
+      std::shared_ptr<ompl_optimization_loader::OptimizationObjectiveLoader> obj_loader = optimization_objective_loader.createSharedInstance(plugin_name);
+
+      optimization_objectives_[plugin_name] = obj_loader;
+    }
+    catch(pluginlib::PluginlibException& ex)
+    {
+      RCLCPP_ERROR(LOGGER, "The plugin %s failed to load for some reason. Error: %s", plugin_name.c_str(), ex.what());
+    }
+  }
+}