Reset preempt_request after canceling execution (moveit#689)

Author: MarqRazz

capabilities/test/test_task_execution.cpp

@@ -60,6 +60,13 @@ TEST_F(PandaMoveTo, preemptExecution) {
 	}
 
 	EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::PREEMPTED);
+
+	// After preempting motion reset the task and make sure we can successfully plan and execute motion again
+	rclcpp::sleep_for(std::chrono::seconds(1));
+	t.reset();
+	ASSERT_TRUE(t.plan());
+	result = t.execute(*t.solutions().front());
+	EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
 }
 
 int main(int argc, char** argv) {

core/src/task.cpp

@@ -316,6 +316,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 	while (result_future.wait_for(std::chrono::milliseconds(10)) != std::future_status::ready) {
 		if (pimpl()->preempt_requested_) {
 			auto cancel_future = execute_ac_->async_cancel_goal(goal_handle);
+			this->resetPreemptRequest();
 			if (rclcpp::spin_until_future_complete(execute_solution_node_, cancel_future) !=
 			    rclcpp::FutureReturnCode::SUCCESS) {
 				RCLCPP_ERROR(execute_solution_node_->get_logger(), "Could not preempt execution");