mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Task::execute()
This commit is contained in:
parent
99a7a9bc9a
commit
b5bc97b3eb
@ -106,6 +106,8 @@ public:
|
|||||||
bool plan(size_t max_solutions = 0);
|
bool plan(size_t max_solutions = 0);
|
||||||
/// interrupt current planning (or execution)
|
/// interrupt current planning (or execution)
|
||||||
void preempt();
|
void preempt();
|
||||||
|
/// execute solution
|
||||||
|
void execute(const SolutionBase& s);
|
||||||
|
|
||||||
/// print current task state (number of found solutions and propagated states) to std::cout
|
/// print current task state (number of found solutions and propagated states) to std::cout
|
||||||
void printState(std::ostream &os = std::cout) const;
|
void printState(std::ostream &os = std::cout) const;
|
||||||
|
|||||||
@ -40,8 +40,10 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
#include <moveit/task_constructor/container.h>
|
#include <moveit/task_constructor/container.h>
|
||||||
#include <moveit/task_constructor/introspection.h>
|
#include <moveit/task_constructor/introspection.h>
|
||||||
|
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
#include <actionlib/client/simple_action_client.h>
|
||||||
|
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||||
@ -232,6 +234,17 @@ void Task::preempt()
|
|||||||
preempt_requested_ = true;
|
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)
|
void Task::publishAllSolutions(bool wait)
|
||||||
{
|
{
|
||||||
enableIntrospection(true);
|
enableIntrospection(true);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user