fixup! Run goalcallback asynchronously (moveit#496)

- Rename goalCallback() -> execCallback() - Revert transition to lambdas, which made the code less readable
ubi-agni · Mar 7, 2024 · 98c7614 · 98c7614
1 parent 90714d4
commit 98c7614
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 18 deletions.
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -90,25 +90,18 @@ 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",
-	    [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);
-	    });
+	    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(
+	        [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
+		        last_goal_future_ =
+		            std::async(std::launch::async, &ExecuteTaskSolutionCapability::execCallback, this, goal_handle);
+	        }));
 }
 
-void ExecuteTaskSolutionCapability::goalCallback(
+void ExecuteTaskSolutionCapability::execCallback(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle) {
 	auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
 

diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h
@@ -63,11 +63,20 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
 	bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
 	                         plan_execution::ExecutableMotionPlan& plan);
 
-	void goalCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
+	void execCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
 	rclcpp_action::CancelResponse
 	preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& goal_handle);
 
+	/** Only accept the goal if we aren't executing one */
+	rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/,
+	                                          const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const {
+		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;
+	}
+
 	ActionServerType::SharedPtr as_;
 	std::future<void> last_goal_future_;
 };