Revert "Enable parallel planning with PipelinePlanner (moveit#450)"

This reverts commit 0e02fca.

Author: rhaschke

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

@@ -32,16 +32,14 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Authors: Robert Haschke, Sebastian Jahr
-   Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
+/* Authors: Robert Haschke
+   Desc:    plan using MoveIt's PlanningPipeline
 */
 
 #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 <moveit_msgs/msg/motion_plan_request.hpp>
 #include <rclcpp/node.hpp>
 #include <moveit/macros/class_forward.h>
 
@@ -59,111 +57,52 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
 class PipelinePlanner : public PlannerInterface
 {
 public:
-	/** 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. Empty string means default.
+	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);
+	}
+
+	static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
+
+	/**
+	 *
+	 * @param node used to load the parameters for the planning pipeline
 	 */
-	PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
-	                const std::string& planner_id = "")
-	  : PipelinePlanner(node, { { pipeline_name, planner_id } }) {}
-
-	/** \brief Constructor
-	 * \param [in] node ROS 2 node
-	 * \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
-	 * \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
-	 * \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
-	 */
-	PipelinePlanner(
-	    const rclcpp::Node::SharedPtr& node,
-	    const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
-	    const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
-	    const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
-	        &moveit::planning_pipeline_interfaces::getShortestSolution);
-
-	/** \brief Set the planner id for a specific planning pipeline
-	 * \param [in] pipeline_name Name of the to-be-used planning pipeline
-	 * \param [in] planner_id Name of the to-be-used planner ID
-	 * \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);
+	PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
 
-	/** \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);
+	PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
 
-	/** \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);
+	void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
+	std::string getPlannerId() const override { return properties().get<std::string>("planner"); }
 
-	/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
-	 * \param [in] robot_model robot model used to initialize the planning pipelines of this solver
-	 */
 	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 Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
-	 * \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
-	 */
 	Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	            const core::JointModelGroup* joint_model_group, double timeout,
-	            robot_trajectory::RobotTrajectoryPtr& result,
+	            const core::JointModelGroup* jmg, 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 Cartesian 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 Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
-	 * \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
-	 */
 	Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
 	            const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
-	            const moveit::core::JointModelGroup* joint_model_group, double timeout,
-	            robot_trajectory::RobotTrajectoryPtr& result,
+	            const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	std::string getPlannerId() const override { return last_successful_planner_; }
-
 protected:
-	/** \brief Actual plan() implementation, targeting the given goal_constraints.
-	 * \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 Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
-	 * \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
-	 */
-	Result 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());
+	Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::msg::MotionPlanRequest& req,
+	            robot_trajectory::RobotTrajectoryPtr& result);
 
+	std::string pipeline_name_;
+	planning_pipeline::PlanningPipelinePtr planner_;
 	rclcpp::Node::SharedPtr node_;
-
-	std::string last_successful_planner_;
-
-	/** \brief Map of instantiated (and named) planning pipelines. */
-	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

core/python/bindings/src/solvers.cpp

@@ -69,18 +69,20 @@ void export_solvers(py::module& m) {
 				from moveit.task_constructor import core
 
 				# Create and configure a planner instance
-				pipelinePlanner = core.PipelinePlanner(node, 'ompl', 'PRMkConfigDefault')
+				pipelinePlanner = core.PipelinePlanner()
+				pipelinePlanner.planner = 'PRMkConfigDefault'
 				pipelinePlanner.num_planning_attempts = 10
 			)")
+	    .property<std::string>("planner", "str: Planner ID")
 	    .property<uint>("num_planning_attempts", "int: Number of planning attempts")
 	    .property<moveit_msgs::msg::WorkspaceParameters>(
 	        "workspace_parameters",
 	        ":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling")
 	    .property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
 	    .property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
 	    .property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
-	    .def(py::init<const rclcpp::Node::SharedPtr&, const std::string&, const std::string&>(), "node"_a,
-	         "pipeline"_a = std::string("ompl"), "planner_id"_a = std::string(""));
+	    .def(py::init<const rclcpp::Node::SharedPtr&, const std::string&>(), "node"_a,
+	         "pipeline"_a = std::string("ompl"));
 
 	properties::class_<JointInterpolationPlanner, PlannerInterface>(
 	    m, "JointInterpolationPlanner",

core/python/test/test_mtc.py

@@ -66,15 +66,21 @@ def test_assign_in_reference(self):
 
         props["planner"] = "planner"
         self.assertEqual(props["planner"], "planner")
+        self.assertEqual(planner.planner, "planner")
 
         props["double"] = 3.14
         a = props
         props["double"] = 2.71
         self.assertEqual(a["double"], 2.71)
 
+        planner.planner = "other"
+        self.assertEqual(props["planner"], "other")
+        self.assertEqual(planner.planner, "other")
+
         del planner
         # We can still access props, because actual destruction of planner is delayed
         self.assertEqual(props["goal_joint_tolerance"], 2.71)
+        self.assertEqual(props["planner"], "other")
 
     def test_iter(self):
         # assign values so we can iterate over them