Enable parallel planning with PipelinePlanner moveit#450

commit 8e2c560
Author: Sebastian Jahr <[email protected]>
Date:   Mon Apr 17 09:49:51 2023 +0200

    Use no default stopping criteria

commit d095f7f
Author: Sebastian Jahr <[email protected]>
Date:   Fri Apr 14 12:20:44 2023 +0200

    Refactor to avoid calling .at(0) twice

commit 4238dc3
Author: Sebastian Jahr <[email protected]>
Date:   Fri Apr 14 12:10:45 2023 +0200

    Format

commit 2c74526
Author: Sebastian Jahr <[email protected]>
Date:   Fri Apr 14 10:39:26 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

commit f06a93c
Author: Sebastian Jahr <[email protected]>
Date:   Fri Apr 14 10:34:02 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

    Co-authored-by: Sebastian Castro <[email protected]>

commit 168cbd5
Author: Sebastian Jahr <[email protected]>
Date:   Tue Apr 11 11:14:19 2023 +0200

    Small clang-tidy fix

commit c0ea5cd
Author: Sebastian Jahr <[email protected]>
Date:   Thu Apr 6 10:34:20 2023 +0200

    Pass pipeline map by reference

commit 9be2ab3
Author: Sebastian Jahr <[email protected]>
Date:   Fri Mar 31 17:55:58 2023 +0200

    Add API to set parallel planning callbacks and deprecate functions

commit 132235f
Author: Sebastian Jahr <[email protected]>
Date:   Thu Mar 30 20:18:50 2023 +0200

    Cleanup and documentation

commit 7dd2743
Author: Sebastian Jahr <[email protected]>
Date:   Thu Mar 30 17:12:44 2023 +0200

    Add callbacks

commit f050adb
Author: Sebastian Jahr <[email protected]>
Date:   Thu Mar 30 16:00:25 2023 +0200

    Enable configuring multiple pipelines

commit b96992c
Author: Sebastian Jahr <[email protected]>
Date:   Thu Mar 30 10:29:47 2023 +0200

    Make usable with parallel planning

commit 2a7234d
Author: Sebastian Jahr <[email protected]>
Date:   Thu Mar 30 09:47:38 2023 +0200

    Re-order plan functions

commit 6679c98
Author: Sebastian Jahr <[email protected]>
Date:   Tue Mar 28 15:28:02 2023 +0200

    Make code readable

Author: sjahr

core/include/moveit/task_constructor/solvers/pipeline_planner.h

@@ -32,13 +32,16 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Authors: Robert Haschke
-   Desc:    plan using MoveIt's PlanningPipeline
+/* Authors: Robert Haschke, Sebastian Jahr
+   Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
 */
 
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
+#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
+#include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
+#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
 #include <rclcpp/node.hpp>
 #include <moveit/macros/class_forward.h>
 
@@ -56,48 +59,132 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
 class PipelinePlanner : public PlannerInterface
 {
 public:
-	struct Specification
-	{
-		moveit::core::RobotModelConstPtr model;
-		std::string ns{ "ompl" };
-		std::string pipeline{ "ompl" };
-		std::string adapter_param{ "request_adapters" };
-	};
-
-	static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
-	                                                     const moveit::core::RobotModelConstPtr& model) {
-		Specification spec;
-		spec.model = model;
-		return create(node, spec);
+	/** Simple Constructor to use only a single pipeline
+	 * \param [in] node ROS 2 node
+	 * \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the
+	 * parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning
+	 */
+	PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
+	                const std::string& planner_id = "");
+
+	[[deprecated("Deprecated: Use new constructor implementations.")]]  // clang-format off
+	PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){};
+
+	/** \brief Constructor
+	 * \param [in] node ROS 2 node
+	 * \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name
+	 * for the planning requests
+	 * \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning
+	 * pipelines
+	 * \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria
+	 * \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used
+	 */
+	PipelinePlanner(const rclcpp::Node::SharedPtr& node,
+	                const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
+	                const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines =
+	                    std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>(),
+					const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
+					const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
+						&moveit::planning_pipeline_interfaces::getShortestSolution);
+
+	[[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]]  // clang-format off
+	void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
 	}
 
