From 69fc142bad6eb83edd1e1d28fda43cfede9e2763 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20=C3=81ngel=20Gonz=C3=A1lez=20Santamarta?= Date: Tue, 16 Apr 2024 21:40:20 +0200 Subject: [PATCH] actions fixed --- .../simple_node/actions/action_client.hpp | 16 +++++++++++++--- .../simple_node/actions/action_server.hpp | 10 +++++++++- .../simple_node/actions/action_client.py | 17 +++++++++++++++-- .../simple_node/actions/action_server.py | 15 ++++++++++----- 4 files changed, 47 insertions(+), 11 deletions(-) diff --git a/simple_node/include/simple_node/actions/action_client.hpp b/simple_node/include/simple_node/actions/action_client.hpp index 41ef1c8..7246008 100644 --- a/simple_node/include/simple_node/actions/action_client.hpp +++ b/simple_node/include/simple_node/actions/action_client.hpp @@ -68,6 +68,7 @@ class ActionClient : public rclcpp_action::Client { void send_goal(Goal goal, FeedbackCallback feedback_cb = nullptr) { + this->goal_handle = nullptr; this->result = nullptr; this->set_status(rclcpp_action::ResultCode::UNKNOWN); @@ -93,7 +94,11 @@ class ActionClient : public rclcpp_action::Client { void cancel_goal() { std::lock_guard lock(this->goal_handle_mutex); if (this->goal_handle) { - this->async_cancel_goal(this->goal_handle); + this->async_cancel_goal(this->goal_handle, + std::bind(&ActionClient::cancel_done, this)); + + std::unique_lock lock(this->action_done_mutex); + this->action_done_cond.wait(lock); } } @@ -118,8 +123,8 @@ class ActionClient : public rclcpp_action::Client { } bool is_working() { - return !this->is_terminated() && - this->get_status() != rclcpp_action::ResultCode::UNKNOWN; + std::lock_guard lock(this->goal_handle_mutex); + return this->goal_handle != nullptr; } bool is_terminated() { @@ -130,6 +135,9 @@ class ActionClient : public rclcpp_action::Client { std::condition_variable action_done_cond; std::mutex action_done_mutex; + std::condition_variable cancel_done_cond; + std::mutex cancel_done_mutex; + Result result; rclcpp_action::ResultCode status; std::mutex status_mtx; @@ -155,6 +163,8 @@ class ActionClient : public rclcpp_action::Client { this->set_status(result.code); this->action_done_cond.notify_one(); } + + void cancel_done() { this->action_done_cond.notify_all(); } }; } // namespace actions diff --git a/simple_node/include/simple_node/actions/action_server.hpp b/simple_node/include/simple_node/actions/action_server.hpp index 8afb292..36cd1c9 100644 --- a/simple_node/include/simple_node/actions/action_server.hpp +++ b/simple_node/include/simple_node/actions/action_server.hpp @@ -88,6 +88,7 @@ class ActionServer : public rclcpp_action::Server { private: UserExecuteCallback execute_callback; UserCancelCallback cancel_callback; + std::unique_ptr cancel_thread; std::mutex handle_accepted_mtx; rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, @@ -113,13 +114,20 @@ class ActionServer : public rclcpp_action::Server { rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { (void)goal_handle; + + this->cancel_thread = std::make_unique(this->cancel_callback); + return rclcpp_action::CancelResponse::ACCEPT; } void handle_execute(const std::shared_ptr goal_handle) { - + this->cancel_thread = nullptr; this->execute_callback(this->goal_handle); this->goal_handle = nullptr; + + if (this->cancel_thread != nullptr) { + this->cancel_thread->join(); + } } }; } // namespace actions diff --git a/simple_node/simple_node/actions/action_client.py b/simple_node/simple_node/actions/action_client.py index 6d48947..b73beb3 100644 --- a/simple_node/simple_node/actions/action_client.py +++ b/simple_node/simple_node/actions/action_client.py @@ -34,6 +34,7 @@ def __init__( ) -> None: self._action_done_event = Event() + self._cancel_done_event = Event() self._result = None self._status = GoalStatus.STATUS_UNKNOWN @@ -69,7 +70,8 @@ def is_aborted(self) -> bool: return self.get_status() == GoalStatus.STATUS_ABORTED def is_working(self) -> bool: - return not self.is_terminated() and self.get_status() != GoalStatus.STATUS_UNKNOWN + with self._goal_handle_lock: + return self._goal_handle is not None def is_terminated(self) -> bool: return (self.is_succeeded() or self.is_canceled() or self.is_aborted()) @@ -83,6 +85,8 @@ def get_result(self) -> Any: def send_goal(self, goal, feedback_cb: Callable = None) -> None: + with self._goal_handle_lock: + self._goal_handle = None self._result = None self._set_status(GoalStatus.STATUS_UNKNOWN) @@ -108,4 +112,13 @@ def _get_result_callback(self, future) -> None: def cancel_goal(self) -> None: with self._goal_handle_lock: if self._goal_handle is not None: - self._goal_handle.cancel_goal() + + cancel_goal_future = self._cancel_goal_async( + self._goal_handle) + cancel_goal_future.add_done_callback(self._cancel_done) + + self._cancel_done_event.clear() + self._cancel_done_event.wait() + + def _cancel_done(self, future) -> None: + self._cancel_done_event.set() diff --git a/simple_node/simple_node/actions/action_server.py b/simple_node/simple_node/actions/action_server.py index 6132f60..6edee7b 100644 --- a/simple_node/simple_node/actions/action_server.py +++ b/simple_node/simple_node/actions/action_server.py @@ -16,8 +16,8 @@ """ Custom action server that add goals to a queue """ -from threading import Lock from typing import Callable +from threading import Lock, Thread from rclpy.node import Node from rclpy.action import ActionServer as ActionServer2 @@ -41,6 +41,7 @@ def __init__( self.__goal_lock = Lock() self.__user_execute_callback = execute_callback self.__user_cancel_callback = cancel_callback + self.__cancel_thread = None self._goal_handle = None self.node = node @@ -78,6 +79,10 @@ def __handle_accepted_callback(self, goal_handle: ServerGoalHandle) -> None: def __cancel_callback(self, goal_handle: ServerGoalHandle) -> int: """ cancel calback """ + if self.__user_cancel_callback is not None: + self.__cancel_thread = Thread(target=self.__user_cancel_callback) + self.__cancel_thread.start() + return CancelResponse.ACCEPT def __execute_callback(self, goal_handle: ServerGoalHandle): @@ -85,11 +90,11 @@ def __execute_callback(self, goal_handle: ServerGoalHandle): execute callback """ + self.__cancel_thread = None results = self.__user_execute_callback(self._goal_handle) - self._goal_handle = None - if goal_handle.is_cancel_requested: - if self.__user_cancel_callback is not None: - self.__user_cancel_callback() + if self.__cancel_thread is not None: + self.__cancel_thread.join() + self._goal_handle = None return results