Add return value to Task::execute (#136)

Author: mvieth

core/include/moveit/task_constructor/task.h

@@ -46,6 +46,8 @@
 
 #include <moveit/macros/class_forward.h>
 
+#include <moveit_msgs/MoveItErrorCodes.h>
+
 namespace moveit {
 namespace core {
 MOVEIT_CLASS_FORWARD(RobotModel)
@@ -119,8 +121,8 @@ class Task : protected WrapperBase
 	bool plan(size_t max_solutions = 0);
 	/// interrupt current planning (or execution)
 	void preempt();
-	/// execute solution
-	void execute(const SolutionBase& s);
+	/// execute solution, return the result
+	moveit_msgs::MoveItErrorCodes 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

@@ -303,14 +303,15 @@ void Task::preempt() {
 	pimpl()->preempt_requested_ = true;
 }
 
-void Task::execute(const SolutionBase& s) {
+moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) {
 	actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
 	ac.waitForServer();
 
 	moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal;
 	s.fillMessage(goal.solution, pimpl()->introspection_.get());
 	ac.sendGoal(goal);
 	ac.waitForResult();
+	return ac.getResult()->error_code;
 }
 
 void Task::publishAllSolutions(bool wait) {