Update stomp planning interface

moveit · May 17, 2023 · 20c9e39 · 20c9e39
1 parent 6fe8af4
commit 20c9e39
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h
@@ -75,8 +75,7 @@ struct PlannerConfigurationSettings
 /** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */
 typedef std::map<std::string, PlannerConfigurationSettings> PlannerConfigurationMap;
 
-using StateCostFn = std::function<double(const moveit::core::RobotState& robot_state, const MotionPlanRequest& request,
-                                         const planning_scene::PlanningSceneConstPtr& planning_scene)>;
+using StateCostFn = std::function<double(const std::vector_double& state_vector)>;
 
 MOVEIT_CLASS_FORWARD(PlanningContext);  // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc
 

diff --git a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp
@@ -72,11 +72,12 @@ class StompSmoothingAdapter : public planning_request_adapter::PlanningRequestAd
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps,
                     const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    std::vector<std::size_t>& /*added_path_index*/,
+                    const planning_interface::StateCostFn& state_cost_function) const override
   {
     // Following call to planner() calls the motion planner defined for the pipeline and stores the trajectory inside
     // the MotionPlanResponse res variable which is then passed to STOMP for optimization
-    if (!planner(ps, req, res))
+    if (!planner(ps, req, res, state_cost_function))
     {
       return false;
     }