Propagate errors from planners

Author: Abishalini

core/src/solvers/cartesian_path.cpp

@@ -71,7 +71,7 @@ tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::Planni
 	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 tl::make_unexpected("no unique tip for joint model group: " + jmg->getName());
 	}
 
 	// reach pose of forward kinematics
@@ -116,7 +116,10 @@ tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::Planni
 	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 tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
+	}
+	return true;
 }
 }  // namespace solvers
 }  // namespace task_constructor

core/src/solvers/joint_interpolation.cpp

@@ -77,7 +77,7 @@ tl::expected<bool, std::string> JointInterpolationPlanner::plan(
 	// add first point
 	result->addSuffixWayPoint(from->getCurrentState(), 0.0);
 	if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
-		return false;
+		return tl::make_unexpected("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;
@@ -86,13 +86,13 @@ tl::expected<bool, std::string> JointInterpolationPlanner::plan(
 		result->addSuffixWayPoint(waypoint, t);
 
 		if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
-			return false;
+			return tl::make_unexpected("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 tl::make_unexpected("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"),
@@ -140,12 +140,12 @@ JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& fro
 		// 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 tl::make_unexpected("IK failed for pose target");
 	}
 	to->getCurrentStateNonConst().update();
 
 	if (std::chrono::steady_clock::now() >= deadline)
-		return false;
+		return tl::make_unexpected("Timed out");
 
 	return plan(from, to, jmg, timeout, result, path_constraints);
 }

core/src/solvers/pipeline_planner.cpp

@@ -224,10 +224,16 @@ tl::expected<bool, std::string> PipelinePlanner::plan(const planning_scene::Plan
 			// Choose the first solution trajectory as response
 			result = solution.trajectory;
 			last_successful_planner_ = solution.planner_id;
-			return bool(result);
+			if (!bool(result))  // If the plan was not a success
+			{
+				return tl::make_unexpected(solution.error_code.source + " returned error code " +
+				                           moveit::core::errorCodeToString(solution.error_code) +
+				                           ". Reason : " + solution.error_code.message);  // Maybe print the error code too
+			}
+			return true;
 		}
 	}
-	return false;
+	return tl::make_unexpected("No solutions generated from Pipeline Planner");
 }
 std::string PipelinePlanner::getPlannerId() const {
 	return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_;

core/src/stages/connect.cpp

@@ -148,6 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	intermediate_scenes.push_back(start);
 
 	bool success = false;
+	std::string error_message = "";
 	std::vector<double> positions;
 	for (const GroupPlannerVector::value_type& pair : planner_) {
 		// set intermediate goal state
@@ -164,6 +165,10 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 		if (planner_solution_maybe.has_value()) {
 			success = planner_solution_maybe.value();
 		}
+		std::string error_message = "";
+		if (!success) {
+			error_message += planner_solution_maybe.error();
+		}
 		trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory }));
 
 		if (!success)
@@ -179,7 +184,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	if (!solution)  // success == false or merging failed: store sequentially
 		solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to);
 	if (!success)  // error during sequential planning
-		solution->markAsFailure();
+		solution->markAsFailure("Hardcode an error message");
 
 	connect(from, to, solution);
 }

core/src/stages/move_relative.cpp

@@ -195,6 +195,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
 	bool success = false;
+	std::string error_message = "";
 
 	if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
@@ -203,6 +204,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		if (planner_solution_maybe.has_value()) {
 			success = planner_solution_maybe.value();
 		}
+		if (!success) {
+			error_message = planner_solution_maybe.error();
+		}
 		solution.setPlannerId(planner_->getPlannerId());
 	} else {
 		// Cartesian targets require an IK reference frame
@@ -295,6 +299,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		if (planner_solution_maybe.has_value()) {
 			success = planner_solution_maybe.value();
 		}
+		if (!success) {
+			error_message = planner_solution_maybe.error();
+		}
 		solution.setPlannerId(planner_->getPlannerId());
 
 		if (robot_trajectory) {  // the following requires a robot_trajectory returned from planning
@@ -338,8 +345,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 			robot_trajectory->reverse();
 		solution.setTrajectory(robot_trajectory);
 
-		if (!success)
+		if (!success && solution.comment().empty()) {
+			solution.markAsFailure(error_message);
+		} else if (!success) {
 			solution.markAsFailure();
+		}
 		return true;
 	}
 	return false;

core/src/stages/move_to.cpp

@@ -205,6 +205,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 	const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
 	bool success = false;
+	std::string error_message = "";
 
 	if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
@@ -213,6 +214,9 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		if (planner_solution_maybe.has_value()) {
 			success = planner_solution_maybe.value();
 		}
+		if (!success) {
+			error_message = planner_solution_maybe.error();
+		}
 		solution.setPlannerId(planner_->getPlannerId());
 	} else {  // Cartesian goal
 		// Where to go?
@@ -250,6 +254,9 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		if (planner_solution_maybe.has_value()) {
 			success = planner_solution_maybe.value();
 		}
+		if (!success) {
+			error_message = planner_solution_maybe.error();
+		}
 		solution.setPlannerId(planner_->getPlannerId());
 	}
 
@@ -266,7 +273,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		solution.setTrajectory(robot_trajectory);
 
 		if (!success)
-			solution.markAsFailure();
+			solution.markAsFailure(error_message);
 
 		return true;
 	}