From c11a34a93514fe317deb8c49a7045f0ee74c3747 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Jun 2022 15:08:22 +0000 Subject: [PATCH] ExecuteTaskSolutionCapability: Reject new goals when busy (#496) - Rename goalCallback() -> execCallback() - Run execCallback asynchronously and use future to know status of execution --- capabilities/src/execute_task_solution_capability.cpp | 7 +++++-- capabilities/src/execute_task_solution_capability.h | 8 ++++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 5c13e9dd..6a84ba9d 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/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>& 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>& goal_handle) { auto result = std::make_shared(); diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index 3248b06a..26b437c5 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -63,18 +63,22 @@ private: bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const std::shared_ptr>& goal_handle); + void execCallback(const std::shared_ptr>& goal_handle); rclcpp_action::CancelResponse preemptCallback(const std::shared_ptr>& 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 last_goal_future_; }; } // namespace move_group