[fix] make stages preemptable when the Task has been preempted

Forward Task::isPreempted() to all stages.

Author: captain-yoshi

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

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 requested to all stages
 	auto* introspection = impl->introspection_.get();
 	impl->traverseStages(
-	    [introspection](Stage& stage, int /*depth*/) {
+	    [introspection, impl](Stage& stage, int /*depth*/) {
 		    stage.pimpl()->setIntrospection(introspection);
+		    stage.pimpl()->setPreemptedCheck(&impl->preempt_requested_);
 		    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<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
 	if (!ac.waitForServer(ros::Duration(0.5))) {