diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index d1c123740..97786e9c2 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -130,6 +130,7 @@ class Task : protected WrapperBase /// interrupt current planning void preempt(); void resetPreemptRequest(); + bool isPreempted(); /// execute solution, return the result moveit::core::MoveItErrorCode execute(const SolutionBase& s); diff --git a/core/src/task.cpp b/core/src/task.cpp index dbba73890..a42bc17b2 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -213,11 +213,12 @@ void Task::init() { // task expects its wrapped child to push to both ends, this triggers interface resolution stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); - // provide introspection instance to all stages + // provide introspection instance and prempted callback to all stages auto* introspection = impl->introspection_.get(); impl->traverseStages( - [introspection](Stage& stage, int /*depth*/) { + [introspection, this](Stage& stage, int /*depth*/) { stage.pimpl()->setIntrospection(introspection); + stage.pimpl()->setPreemptedCallback(std::bind(&Task::isPreempted, this)); return true; }, 1, UINT_MAX); @@ -232,7 +233,11 @@ bool Task::canCompute() const { } void Task::compute() { - stages()->pimpl()->runCompute(); + try { + stages()->pimpl()->runCompute(); + } catch (const PreemptStageException& e) { + // do nothing, needed for early stop + } } moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { @@ -274,6 +279,10 @@ void Task::resetPreemptRequest() { pimpl()->preempt_requested_ = false; } +bool Task::isPreempted() { + return pimpl()->preempt_requested_; +} + moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); if (!ac.waitForServer(ros::Duration(0.5))) {