Propagate errors from planners.

core/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(moveit_task_constructor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_marker_tools REQUIRED)
 find_package(tf2_eigen REQUIRED)
+find_package(tl_expected REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
@@ -44,5 +45,6 @@ ament_export_dependencies(moveit_task_constructor_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rviz_marker_tools)
 ament_export_dependencies(tf2_eigen)
+ament_export_dependencies(tl_expected)
 ament_export_dependencies(visualization_msgs)
 ament_package()

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;
 
-	bool 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;
 
-	bool 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;
 
-	bool 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;
 
-	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 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 std::string("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;
 
-	bool 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;
 
-	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 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

@@ -126,7 +126,7 @@ class PipelinePlanner : public PlannerInterface
 	 * \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,
+	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;
@@ -142,7 +142,7 @@ class PipelinePlanner : public PlannerInterface
 	 * \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,
+	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,
@@ -170,7 +170,7 @@ class PipelinePlanner : public PlannerInterface
 	 * problem
 	 * \return true If the solver found a successful solution for the given planning problem
 	 */
-	bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
+	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,

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,6 +65,9 @@ namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
+using MoveItErrorCode = moveit::core::MoveItErrorCode;
+using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;
+
 MOVEIT_CLASS_FORWARD(PlannerInterface);
 class PlannerInterface
 {
@@ -88,17 +92,17 @@ class PlannerInterface
 	virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
 
 	/// plan trajectory between to robot states
-	virtual bool 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 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 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;

core/package.xml

@@ -22,6 +22,7 @@
 	<depend>rclcpp</depend>
 	<depend>rviz_marker_tools</depend>
 	<depend>tf2_eigen</depend>
+	<depend>tl_expected</depend>
 	<depend>visualization_msgs</depend>
 
 	<test_depend>ament_cmake_gmock</test_depend>

core/src/solvers/cartesian_path.cpp

@@ -63,26 +63,27 @@ CartesianPath::CartesianPath() {
 
 void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
 
-bool CartesianPath::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) {
+MoveItErrorCode CartesianPath::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) {
 	const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
 	if (!link) {
 		RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
-		return false;
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "no unique tip for joint model group: " + jmg->getName());
 	}
 
 	// reach pose of forward kinematics
 	return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
 	            std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
 }
 
-bool CartesianPath::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) {
+MoveItErrorCode CartesianPath::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) {
 	const auto& props = properties();
 	planning_scene::PlanningScenePtr sandbox_scene = from->diff();
 
@@ -114,7 +115,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
 	timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
 	                          props.get<double>("max_acceleration_scaling_factor"));
 
-	return achieved_fraction >= props.get<double>("min_fraction");
+	if (achieved_fraction < props.get<double>("min_fraction")) {
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE,
+		                       "Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
+	}
+	return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
 }
 }  // namespace solvers
 }  // namespace task_constructor

core/src/solvers/joint_interpolation.cpp

@@ -59,11 +59,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
 
 void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
 
-bool JointInterpolationPlanner::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*/) {
+MoveItErrorCode JointInterpolationPlanner::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*/) {
 	const auto& props = properties();
 
 	// Get maximum joint distance
@@ -78,7 +78,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 	// add first point
 	result->addSuffixWayPoint(from->getCurrentState(), 0.0);
 	if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
-		return false;
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds");
 
 	moveit::core::RobotState waypoint(from_state);
 	double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
@@ -87,13 +87,14 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 		result->addSuffixWayPoint(waypoint, t);
 
 		if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
-			return false;
+			return MoveItErrorCode(MoveItErrorCodes::FAILURE,
+			                       "One of the waypoint's state is in collision or not within bounds");
 	}
 
 	// add goal point
 	result->addSuffixWayPoint(to_state, 1.0);
 	if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
-		return false;
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds");
 
 	auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
 	timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
@@ -113,14 +114,15 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 		}
 	}
 
-	return true;
+	return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
 }
 
-bool JointInterpolationPlanner::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) {
+MoveItErrorCode JointInterpolationPlanner::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) {
 	timeout = std::min(timeout, properties().get<double>("timeout"));
 	const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
 
@@ -140,12 +142,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 		// TODO(v4hn): planners need a way to add feedback to failing plans
 		// in case of an invalid solution feedback should include unwanted collisions or violated constraints
 		RCLCPP_WARN(LOGGER, "IK failed for pose target");
-		return false;
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target.");
 	}
 	to->getCurrentStateNonConst().update();
 
-	if (std::chrono::steady_clock::now() >= deadline)
-		return false;
+	if (std::chrono::steady_clock::now() >= deadline) {
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timed out.");
+	}
 
 	return plan(from, to, jmg, timeout, result, path_constraints);
 }