Make accessible with moveit_cpp

moveit_core/CMakeLists.txt

@@ -117,6 +117,7 @@ install(
     moveit_collision_detection_fcl
     moveit_collision_distance_field
     moveit_constraint_samplers
+    moveit_cost_functions
     moveit_distance_field
     moveit_dynamics_solver
     moveit_exceptions

moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp

@@ -45,9 +45,9 @@ namespace moveit
 {
 namespace cost_functions
 {
-[[nodiscard]] planning_interface::StateCostFn getClearanceCostFn();
+[[nodiscard]] ::planning_interface::StateCostFn getClearanceCostFn();
 
-[[nodiscard]] planning_interface::StateCostFn
-getWeightedCostFnSum(std::vector<std::pair<double, planning_interface::StateCostFn>> weight_cost_vector);
+[[nodiscard]] ::planning_interface::StateCostFn
+getWeightedCostFnSum(std::vector<std::pair<double, ::planning_interface::StateCostFn>> weight_cost_vector);
 }  // namespace cost_functions
 }  // namespace moveit

moveit_core/cost_functions/src/cost_functions.cpp

@@ -40,7 +40,7 @@ namespace moveit
 {
 namespace cost_functions
 {
-[[nodiscard]] planning_interface::StateCostFn getClearanceCostFn()
+[[nodiscard]] ::planning_interface::StateCostFn getClearanceCostFn()
 {
   // Create cost function
   return [](const moveit::core::RobotState& robot_state, const planning_interface::MotionPlanRequest& request,
@@ -57,8 +57,8 @@ namespace cost_functions
   };
 }
 
-[[nodiscard]] planning_interface::StateCostFn
-getWeightedCostFnSum(std::vector<std::pair<double, planning_interface::StateCostFn>> weight_cost_vector)
+[[nodiscard]] ::planning_interface::StateCostFn
+getWeightedCostFnSum(std::vector<std::pair<double, ::planning_interface::StateCostFn>> weight_cost_vector)
 {
   return [weight_cost_vector](const moveit::core::RobotState& robot_state,
                               const planning_interface::MotionPlanRequest& request,

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -378,8 +378,11 @@ void ompl_interface::ModelBasedPlanningContext::useConfig(const planning_scene::
 
   if (state_cost_function_ != nullptr)
   {
+    RCLCPP_WARN(LOGGER, "%s: Use user defined optimization function!",
+                  name_.c_str());
     objective = std::make_shared<ompl_interface::PlanningInterfaceObjective>(ompl_simple_setup_->getSpaceInformation(),
                                                                              planning_scene, req, state_cost_function_);
+    ompl_simple_setup_->setOptimizationObjective(objective);
   }
 
   // Don't clear planner data if multi-query planning is enabled

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

@@ -39,6 +39,7 @@
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_interface/planning_response.h>
 #include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
 #include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
@@ -189,6 +190,13 @@ class PlanningComponent
   /** \brief Set the trajectory constraints generated from a moveit msg Constraints */
   bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints);
 
+  /** \brief Set planning cost function */
+  bool setStateCostFunction(const planning_interface::StateCostFn& state_cost_function)
+  {
+    state_cost_function_ = state_cost_function;
+    return true;
+  };
+
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using
    * default parameters. */
   planning_interface::MotionPlanResponse plan();
@@ -230,6 +238,7 @@ class PlanningComponent
   const std::string group_name_;
   // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources
   const moveit::core::JointModelGroup* joint_model_group_;
+  planning_interface::StateCostFn state_cost_function_;
 
   // Planning
   // The start state used in the planning motion request

moveit_ros/planning/moveit_cpp/src/planning_component.cpp

@@ -141,8 +141,8 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest
   planning_scene->setCurrentState(request.start_state);
 
   // Run planning attempt
-  return moveit::planning_pipeline_interfaces::planWithSinglePipeline(request, planning_scene,
-                                                                      moveit_cpp_->getPlanningPipelines());
+  return moveit::planning_pipeline_interfaces::planWithSinglePipeline(
+      request, planning_scene, moveit_cpp_->getPlanningPipelines(), state_cost_function_);
 }
 
 planning_interface::MotionPlanResponse PlanningComponent::plan(

moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp

@@ -40,6 +40,7 @@
 #include <moveit/planning_pipeline_interfaces/plan_responses_container.hpp>
 #include <moveit/planning_interface/planning_response.h>
 #include <moveit/planning_interface/planning_request.h>
+#include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/planning_scene/planning_scene.h>
 
@@ -73,7 +74,8 @@ typedef std::function<::planning_interface::MotionPlanResponse(
 ::planning_interface::MotionPlanResponse planWithSinglePipeline(
     const ::planning_interface::MotionPlanRequest& motion_plan_request,
     const ::planning_scene::PlanningSceneConstPtr& planning_scene,
-    const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines);
+    const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines,
+    const ::planning_interface::StateCostFn& state_cost_function = ::planning_interface::StateCostFn());
 
 /** \brief Function to solve multiple planning problems in parallel threads with multiple planning pipelines at the same
  time

moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp

@@ -47,7 +47,8 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.::planning_inter
 ::planning_interface::MotionPlanResponse
 planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_plan_request,
                        const ::planning_scene::PlanningSceneConstPtr& planning_scene,
-                       const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines)
+                       const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines,
+                       const ::planning_interface::StateCostFn& state_cost_function)
 {
   ::planning_interface::MotionPlanResponse motion_plan_response;
   auto it = planning_pipelines.find(motion_plan_request.pipeline_id);
@@ -58,7 +59,7 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla
     return motion_plan_response;
   }
   const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
-  pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response);
+  pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response, state_cost_function);
   return motion_plan_response;
 }