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

This commit is contained in:
Markus Vieth 2020-02-20 11:31:51 +01:00 committed by GitHub
parent c44d0cac19
commit 534d520a04
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 6 additions and 3 deletions

View File

@ -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 @@ 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;

View File

@ -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<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> 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) {