From 6bbd283dc4633f5a102ffbc7970c60693eed7c67 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Tue, 16 Jul 2024 12:18:14 -0400
Subject: [PATCH] reset the preempt flag when leaving Task::plan()

---
 core/src/task.cpp | 58 +++++++++++++++++++++++++++--------------------
 1 file changed, 33 insertions(+), 25 deletions(-)

diff --git a/core/src/task.cpp b/core/src/task.cpp
index f48f2971b..a78e3a181 100644
--- a/core/src/task.cpp
+++ b/core/src/task.cpp
@@ -234,31 +234,39 @@ void Task::compute() {
 }
 
 moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
-	auto impl = pimpl();
-	init();
-
-	// Print state and return success if there are solutions otherwise the input error_code
-	const auto success_or = [this](const int32_t error_code) -> int32_t {
-		if (numSolutions() > 0)
-			return moveit::core::MoveItErrorCode::SUCCESS;
-		printState();
-		explainFailure();
-		return error_code;
-	};
-	const double available_time = timeout();
-	const auto start_time = std::chrono::steady_clock::now();
-	while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
-		if (impl->preempt_requested_)
-			return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
-		if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() >= available_time)
-			return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
-		compute();
-		for (const auto& cb : impl->task_cbs_)
-			cb(*this);
-		if (impl->introspection_)
-			impl->introspection_->publishTaskState();
-	};
-	return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
+	try {
+		auto impl = pimpl();
+		init();
+
+		// Print state and return success if there are solutions otherwise the input error_code
+		const auto success_or = [this](const int32_t error_code) -> int32_t {
+			// reset preempt flag
+			pimpl()->preempt_requested_ = false;
+
+			if (numSolutions() > 0)
+				return moveit::core::MoveItErrorCode::SUCCESS;
+			printState();
+			explainFailure();
+			return error_code;
+		};
+		const double available_time = timeout();
+		const auto start_time = std::chrono::steady_clock::now();
+		while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
+			if (impl->preempt_requested_)
+				return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
+			if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() >= available_time)
+				return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
+			compute();
+			for (const auto& cb : impl->task_cbs_)
+				cb(*this);
+			if (impl->introspection_)
+				impl->introspection_->publishTaskState();
+		};
+		return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
+	} catch (const std::exception& e) {
+		pimpl()->preempt_requested_ = false;
+		throw;  // rethrow the original exception
+	}
 }
 
 void Task::preempt() {