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 9c39bca6ea..022266bafc 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 @@ -46,6 +46,9 @@ #include #include +#include +#include + 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> 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 f696786592..3a0c72499b 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 -#include -#include - namespace ompl_interface { @@ -102,6 +99,8 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: complete_initial_robot_state_.update(); constraints_library_ = std::make_shared(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_simple_setup_->getSpaceInformation()); } + else if (optimization_objectives_.find(optimizer) != optimization_objectives_.end()) + { + objective = + optimization_objectives_[optimizer]->getOptimizationObjective(ompl_simple_setup_->getSpaceInformation()); + } else { - pluginlib::ClassLoader poly_loader("moveit_core", "ompl_optimization_loader::OptimizationObjectiveLoader"); - - try - { - RCLCPP_DEBUG(LOGGER, "Using optimization objective : %s", optimizer.c_str()); - std::shared_ptr 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_simple_setup_->getSpaceInformation()); - - } + objective = + std::make_shared(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("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 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()); + } + } +}