-	static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
-
-	/**
-	 *
-	 * @param node used to load the parameters for the planning pipeline
+	/** \brief Set the planner id for a specific planning pipeline for the planning requests
+	 * \param [in] pipeline_name Name of the planning pipeline for which the planner id is set
+	 * \param [in] planner_id Name of the planner ID that should be used by the planning pipeline
+	 * \return true if the pipeline exists and the corresponding ID is set successfully
+	 */
+	bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
+
+	/** \brief Set stopping criterion function for parallel planning
+	 * \param [in] stopping_criterion_callback New stopping criterion function that will be used
+	*/
+	void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
+
+	/** \brief Set solution selection function for parallel planning
+	 * \param [in] solution_selection_function New solution selection that will be used
+	*/
+	void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
+
+	/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
+	 * configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
+	 * initialized with the given robot model.
+	 * \param [in] robot_model A robot model that is used to initialize the
+	 * planning pipelines of this solver
 	 */
-	PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
-
-	PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
-
-	void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
-
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
+	/**
+	 * \brief Plan a trajectory from a planning scene 'from' to scene 'to'
+	 * \param [in] from Start planning scene
+	 * \param [in] to Goal planning scene (used to create goal constraints)
+	 * \param [in] joint_model_group Group of joints for which this trajectory is created
+	 * \param [in] timeout ?
+	 * \param [in] result Reference to the location where the created trajectory is stored if planning is successful
+	 * \param [in] path_constraints Path contraints for the planning problem
+	 * \return true If the solver found a successful solution for the given planning problem
+	 */
 	bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	          const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	          const core::JointModelGroup* joint_model_group, double timeout,
+	          robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
+	/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
+	 * \param [in] from Start planning scene
+	 * \param [in] link Link for which a target pose is given
+	 * \param [in] offset Offset to be applied to a given target pose
+	 * \param [in] target Target pose
+	 * \param [in] joint_model_group Group of joints for which this trajectory is created
+	 * \param [in] timeout ?
+	 * \param [in] result Reference to the location where the created trajectory is stored if planning is successful
+	 * \param [in] path_constraints Path contraints for the planning problem
+	 * \return true If the solver found a successful solution for the given planning problem
+	 */
 	bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
-	          double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
+	          const moveit::core::JointModelGroup* joint_model_group, double timeout,
+	          robot_trajectory::RobotTrajectoryPtr& result,
 	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
 protected:
-	std::string pipeline_name_;
-	planning_pipeline::PlanningPipelinePtr planner_;
+	/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
+	 * the public plan function after the goal constraints are generated. This function uses a predefined number of
+	 * planning pipelines in parallel to solve the planning problem and choose the automatically the best (user-defined)
+	 * solution.
+	 * \param [in] planning_scene Scene for which the planning should be solved
+	 * \param [in] joint_model_group
+	 * Group of joints for which this trajectory is created
+	 * \param [in] goal_constraints Set of constraints that need to
+	 * be satisfied by the solution
+	 * \param [in] timeout ?
+	 * \param [in] result Reference to the location where the created
+	 * trajectory is stored if planning is successful
+	 * \param [in] path_constraints Path contraints for the planning
+	 * problem
+	 * \return true If the solver found a successful solution for the given planning problem
+	 */
+	bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
+	          const moveit::core::JointModelGroup* joint_model_group,
+	          const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
+	          robot_trajectory::RobotTrajectoryPtr& result,
+	          const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
+
 	rclcpp::Node::SharedPtr node_;
+
+	/** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every
+	 * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are
+	 * used to initialize a set of planning pipelines.  */
+	std::unordered_map<std::string, std::string> pipeline_id_planner_id_map_;
+
+	/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
+	 * planning pipeline when during plan(..) */
+	std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
+
+	moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
+	moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_;
+
 };
 }  // namespace solvers
 }  // namespace task_constructor