Extract parallel planning from moveit cpp

moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp

@@ -45,7 +45,7 @@ planning_interface::MotionPlanResponse
 plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
      std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>& single_plan_parameters,
      std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>& multi_plan_parameters,
-     std::optional<const moveit_cpp::PlanningComponent::SolutionCallbackFunction> solution_selection_callback,
+     std::optional<const moveit_cpp::PlanningComponent::SolutionCallbackFunction> solution_selection_function,
      std::optional<moveit_cpp::PlanningComponent::StoppingCriterionFunction> stopping_criterion_callback)
 {
   // parameter argument checking
@@ -70,14 +70,14 @@ plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
         std::const_pointer_cast<const moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>(
             multi_plan_parameters);
 
-    if (solution_selection_callback && stopping_criterion_callback)
+    if (solution_selection_function && stopping_criterion_callback)
     {
-      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback),
+      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function),
                                       *stopping_criterion_callback);
     }
-    else if (solution_selection_callback)
+    else if (solution_selection_function)
     {
-      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback));
+      return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function));
     }
     else if (stopping_criterion_callback)
     {
@@ -320,7 +320,7 @@ void init_planning_component(py::module& m)
 
       // TODO (peterdavidfagan): improve the plan API
       .def("plan", &moveit_py::bind_planning_component::plan, py::arg("single_plan_parameters") = nullptr,
-           py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_callback") = nullptr,
+           py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_function") = nullptr,
            py::arg("stopping_criterion_callback") = nullptr, py::return_value_policy::move,
            R"(
       	   Plan a motion plan using the current start and goal states.

moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h

@@ -63,7 +63,7 @@ planning_interface::MotionPlanResponse
 plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
      std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>& single_plan_parameters,
      std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>& multi_plan_parameters,
-     std::optional<const moveit_cpp::PlanningComponent::SolutionCallbackFunction> solution_selection_callback,
+     std::optional<const moveit_cpp::PlanningComponent::SolutionCallbackFunction> solution_selection_function,
      std::optional<moveit_cpp::PlanningComponent::StoppingCriterionFunction> stopping_criterion_callback);
 
 bool set_goal(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,

moveit_ros/planning/CMakeLists.txt

@@ -33,6 +33,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     robot_model_loader/include
     constraint_sampler_manager_loader/include
     planning_pipeline/include
+    planning_pipeline_interfaces/include
     planning_scene_monitor/include
     trajectory_execution_manager/include
     plan_execution/include
@@ -46,6 +47,7 @@ set(THIS_PACKAGE_LIBRARIES
   moveit_robot_model_loader
   moveit_constraint_sampler_manager_loader
   moveit_planning_pipeline
+  moveit_planning_pipeline_interfaces
   moveit_trajectory_execution_manager
   moveit_plan_execution
   moveit_planning_scene_monitor
@@ -83,6 +85,7 @@ add_subdirectory(kinematics_plugin_loader)
 add_subdirectory(robot_model_loader)
 add_subdirectory(constraint_sampler_manager_loader)
 add_subdirectory(planning_pipeline)
+add_subdirectory(planning_pipeline_interfaces)
 add_subdirectory(planning_request_adapter_plugins)
 add_subdirectory(planning_scene_monitor)
 add_subdirectory(planning_components_tools)

moveit_ros/planning/moveit_cpp/CMakeLists.txt

@@ -1,6 +1,5 @@
 add_library(moveit_cpp SHARED
   src/moveit_cpp.cpp
-  src/parallel_planning_callbacks.cpp
   src/planning_component.cpp
 )
 set_target_properties(moveit_cpp PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
@@ -11,6 +10,7 @@ ament_target_dependencies(moveit_cpp
 target_link_libraries(moveit_cpp
   moveit_planning_scene_monitor
   moveit_planning_pipeline
+  moveit_planning_pipeline_interfaces
   moveit_trajectory_execution_manager)
 
 install(DIRECTORY include/ DESTINATION include/moveit_ros_planning)

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

@@ -39,7 +39,9 @@
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/moveit_cpp/plan_solutions.hpp>
+#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>
 #include <moveit/planning_interface/planning_response.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit/robot_state/robot_state.h>
@@ -106,13 +108,13 @@ class PlanningComponent
     MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node,
                                        const std::vector<std::string>& planning_pipeline_names)
     {
-      multi_plan_request_parameters.reserve(planning_pipeline_names.size());
+      plan_request_parameter_vector.reserve(planning_pipeline_names.size());
 
       for (const auto& planning_pipeline_name : planning_pipeline_names)
       {
         PlanRequestParameters parameters;
         parameters.load(node, planning_pipeline_name);
-        multi_plan_request_parameters.push_back(parameters);
+        plan_request_parameter_vector.push_back(parameters);
       }
     }
 
@@ -122,18 +124,9 @@ class PlanningComponent
     }
 
     // Plan request parameters for the individual planning pipelines which run concurrently
-    std::vector<PlanRequestParameters> multi_plan_request_parameters;
+    std::vector<PlanRequestParameters> plan_request_parameter_vector;
   };
 
-  /** \brief A solution callback function type for the parallel planning API of planning component  */
-  typedef std::function<planning_interface::MotionPlanResponse(
-      const std::vector<planning_interface::MotionPlanResponse>& solutions)>
-      SolutionCallbackFunction;
-  /** \brief A stopping criterion callback function for the parallel planning API of planning component */
-  typedef std::function<bool(const PlanSolutions& solutions,
-                             const MultiPipelinePlanRequestParameters& plan_request_parameters)>
-      StoppingCriterionFunction;
-
   /** \brief Constructor */
   PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node);
   PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp);
@@ -202,16 +195,17 @@ class PlanningComponent
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
    * provided PlanRequestParameters. */
   planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters,
-                                              planning_scene::PlanningScenePtr const_planning_scene = nullptr);
+                                              planning_scene::PlanningScenePtr planning_scene = nullptr);
 
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
    * provided PlanRequestParameters. This defaults to taking the full planning time (null stopping_criterion_callback)
    * and finding the shortest solution in joint space. */
   planning_interface::MotionPlanResponse
   plan(const MultiPipelinePlanRequestParameters& parameters,
-       const SolutionCallbackFunction& solution_selection_callback = &getShortestSolution,
-       StoppingCriterionFunction stopping_criterion_callback = nullptr,
-       const planning_scene::PlanningScenePtr planning_scene = nullptr);
+       const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
+           &moveit::planning_pipeline_interfaces::getShortestSolution,
+       moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback = nullptr,
+       planning_scene::PlanningScenePtr planning_scene = nullptr);
 
   /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates
    * after the execution is complete. The execution can be run in background by setting blocking to false. */
@@ -220,6 +214,11 @@ class PlanningComponent
     return false;
   };
 
+  ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters);
+
+  std::vector<::planning_interface::MotionPlanRequest>
+  getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters);
+
 private:
   // Core properties and instances
   rclcpp::Node::SharedPtr node_;
@@ -234,7 +233,6 @@ class PlanningComponent
   std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;
   moveit_msgs::msg::Constraints current_path_constraints_;
   moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_;
-  PlanRequestParameters plan_request_parameters_;
   moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
   bool workspace_parameters_set_ = false;