Fix early planning preemption (moveit#597)

Calling preempt() before plan() is able to reset the preempt_requested_ flag causes the preemption request to get lost. To avoid this issue, we allow a) manual resetting of the request and b) reset the request before leaving plan().

Author: captain-yoshi

.gitmodules

@@ -3,3 +3,6 @@
 	url = https://github.com/pybind/pybind11
 	branch = smart_holder
 	shallow = true
+[submodule "core/src/scope_guard"]
+	path = core/src/scope_guard
+	url = https://github.com/ricab/scope_guard

core/include/moveit/task_constructor/task.h

@@ -127,8 +127,9 @@ class Task : protected WrapperBase
 
 	/// reset, init scene (if not yet done), and init all stages, then start planning
 	moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
-	/// interrupt current planning (or execution)
+	/// interrupt current planning
 	void preempt();
+	void resetPreemptRequest();
 	/// 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/scope_guard

@@ -46,6 +46,8 @@
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 
+#include <scope_guard/scope_guard.hpp>
+
 #include <functional>
 
 namespace {
@@ -234,6 +236,9 @@ void Task::compute() {
 }
 
 moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
+	// ensure the preempt request is resetted once this method exits
+	auto guard = sg::make_scope_guard([this]() noexcept { this->resetPreemptRequest(); });
+
 	auto impl = pimpl();
 	init();
 
@@ -245,7 +250,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 +270,10 @@ void Task::preempt() {
 	pimpl()->preempt_requested_ = true;
 }
 
+void Task::resetPreemptRequest() {
+	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))) {

core/src/task.cpp

@@ -672,3 +672,31 @@ TEST(Task, timeout) {
 	EXPECT_TRUE(t.plan());
 	EXPECT_EQ(t.solutions().size(), 2u);
 }
+
+// https://github.com/moveit/moveit_task_constructor/pull/597
+// start planning in another thread, then preempt it in this thread
+TEST(Task, preempt) {
+	moveit::core::MoveItErrorCode ec;
+	resetMockupIds();
+
+	Task t;
+	t.setRobotModel(getModel());
+
+	auto timeout = std::chrono::milliseconds(10);
+	t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
+	t.add(std::make_unique<TimedForwardMockup>(timeout));
+
+	// preempt before preempt_request_ is reset in plan()
+	{
+		std::thread thread{ [&ec, &t, timeout] {
+			std::this_thread::sleep_for(timeout);
+			ec = t.plan(1);
+		} };
+		t.preempt();
+		thread.join();
+	}
+
+	EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED);
+	EXPECT_EQ(t.solutions().size(), 0u);
+	EXPECT_TRUE(t.plan(1));  // make sure the preempt request has been resetted on the previous call to plan()
+}