Allowing users to load and change dynamically new optimization objective for OMPL's planners

moveit_planners/ompl/CMakeLists.txt

@@ -34,7 +34,7 @@ install(TARGETS moveit_ompl_interface moveit_ompl_planner_plugin
   INCLUDES DESTINATION include/moveit_planners_ompl
 )
 ament_export_targets(moveit_planners_omplTargets HAS_LIBRARY_TARGET)
-ament_export_dependencies(moveit_core ompl)
+ament_export_dependencies(moveit_core ompl moveit_ros_planning)
 
 pluginlib_export_plugin_description_file(moveit_core ompl_interface_plugin_description.xml)
 

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(const 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/include/moveit/ompl_interface/ompl_optimization_objective_loader.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Antoine Duplex */
+
+#ifndef SRC_OMPL_OPTIMIZATION_OBJECTIVE_LOADER_H
+#define SRC_OMPL_OPTIMIZATION_OBJECTIVE_LOADER_H
+
+#include <ompl/base/OptimizationObjective.h>
+#include <ompl/base/SpaceInformation.h>
+
+#include <string>
+#include <iostream>
+
+#define MOVEIT_OPTIMIZATION_OBJECTIVE_PLUGIN(my_namespace, OptimizationObjective)                                      \
+  namespace my_namespace                                                                                               \
+  {                                                                                                                    \
+  class OptimizationObjective##Loader : public ompl_optimization_loader::OptimizationObjectiveLoader                   \
+  {                                                                                                                    \
+  public:                                                                                                              \
+    ompl::base::OptimizationObjectivePtr getOptimizationObjective(ompl::base::SpaceInformationPtr si) override         \
+    {                                                                                                                  \
+      return std::make_shared<OptimizationObjective>(si);                                                              \
+    }                                                                                                                  \
+  };                                                                                                                   \
+  PLUGINLIB_EXPORT_CLASS(my_namespace::OptimizationObjective##Loader,                                                  \
+                         ompl_optimization_loader::OptimizationObjectiveLoader)                                        \
+  }
+
+namespace ompl_optimization_loader
+{
+class OptimizationObjectiveLoader
+{
+public:
+  OptimizationObjectiveLoader()
+  {
+  }
+  ~OptimizationObjectiveLoader() = default;
+
+  virtual ompl::base::OptimizationObjectivePtr getOptimizationObjective(ompl::base::SpaceInformationPtr si) = 0;
+};
+}  // namespace ompl_optimization_loader
+
+#endif  // SRC_OMPL_OPTIMIZATION_OBJECTIVE_LOADER_H

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -98,6 +98,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,
@@ -354,8 +356,16 @@ 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
     {
+      RCLCPP_ERROR(LOGGER, "Unknown optimization objective for OMPL planner");
+      RCLCPP_ERROR(LOGGER, "Fall back to the default optimizer : PathLengthOptimizationObjective");
+
       objective =
           std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
     }
@@ -1054,3 +1064,25 @@ 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());
+    }
+  }
+}