Task::execute()

This commit is contained in:
Robert Haschke 2018-06-10 18:30:26 +02:00
parent 99a7a9bc9a
commit b5bc97b3eb
2 changed files with 15 additions and 0 deletions

View File

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

View File

@ -40,8 +40,10 @@
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
@ -232,6 +234,17 @@ void Task::preempt()
preempt_requested_ = true;
}
void 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, introspection_.get());
ac.sendGoal(goal);
ac.waitForResult();
}
void Task::publishAllSolutions(bool wait)
{
enableIntrospection(true);