fix: add turn action cancel logic

uppaal2jetracer/controller/hardware_controller.py

@@ -122,6 +122,7 @@ class JRHardwareController(HardwareController):
         """
 
         self.set_speed(0.0)
+        self._turn_controller.stop()
 
     def turn(self, theta: float) -> HardwareResult[None]:
         """

uppaal2jetracer/jetracerros2/jetracerros2/jetracer_controller.py

@@ -15,7 +15,7 @@ import rclpy
 from rclpy.node import Node
 from rclpy.publisher import Publisher
 from rclpy.service import Service
-from rclpy.action import ActionServer
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.executors import MultiThreadedExecutor
 
 from geometry_msgs.msg import Twist
@@ -52,7 +52,10 @@ class JetRacerController(Node):
             Speed, "set_speed", self._speed_service_callback)
 
         self._turn_action_server: ActionServer = ActionServer(
-            self, Turn, "turn", self._execute_turn_callback)
+            self, Turn, "turn",
+            execute_callback = self._execute_turn_callback,
+            goal_callback = self._goal_turn_callback,
+            cancel_callback = self._cancel_turn_callback)
 
     def _speed_service_callback(self, request, response):
         self._speed = request.speed
@@ -68,15 +71,14 @@ class JetRacerController(Node):
             goal_handle.succeed()
             return Turn.Result()
 
-        self._execute_turn(request.theta)
+        self._execute_turn(request.theta, goal_handle)
 
         result = Turn.Result()
         result.turned = request.theta
-        goal_handle.succeed()
 
         return result
 
-    def _execute_turn(self, theta: float):
+    def _execute_turn(self, theta: float, goal_handle):
         old_speed = self._speed
 
         if self._speed == 0:
@@ -95,7 +97,15 @@ class JetRacerController(Node):
 
         self._publish_to_cmd_vel()
 
-        time.sleep(turn_time)
+        start_time = time.time()
+
+        while time.time() - start_time < turn_time:
+            if goal_handle.is_cancel_requested:
+                self._speed = 0.0
+                self._steering_angle = 0.0
+                self._publish_to_cmd_vel()
+                goal_handle.canceled()
+                return
 
         self._steering_angle = 0.0
         if self._speed != 0.0:
@@ -103,6 +113,15 @@ class JetRacerController(Node):
 
         self._publish_to_cmd_vel()
 
+        goal_handle.succeed()
+
+
+    def _cancel_turn_callback(self, goal_handle):
+        return CancelResponse.ACCEPT
+
+    def _goal_turn_callback(self, goal_handle):
+        return GoalResponse.ACCEPT
+
     def _publish_timer_callback(self):
         self._publish_to_cmd_vel()
 

uppaal2jetracer/jetracerros2/jetracerros2/turn_controller.py

@@ -35,13 +35,15 @@ class TurnActionClientAsync(Node):
     :vartype _status: GoalStatus
     """
 
-    __slots__ = ["_turn_action_client_async", "_result", "_status"]
+    __slots__ = ["_turn_action_client_async", "_result", "_status",
+                 "_goal_handle"]
 
     def __init__(self):
         super().__init__("turn_action_client", namespace = "/jetracerros2")
         self._turn_action_client = ActionClient(self, Turn, "turn")
         self._result = Turn.Result()
         self._status = GoalStatus.STATUS_UNKNOWN
+        self._goal_handle = None
 
     @property
     def result(self) -> Turn.Result:
@@ -72,9 +74,7 @@ class TurnActionClientAsync(Node):
         This method should only be called after the turn action has finished!
         """
 
-        if self._status == GoalStatus.STATUS_SUCCEEDED:
-            self._status = GoalStatus.STATUS_UNKNOWN
-            logger.debug("Cleanup complete.")
+        self.status = GoalStatus.STATUS_UNKNOWN
 
     def send_turn_goal_async(self, theta: float):
         """
@@ -92,9 +92,29 @@ class TurnActionClientAsync(Node):
         self._turn_action_client.wait_for_server()
         logger.debug("Sending %f turn request async!", theta)
         send_goal_future = self._turn_action_client.send_goal_async(goal)
-
+        self._status = GoalStatus.STATUS_UNKNOWN
         send_goal_future.add_done_callback(self._goal_response_callback)
 
+    def cancel_turn_goal_async(self):
+        """
+        Cancel the turn goal.
+        """
+        if not self._goal_handle:
+            logger.warning("No goal handle to cancel!")
+            return
+
+        cancel_future = self._goal_handle.cancel_goal_async()
+        cancel_future.add_done_callback(self._cancel_response_callback)
+
+    def is_turn_goal_active(self) -> bool:
+        """
+        Check if the turn goal is active.
+
+        :return: True if the turn goal is active, False otherwise.
+        """
+        return (self._status != GoalStatus.STATUS_SUCCEDDED
+                and self._status != GoalStatus.STATUS_ABORTED)
+
     def _goal_response_callback(self, future):
         goal_handle = future.result()
         if not goal_handle.accepted:
@@ -103,6 +123,7 @@ class TurnActionClientAsync(Node):
 
         self._status = GoalStatus.STATUS_ACCEPTED
         logger.debug("Turn request accepted!")
+        self._goal_handle = goal_handle
         self._get_result_future = goal_handle.get_result_async()
         self._get_result_future.add_done_callback(self._get_result_callback)
 
@@ -111,19 +132,31 @@ class TurnActionClientAsync(Node):
         self._status = GoalStatus.STATUS_SUCCEEDED
         logger.debug("Turn request marked as succeeded!")
 
+    def _cancel_response_callback(self, future):
+        if future.result():
+            logger.debug("Turn request canceled!")
+            self._status = GoalStatus.STATUS_ABORTED
+
 
 class TurnController:
     """
     This class is used to send a goal to the turn action server and wait for the result.
     """
 
-    __slots__ = ["_turn_action_client"]
+    __slots__ = ["_turn_action_client",]
 
     def __init__(self):
         if not rclpy.ok():
             rclpy.init()
         self._turn_action_client = TurnActionClientAsync()
 
+    def stop(self):
+        """
+        Stop the JetRacer.
+        """
+        self._turn_action_client.cancel_turn_goal_async()
+        self._turn_action_client.cleanup()
+
     def shutdown(self):
         """
         Shutdown the TurnController and destroy the node.
@@ -144,12 +177,8 @@ class TurnController:
 
         self._turn_action_client.send_turn_goal_async(theta)
 
-        try:
-            while self._turn_action_client.status != GoalStatus.STATUS_SUCCEEDED and rclpy.ok():
-                logger.debug("Waiting for Turn action request %f to succeed.", theta)
-                rclpy.spin_once(self._turn_action_client)
-        except KeyboardInterrupt:
-            logger.warning("Turn action request %f was interrupted!", theta)
-            raise
+        while (self._turn_action_client.is_turn_goal_active() and rclpy.ok()):
+            logger.debug("Waiting for Turn action request %f to succeed.", theta)
+            rclpy.spin_once(self._turn_action_client)
 
         self._turn_action_client.cleanup()