Run goalcallback asynchronously (moveit#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>
ubi-agni · Mar 7, 2024 · 90714d4 · 90714d4
1 parent ecc7213
commit 90714d4
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 12 deletions.
diff --git 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
@@ -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