From cc7bd99dd2f0441989c352d90bae4a4bd545278a Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Wed, 13 Dec 2023 15:02:34 -0500
Subject: [PATCH] Add action cancellation callback

---
 .../capabilities/advertise_action.py              | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py
index fb4027cae..e94ae83fb 100644
--- a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py
+++ b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py
@@ -35,6 +35,7 @@
 
 import rclpy
 from rclpy.action import ActionServer
+from rclpy.action.server import CancelResponse, ServerGoalHandle
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rosbridge_library.capability import Capability
 from rosbridge_library.internal import message_conversion
@@ -62,6 +63,7 @@ def __init__(
             get_action_class(action_type),
             action_name,
             self.execute_callback,
+            cancel_callback=self.cancel_callback,
             callback_group=ReentrantCallbackGroup(),  # https://github.com/ros2/rclpy/issues/834#issuecomment-961331870
         )
 
@@ -71,6 +73,7 @@ def next_id(self) -> int:
         return id
 
     async def execute_callback(self, goal: Any) -> Any:
+        """Action server goal callback function."""
         # generate a unique ID
         goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
 
@@ -103,6 +106,14 @@ def done_callback(fut: rclpy.task.Future()) -> None:
             del self.goal_futures[goal_id]
             del self.goal_handles[goal_id]
 
+    def cancel_callback(self, cancel_request: ServerGoalHandle) -> CancelResponse:
+        """Action server cancel callback function."""
+        for goal_id, goal_handle in self.goal_handles.items():
+            if cancel_request.goal_id == goal_handle.goal_id:
+                self.protocol.log("warning", f"Canceling action {goal_id}")
+                self.goal_futures[goal_id].cancel()
+        return CancelResponse.ACCEPT
+
     def handle_feedback(self, goal_id: str, feedback: Any) -> None:
         """
         Called by the ActionFeedback capability to handle action feedback from the external client.
@@ -147,6 +158,10 @@ def graceful_shutdown(self) -> None:
                 future = self.goal_futures[future_id]
                 future.set_exception(RuntimeError(f"Action {self.action_name} was unadvertised"))
 
+        # Uncommenting this, you may get a segfault.
+        # See https://github.com/ros2/rclcpp/issues/2163#issuecomment-1850925883
+        # self.action_server.destroy()
+
 
 class AdvertiseAction(Capability):
     actions_glob = None