From c08dcaaa0ac296e19d9b0d7fba57406613bf3fba Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Jun 2022 15:08:22 +0000 Subject: [PATCH 1/5] Run goalCallback in a seperate asynchronously --- capabilities/src/execute_task_solution_capability.cpp | 6 ++++-- capabilities/src/execute_task_solution_capability.h | 8 ++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 5c13e9ddf..0bce82666 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -94,8 +94,10 @@ void ExecuteTaskSolutionCapability::initialize() { 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 std::shared_ptr> 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..b79ec17db 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -68,13 +68,17 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability 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 { + 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 From 0992a286ff10aa5f9d20d22049c04cd0c33178d5 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 16 May 2023 16:43:14 +0200 Subject: [PATCH 2/5] Clang-tidy fixes --- capabilities/src/execute_task_solution_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 0bce82666..f1acddf59 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -94,7 +94,7 @@ void ExecuteTaskSolutionCapability::initialize() { std::placeholders::_1, std::placeholders::_2)), ActionServerType::CancelCallback( std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)), - [this](const std::shared_ptr> goal_handle) { + [this](const std::shared_ptr>& goal_handle) { last_goal_future_ = std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle); }); From 1b56805f9c05518bd7b27a87a39b280fc3e0c836 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 24 Oct 2023 11:36:57 +0200 Subject: [PATCH 3/5] Replace std::bind with lambdas --- .../src/execute_task_solution_capability.cpp | 15 +++++++++++---- .../src/execute_task_solution_capability.h | 9 --------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index f1acddf59..a72833e9a 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -90,10 +90,17 @@ void ExecuteTaskSolutionCapability::initialize() { // configure the action server as_ = rclcpp_action::create_server( 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)), + [this](const rclcpp_action::GoalUUID& /*uuid*/, + const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) { + 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>& goal_handle) { + return preemptCallback(goal_handle); + }, [this](const std::shared_ptr>& goal_handle) { last_goal_future_ = std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle); diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index b79ec17db..5682d44a6 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -68,15 +68,6 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability rclcpp_action::CancelResponse preemptCallback(const std::shared_ptr>& 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 last_goal_future_; }; From 3528c0880e21d839b50fd3e6bb18a4812c2749a1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 26 Oct 2023 09:05:54 +0200 Subject: [PATCH 4/5] Add comment --- capabilities/src/execute_task_solution_capability.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index a72833e9a..8c49b01a3 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -92,6 +92,7 @@ void ExecuteTaskSolutionCapability::initialize() { 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; From fa7a25c2e4c75dedc53a05e4acce82ebb2dae5f8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 27 Oct 2023 09:14:59 +0200 Subject: [PATCH 5/5] Format --- capabilities/src/execute_task_solution_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 8c49b01a3..ba0de5bc8 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -92,7 +92,7 @@ void ExecuteTaskSolutionCapability::initialize() { 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 + // 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;