diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index aa1ca35b..2508ee23 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -46,6 +46,8 @@ #include +#include + namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) @@ -119,8 +121,8 @@ public: 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; diff --git a/core/src/task.cpp b/core/src/task.cpp index a8e873d3..a6a77906 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -303,7 +303,7 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } -void Task::execute(const SolutionBase& s) { +moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); ac.waitForServer(); @@ -311,6 +311,7 @@ void Task::execute(const SolutionBase& s) { s.fillMessage(goal.solution, pimpl()->introspection_.get()); ac.sendGoal(goal); ac.waitForResult(); + return ac.getResult()->error_code; } void Task::publishAllSolutions(bool wait) {