ExecuteTaskSolutionCapability: Reject new goals when busy (moveit#496)

- Rename goalCallback() -> execCallback()
- Run execCallback asynchronously and use future to know status of execution

Author: JafarAbdi

capabilities/src/execute_task_solution_capability.cpp

@@ -95,10 +95,13 @@ void ExecuteTaskSolutionCapability::initialize() {
 	    ActionServerType::CancelCallback(
 	        std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
 	    ActionServerType::AcceptedCallback(
-	        std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
+	        [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>();
 

capabilities/src/execute_task_solution_capability.h

@@ -63,18 +63,22 @@ 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);
 
-	/** Always accept the goal */
+	/** 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_;
 };
 
 }  // namespace move_group