actions fixed

Author: mgonzs13

simple_node/include/simple_node/actions/action_client.hpp

@@ -68,6 +68,7 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
 
   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<ActionT> {
   void cancel_goal() {
     std::lock_guard<std::mutex> 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<std::mutex> lock(this->action_done_mutex);
+      this->action_done_cond.wait(lock);
     }
   }
 
@@ -118,8 +123,8 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
   }
 
   bool is_working() {
-    return !this->is_terminated() &&
-           this->get_status() != rclcpp_action::ResultCode::UNKNOWN;
+    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
+    return this->goal_handle != nullptr;
   }
 
   bool is_terminated() {
@@ -130,6 +135,9 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
   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<ActionT> {
     this->set_status(result.code);
     this->action_done_cond.notify_one();
   }
+
+  void cancel_done() { this->action_done_cond.notify_all(); }
 };
 
 } // namespace actions

simple_node/include/simple_node/actions/action_server.hpp

@@ -88,6 +88,7 @@ class ActionServer : public rclcpp_action::Server<ActionT> {
 private:
   UserExecuteCallback execute_callback;
   UserCancelCallback cancel_callback;
+  std::unique_ptr<std::thread> 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<ActionT> {
   rclcpp_action::CancelResponse
   handle_cancel(const std::shared_ptr<GoalHandle> goal_handle) {
     (void)goal_handle;
+
+    this->cancel_thread = std::make_unique<std::thread>(this->cancel_callback);
+
     return rclcpp_action::CancelResponse::ACCEPT;
   }
 
   void handle_execute(const std::shared_ptr<GoalHandle> 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

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()

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,18 +79,22 @@ 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):
         """
             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