From 90714d4d2a9080cc46bc33251e7e58e14f1e74d9 Mon Sep 17 00:00:00 2001
From: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date: Fri, 27 Oct 2023 10:31:44 +0200
Subject: [PATCH] Run goalcallback asynchronously (#496)

* Run goalCallback in a seperate asynchronously

* Clang-tidy fixes

* Replace std::bind with lambdas

* Add comment

* Format

---------

Co-authored-by: JafarAbdi <cafer.abdi@gmail.com>
---
 .../src/execute_task_solution_capability.cpp  | 22 ++++++++++++++-----
 .../src/execute_task_solution_capability.h    |  7 +-----
 2 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
index 5c13e9ddf..ba0de5bc8 100644
--- a/capabilities/src/execute_task_solution_capability.cpp
+++ b/capabilities/src/execute_task_solution_capability.cpp
@@ -90,12 +90,22 @@ void ExecuteTaskSolutionCapability::initialize() {
 	// configure the action server
 	as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
 	    context_->moveit_cpp_->getNode(), "execute_task_solution",
-	    ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
-	                                             std::placeholders::_1, std::placeholders::_2)),
-	    ActionServerType::CancelCallback(
-	        std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
-	    ActionServerType::AcceptedCallback(
-	        std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
+	    [this](const rclcpp_action::GoalUUID& /*uuid*/,
+	           const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) {
+		    // Reject new goal if another goal is currently processed
+		    if (last_goal_future_.valid() &&
+		        last_goal_future_.wait_for(std::chrono::seconds::zero()) != std::future_status::ready) {
+			    return rclcpp_action::GoalResponse::REJECT;
+		    }
+		    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+	    },
+	    [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
+		    return preemptCallback(goal_handle);
+	    },
+	    [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
+		    last_goal_future_ =
+		        std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle);
+	    });
 }
 
 void ExecuteTaskSolutionCapability::goalCallback(
diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h
index 3248b06a8..5682d44a6 100644
--- a/capabilities/src/execute_task_solution_capability.h
+++ b/capabilities/src/execute_task_solution_capability.h
@@ -68,13 +68,8 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
 	rclcpp_action::CancelResponse
 	preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
-	/** Always accept the goal */
-	rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/,
-	                                          const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const {
-		return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-	}
-
 	ActionServerType::SharedPtr as_;
+	std::future<void> last_goal_future_;
 };
 
 }  // namespace move_group