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_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>
@@ -102,28 +104,19 @@ 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);
       }
     }
     // 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);
@@ -191,15 +184,18 @@ class PlanningComponent
   planning_interface::MotionPlanResponse plan();
   /** \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_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters,
+                                              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 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. */
@@ -208,6 +204,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_;
@@ -222,7 +227,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_;
 }