Cleanup moveit#525

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

@@ -113,7 +113,8 @@ 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 MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
+	 * \return MoveItErrorCodes::SUCCESS if solver found a successful solution.
+	 *         Otherwise, return an appropriate error code and message explaining why it failed.
 	 */
 	MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
 	          const core::JointModelGroup* joint_model_group, double timeout,
@@ -129,7 +130,8 @@ 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 MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
+	 * \return MoveItErrorCodes::SUCCESS if solver found a successful solution.
+	 *         Otherwise, return an appropriate error code and message explaining why it failed.
 	 */
 	MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
 	          const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
@@ -147,7 +149,8 @@ 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 MoveItErrorCodes::SUCCESS if solver found a successful solution.
+	 *         Otherwise, return an appropriate error code and message explaining why it failed.
 	 */
 	MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
 	          const moveit::core::JointModelGroup* joint_model_group,

core/src/solvers/cartesian_path.cpp

@@ -69,10 +69,8 @@ MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr&
                                     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());
+	if (!link)
 		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,
@@ -117,7 +115,7 @@ MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr&
 
 	if (achieved_fraction < props.get<double>("min_fraction")) {
 		return MoveItErrorCode(MoveItErrorCodes::FAILURE,
-		                       "Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
+		                       "min_fraction not met. Achieved: " + std::to_string(achieved_fraction));
 	}
 	return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
 }

core/src/solvers/joint_interpolation.cpp

@@ -77,38 +77,32 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc
 
 	// add first point
 	result->addSuffixWayPoint(from->getCurrentState(), 0.0);
-	if (from->isStateColliding(from_state, jmg->getName())) {
+	if (from->isStateColliding(from_state, jmg->getName()))
 		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state is in collision!");
-	}
 
-	if (!from_state.satisfiesBounds(jmg)) {
-		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state is not within bounds!");
-	}
+	if (!from_state.satisfiesBounds(jmg))
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state is out of bounds!");
 
 	moveit::core::RobotState waypoint(from_state);
 	double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
 	for (double t = delta; t < 1.0; t += delta) {  // NOLINT(clang-analyzer-security.FloatLoopCounter)
 		from_state.interpolate(to_state, t, waypoint);
 		result->addSuffixWayPoint(waypoint, t);
 
-		if (from->isStateColliding(waypoint, jmg->getName())) {
-			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is in collision!");
-		}
+		if (from->isStateColliding(waypoint, jmg->getName()))
+			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Waypoint is in collision!");
 
-		if (!waypoint.satisfiesBounds(jmg)) {
-			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is not within bounds!");
-		}
+		if (!waypoint.satisfiesBounds(jmg))
+			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Waypoint is out of bounds!");
 	}
 
 	// add goal point
 	result->addSuffixWayPoint(to_state, 1.0);
-	if (from->isStateColliding(to_state, jmg->getName())) {
+	if (from->isStateColliding(to_state, jmg->getName()))
 		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state is in collision!");
-	}
 
-	if (!to_state.satisfiesBounds(jmg)) {
-		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state is not within bounds!");
-	}
+	if (!to_state.satisfiesBounds(jmg))
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state is out of bounds!");
 
 	auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
 	timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
@@ -152,15 +146,12 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc
 		return to->isStateValid(*robot_state, constraints, jmg->getName());
 	} };
 
-	if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
-		RCLCPP_WARN(LOGGER, "IK failed for pose target");
+	if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid))
 		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target.");
-	}
 	to->getCurrentStateNonConst().update();
 
-	if (std::chrono::steady_clock::now() >= deadline) {
-		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timed out.");
-	}
+	if (std::chrono::steady_clock::now() >= deadline)
+		return MoveItErrorCode(MoveItErrorCodes::FAILURE, "timeout");
 
 	return plan(from, to, jmg, timeout, result, path_constraints);
 }

core/src/solvers/multi_planner.cpp

@@ -57,19 +57,23 @@ MoveItErrorCode MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr&
 	double remaining_time = std::min(timeout, properties().get<double>("timeout"));
 	auto start_time = std::chrono::steady_clock::now();
 
+	std::string comment = "No planner specified";
 	for (const auto& p : *this) {
 		if (remaining_time < 0)
-			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout");  // timeout
+			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "timeout");
 		if (result)
 			result->clear();
-		if (p->plan(from, to, jmg, remaining_time, result, path_constraints))
-			return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
+		auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints);
+		if (r)
+			return r;
+		else
+			comment = r.message;
 
 		auto now = std::chrono::steady_clock::now();
 		remaining_time -= std::chrono::duration<double>(now - start_time).count();
 		start_time = now;
 	}
-	return MoveItErrorCode(MoveItErrorCodes::FAILURE);
+	return MoveItErrorCode(MoveItErrorCodes::FAILURE, comment);
 }
 
 MoveItErrorCode MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
@@ -80,19 +84,23 @@ MoveItErrorCode MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr&
 	double remaining_time = std::min(timeout, properties().get<double>("timeout"));
 	auto start_time = std::chrono::steady_clock::now();
 
