Extract parallel planning from moveit cpp (moveit#2043)

* Add parallel_planning_interface

* Add parallel planning interface

* Rename package to pipeline_planning_interface

* Move plan_responses_container into own header + source file

* Add plan_responses_contrainer source file

* Add solution selection and stopping criterion function files

* Remove parallel planning from moveit_cpp

* Move parallel planning into planning package

* Update moveit_cpp

* Drop planning_interface changes

* Add documentation

* Update other moveit packages

* Remove removed header

* Address CI complains

* Address clang-tidy complains

* Address clang-tidy complains 2

* Address clang-tidy complains 3

* Extract planning pipeline map creation function from moveit_cpp

* Cleanup comment

* Use const moveit::core::RobotModelConstPtr&

* Formatting

* Add header descriptions

* Remove superfluous TODOs

* Cleanup

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

@@ -45,8 +45,8 @@ 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<moveit_cpp::PlanningComponent::StoppingCriterionFunction> stopping_criterion_callback)
+     std::optional<const moveit::planning_pipeline_interfaces::SolutionSelectionFunction> solution_selection_function,
+     std::optional<moveit::planning_pipeline_interfaces::StoppingCriterionFunction> stopping_criterion_callback)
 {
   // parameter argument checking
   if (single_plan_parameters && multi_plan_parameters)
@@ -70,18 +70,19 @@ 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)
     {
-      return planning_component->plan(*const_multi_plan_parameters, moveit_cpp::getShortestSolution,
+      return planning_component->plan(*const_multi_plan_parameters,
+                                      moveit::planning_pipeline_interfaces::getShortestSolution,
                                       *stopping_criterion_callback);
     }
     else
@@ -241,7 +242,7 @@ void init_multi_plan_request_parameters(py::module& m)
         return params;
       }))
       .def_readonly("multi_plan_request_parameters",
-                    &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::multi_plan_request_parameters);
+                    &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::plan_request_parameter_vector);
 }
 void init_planning_component(py::module& m)
 {
@@ -320,7 +321,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

@@ -45,7 +45,6 @@
 #include <moveit_py/moveit_py_utils/ros_msg_typecasters.h>
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit/moveit_cpp/planning_component.h>
-#include <moveit/moveit_cpp/plan_solutions.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <moveit_msgs/msg/constraints.hpp>
 
@@ -63,8 +62,8 @@ 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<moveit_cpp::PlanningComponent::StoppingCriterionFunction> stopping_criterion_callback);
+     std::optional<const moveit::planning_pipeline_interfaces::SolutionSelectionFunction> solution_selection_function,
+     std::optional<moveit::planning_pipeline_interfaces::StoppingCriterionFunction> stopping_criterion_callback);
 
 bool set_goal(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
               std::optional<std::string> configuration_name, std::optional<moveit::core::RobotState> robot_state,

moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

@@ -934,7 +934,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request
           .max_velocity_scaling_factor = request.max_velocity_scaling_factor,
           .max_acceleration_scaling_factor = request.max_acceleration_scaling_factor
         };
-        multi_pipeline_plan_request.multi_plan_request_parameters.push_back(plan_req_params);
+        multi_pipeline_plan_request.plan_request_parameter_vector.push_back(plan_req_params);
       }
 
       // Iterate runs
@@ -960,7 +960,8 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request
         std::chrono::system_clock::time_point start = std::chrono::system_clock::now();
 
         auto const t1 = std::chrono::system_clock::now();
-        auto const response = planning_component->plan(multi_pipeline_plan_request, &moveit_cpp::getShortestSolution,
+        auto const response = planning_component->plan(multi_pipeline_plan_request,
+                                                       &moveit::planning_pipeline_interfaces::getShortestSolution,
                                                        nullptr, planning_scene_);
         auto const t2 = std::chrono::system_clock::now();
 

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/moveit_cpp.h

@@ -150,7 +150,7 @@ class MoveItCpp
   moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0);
 
   /** \brief Get all loaded planning pipeline instances mapped to their reference names */
-  const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
+  const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
 
   /** \brief Get the stored instance of the planning scene monitor */
   const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const;
@@ -189,8 +189,8 @@ class MoveItCpp
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
   // Planning
-  std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
-  std::map<std::string, std::set<std::string>> groups_algorithms_map_;
+  std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
+  std::unordered_map<std::string, std::set<std::string>> groups_algorithms_map_;
 
   // Execution
   trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;

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,
+       const 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,15 @@ class PlanningComponent
     return false;
   };
 
+  /** \brief Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the
+   * PlanningComponent instance */
+  ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters);
+
+  /** \brief Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the
+   * internal state of the PlanningComponent instance */
+  std::vector<::planning_interface::MotionPlanRequest>
+  getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters);
+
 private:
   // Core properties and instances
   rclcpp::Node::SharedPtr node_;
@@ -234,7 +237,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;
 

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -37,14 +37,14 @@
 #include <stdexcept>
 
 #include <moveit/controller_manager/controller_manager.h>
+#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <geometry_msgs/msg/quaternion_stamped.hpp>
 
 namespace moveit_cpp
 {
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.moveit_cpp");
-constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin";
 
 MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Options(node))
 {
@@ -129,34 +129,13 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti
 bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
 {
   // TODO(henningkayser): Use parent namespace for planning pipeline config lookup
-  // ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace);
-  for (const auto& planning_pipeline_name : options.pipeline_names)
-  {
-    if (planning_pipelines_.count(planning_pipeline_name) > 0)
-    {
-      RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
-      continue;
-    }
-    RCLCPP_INFO(LOGGER, "Loading planning pipeline '%s'", planning_pipeline_name.c_str());
-    planning_pipeline::PlanningPipelinePtr pipeline;
-    pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, node_, planning_pipeline_name,
-                                                                     PLANNING_PLUGIN_PARAM);
-
-    if (!pipeline->getPlannerManager())
-    {
-      RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
-      continue;
-    }
-
-    planning_pipelines_[planning_pipeline_name] = pipeline;
-  }
-
+  planning_pipelines_ =
+      moveit::planning_pipeline_interfaces::createPlanningPipelineMap(options.pipeline_names, robot_model_, node_);
   if (planning_pipelines_.empty())
   {
     RCLCPP_ERROR(LOGGER, "Failed to load any planning pipelines.");
     return false;
   }
-
   return true;
 }
 
@@ -191,7 +170,7 @@ moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait)
   return current_state;
 }
 
-const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::getPlanningPipelines() const
+const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::getPlanningPipelines() const
 {
   return planning_pipelines_;
 }