From 413bbee86b718040d259fbced7cc5801038a42f0 Mon Sep 17 00:00:00 2001
From: Antoine <antoine.duplex@free.fr>
Date: Tue, 9 Aug 2022 18:15:03 +0200
Subject: [PATCH] Loading OMPL's optimization objectives at launch in order to
 reduce the number of call to PluginLib

---
 .../model_based_planning_context.h            | 10 ++++
 .../src/model_based_planning_context.cpp      | 52 +++++++++++--------
 2 files changed, 41 insertions(+), 21 deletions(-)

diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h
index c166d3168a3..ebffb376af1 100644
--- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h
+++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h
@@ -47,6 +47,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;
@@ -388,6 +391,10 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext
   void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc);
   void unregisterTerminationCondition();
 
+  /** \brief Construct the map with all the declared optimization objective plugin
+   */
+  void constructOptimizationObjectives();
+
   ModelBasedPlanningContextSpecification spec_;
 
   moveit::core::RobotState complete_initial_robot_state_;
@@ -450,5 +457,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
diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
index f6bf4fbc2c7..094e7da00e1 100644
--- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
+++ b/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());
 
     }
 
@@ -1008,3 +998,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());
+    }
+  }
+}