Revert PipelinePlanner changes, use MoveItErrorCode and minor cleanups

Author: sjahr

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

@@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
 	            const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
+	MoveItErrorCode 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

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

@@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	            const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	MoveItErrorCode
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	Result 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	MoveItErrorCode
+	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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
 	std::string getPlannerId() const override { return "JointInterpolationPlanner"; }
 };

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

@@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
-	            const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	MoveItErrorCode
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
-	Result 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	MoveItErrorCode
+	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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 };
 }  // namespace solvers
 }  // namespace task_constructor

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

@@ -61,30 +61,35 @@ 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.
+	 * \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 = "")
-	  : PipelinePlanner(node, { { pipeline_name, planner_id } }) {}
+	                const std::string& 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
+	 * \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);
 
-	/** \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
+	/** \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);
@@ -101,26 +106,31 @@ class PipelinePlanner : public PlannerInterface
 	void setSolutionSelectionFunction(
 	    const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
 
-	/** \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
+	/** \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
 	 */
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
-	/** \brief Plan a trajectory from a planning scene 'from' to scene 'to'
+	/**
+	 * \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
+	 * \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise,
+	 * return an appropriate error type and message explaining why it failed.
 	 */
-	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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+	MoveItErrorCode
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     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 Cartesian target pose with an offset
+	/** \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
@@ -129,37 +139,55 @@ class PipelinePlanner : public PlannerInterface
 	 * \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
+	 * \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise,
+	 * return an appropriate error type and message explaining why it failed.
 	 */
-	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_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
-
-	std::string getPlannerId() const override { return last_successful_planner_; }
+	MoveItErrorCode
+	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_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+
+	/**
+	 * \brief Get planner name
+	 * \return Name of the last successful planner
+	 */
+	std::string getPlannerId() const override;
 
 protected:
-	/** \brief Actual plan() implementation, targeting the given goal_constraints.
+	/** \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 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] 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
+	 * \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());
+	MoveItErrorCode 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_;
 
 	std::string last_successful_planner_;
 
-	/** \brief Map of instantiated (and named) planning pipelines. */
+	/** \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_;

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

@@ -41,6 +41,7 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit_msgs/msg/constraints.hpp>
 #include <moveit/task_constructor/properties.h>
+#include <moveit/utils/moveit_error_code.h>
 #include <Eigen/Geometry>
 
 namespace planning_scene {
@@ -64,21 +65,16 @@ namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
+using MoveItErrorCode = moveit::core::MoveItErrorCode;
+using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;
+
 MOVEIT_CLASS_FORWARD(PlannerInterface);
 class PlannerInterface
 {
 	// these properties take precedence over stage properties
 	PropertyMap properties_;
 
 public:
-	struct Result
-	{
-		bool success;
-		std::string message;
-
-		operator bool() const { return success; }
-	};
-
 	PlannerInterface();
 	virtual ~PlannerInterface() {}
 
@@ -96,17 +92,17 @@ class PlannerInterface
 	virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
 
 	/// plan trajectory between to robot states
-	virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
-	                    const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
-	                    double timeout, robot_trajectory::RobotTrajectoryPtr& result,
-	                    const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
+	virtual MoveItErrorCode
+	plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
+	     const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	     const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
 
 	/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
-	virtual Result 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
+	virtual MoveItErrorCode
+	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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
 
 	// get name of the planner
 	virtual std::string getPlannerId() const = 0;