+	std::string comment = "No planner specified";
 	for (const auto& p : *this) {
 		if (remaining_time < 0)
-			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout");  // timeout
+			return MoveItErrorCode(MoveItErrorCodes::FAILURE, "timeout");
 		if (result)
 			result->clear();
-		if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints))
-			return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
+		auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints);
+		if (r)
+			return r;
+		else
+			comment = r.message;
 
 		auto now = std::chrono::steady_clock::now();
 		remaining_time -= std::chrono::duration<double>(now - start_time).count();
 		start_time = now;
 	}
-	return MoveItErrorCode(MoveItErrorCodes::FAILURE);
+	return MoveItErrorCode(MoveItErrorCodes::FAILURE, comment);
 }
 }  // namespace solvers
 }  // namespace task_constructor

core/src/solvers/pipeline_planner.cpp

@@ -208,11 +208,7 @@ MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPt
 			// Choose the first solution trajectory as response
 			result = solution.trajectory;
 			last_successful_planner_ = solution.planner_id;
-			if (!bool(result))  // If the plan was not a success
-			{
-				return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source);
-			}
-			return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
+			return solution.error_code;
 		}
 	}
 	return MoveItErrorCode(MoveItErrorCodes::FAILURE, "No solutions generated from Pipeline Planner");

core/src/stages/connect.cpp

@@ -148,7 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	intermediate_scenes.push_back(start);
 
 	bool success = false;
-	std::string error_message = "";
+	std::string comment = "No planners specified";
 	std::vector<double> positions;
 	for (const GroupPlannerVector::value_type& pair : planner_) {
 		// set intermediate goal state
@@ -161,15 +161,12 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 		intermediate_scenes.push_back(end);
 
 		robot_trajectory::RobotTrajectoryPtr trajectory;
-		const auto planner_solution_status = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
-		if (bool(planner_solution_status)) {
-			success = true;
-		}
-
+		auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
+		success = bool(result);
 		sub_trajectories.push_back({ pair.second->getPlannerId(), trajectory });
 
 		if (!success) {
-			error_message = planner_solution_status.message;
+			comment = result.message;
 			break;
 		}
 
@@ -183,7 +180,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	if (!solution)  // success == false or merging failed: store sequentially
 		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
 	if (!success)  // error during sequential planning
-		solution->markAsFailure(error_message);
+		solution->markAsFailure(comment);
 	connect(from, to, solution);
 }
 

core/src/stages/move_relative.cpp

@@ -195,18 +195,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
 	bool success = false;
-	std::string error_message = "";
+	std::string comment = "";
 
 	if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
-		const auto planner_solution_status =
-		    planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
-		if (bool(planner_solution_status)) {
-			success = true;
-		}
-		if (!success) {
-			error_message = planner_solution_status.message;
-		}
+		auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
+		success = bool(result);
+		if (!success)
+			comment = result.message;
 		solution.setPlannerId(planner_->getPlannerId());
 	} else {
 		// Cartesian targets require an IK reference frame
@@ -294,14 +290,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		// offset from link to ik_frame
 		const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
 
-		const auto planner_solution_status =
+		auto result =
 		    planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
-		if (bool(planner_solution_status)) {
-			success = true;
-		}
-		if (!success) {
-			error_message = planner_solution_status.message;
-		}
+		success = bool(result);
+		if (!success)
+			comment = result.message;
 		solution.setPlannerId(planner_->getPlannerId());
 
 		if (robot_trajectory) {  // the following requires a robot_trajectory returned from planning
@@ -345,11 +338,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 			robot_trajectory->reverse();
 		solution.setTrajectory(robot_trajectory);
 
-		if (!success && solution.comment().empty()) {
-			solution.markAsFailure(error_message);
-		} else if (!success) {
-			solution.markAsFailure();
-		}
+		if (!success)
+			solution.markAsFailure(comment);
 		return true;
 	}
 	return false;

core/src/stages/move_to.cpp

@@ -205,18 +205,14 @@ 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 = "";
+	std::string comment = "";
 
 	if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
-		const auto planner_solution_status =
-		    planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
-		if (bool(planner_solution_status)) {
-			success = true;
-		}
-		if (!success) {
-			error_message = planner_solution_status.message;
-		}
+		auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
+		success = bool(result);
+		if (!success)
+			comment = result.message;
 		solution.setPlannerId(planner_->getPlannerId());
 	} else {  // Cartesian goal
 		// Where to go?
@@ -249,14 +245,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
 
 		// plan to Cartesian target
-		const auto planner_solution_status =
+		const auto result =
 		    planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
-		if (bool(planner_solution_status)) {
-			success = true;
-		}
-		if (!success) {
-			error_message = planner_solution_status.message;
-		}
+		success = bool(result);
+		if (!success)
+			comment = result.message;
 		solution.setPlannerId(planner_->getPlannerId());
 	}
 
@@ -273,7 +266,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		solution.setTrajectory(robot_trajectory);
 
 		if (!success)
-			solution.markAsFailure(error_message);
+			solution.markAsFailure(comment);
 
 		return true;
 	}