From b5bc97b3eb35f5c87f0c81f180bce6be9b6153bd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 10 Jun 2018 18:30:26 +0200 Subject: [PATCH] Task::execute() --- core/include/moveit/task_constructor/task.h | 2 ++ core/src/task.cpp | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index e736ac36..242f0e12 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -106,6 +106,8 @@ public: bool plan(size_t max_solutions = 0); /// interrupt current planning (or execution) void preempt(); + /// execute solution + void 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 74909d06..3772b5b3 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -40,8 +40,10 @@ #include #include #include +#include #include +#include #include #include @@ -232,6 +234,17 @@ void Task::preempt() preempt_requested_ = true; } +void Task::execute(const SolutionBase &s) +{ + actionlib::SimpleActionClient ac("execute_task_solution"); + ac.waitForServer(); + + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; + s.fillMessage(goal.solution, introspection_.get()); + ac.sendGoal(goal); + ac.waitForResult(); +} + void Task::publishAllSolutions(bool wait) { enableIntrospection(true);