Return MoveItErrorCode from task::plan (#319)

... to know whether the plan failed due to timeout, preemption, or actual planning failure

Author: JafarAbdi

core/include/moveit/task_constructor/task.h

@@ -47,6 +47,7 @@
 #include <moveit/macros/class_forward.h>
 
 #include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit/utils/moveit_error_code.h>
 
 namespace moveit {
 namespace core {
@@ -117,11 +118,11 @@ class Task : protected WrapperBase
 	void init();
 
 	/// reset, init scene (if not yet done), and init all stages, then start planning
-	bool plan(size_t max_solutions = 0);
+	moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
 	/// interrupt current planning (or execution)
 	void preempt();
 	/// execute solution, return the result
-	moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s);
+	moveit::core::MoveItErrorCode execute(const SolutionBase& s);
 
 	/// print current task state (number of found solutions and propagated states) to std::cout
 	void printState(std::ostream& os = std::cout) const;

core/src/task.cpp

@@ -246,30 +246,37 @@ void Task::compute() {
 	stages()->pimpl()->runCompute();
 }
 
-bool Task::plan(size_t max_solutions) {
+moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 	auto impl = pimpl();
 	init();
 
+	// Print state and return success if there are solutions otherwise the input error_code
+	const auto success_or = [this](const int32_t error_code) {
+		printState();
+		return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code;
+	};
 	impl->preempt_requested_ = false;
 	const double available_time = timeout();
 	const auto start_time = std::chrono::steady_clock::now();
-	while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) &&
-	       std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < available_time) {
+	while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
+		if (impl->preempt_requested_)
+			return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
+		if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() > available_time)
+			return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
 		compute();
 		for (const auto& cb : impl->task_cbs_)
 			cb(*this);
 		if (impl->introspection_)
 			impl->introspection_->publishTaskState();
-	}
-	printState();
-	return numSolutions() > 0;
+	};
+	return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
 }
 
 void Task::preempt() {
 	pimpl()->preempt_requested_ = true;
 }
 
-moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) {
+moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 	actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
 	ac.waitForServer();
 

core/test/test_container.cpp

@@ -655,7 +655,7 @@ TEST(Task, timeout) {
 	// zero timeout fails
 	t.reset();
 	t.setTimeout(0.0);
-	EXPECT_FALSE(t.plan());
+	EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT);
 
 	// time for 1 solution
 	t.reset();

demo/src/pick_place_task.cpp

@@ -490,7 +490,7 @@ bool PickPlaceTask::plan() {
 	ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
 	int max_solutions = pnh_.param<int>("max_solutions", 10);
 
-	return task_->plan(max_solutions);
+	return static_cast<bool>(task_->plan(max_solutions));
 }
 
 bool PickPlaceTask::execute() {