[fix] early preempt lost request

Remove the initialization of preemt_requested_ in Task::plan(). Make preempted flag atomic.
Added method to reset the preempt request.

core/include/moveit/task_constructor/task.h

@@ -129,6 +129,7 @@ class Task : protected WrapperBase
 	moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
 	/// interrupt current planning
 	void preempt();
+	void resetPreemptFlag();
 	/// execute solution, return the result
 	moveit::core::MoveItErrorCode execute(const SolutionBase& s);
 

core/include/moveit/task_constructor/task_p.h

@@ -63,7 +63,7 @@ class TaskPrivate : public WrapperBasePrivate
 	std::string ns_;
 	robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
 	moveit::core::RobotModelConstPtr robot_model_;
-	bool preempt_requested_;
+	std::atomic<bool> preempt_requested_;
 
 	// introspection and monitoring
 	std::unique_ptr<Introspection> introspection_;

core/src/task.cpp

@@ -245,7 +245,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 		explainFailure();
 		return error_code;
 	};
-	impl->preempt_requested_ = false;
 	const double available_time = timeout();
 	const auto start_time = std::chrono::steady_clock::now();
 	while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
@@ -266,6 +265,10 @@ void Task::preempt() {
 	pimpl()->preempt_requested_ = true;
 }
 
+void Task::resetPreemptFlag() {
+	pimpl()->preempt_requested_ = false;
+}
+
 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))) {