From 984a6a25912fab19cdc5bb3b7ca3294115103344 Mon Sep 17 00:00:00 2001
From: Louis Fink <uutyc@student.kit.edu>
Date: Fri, 14 Mar 2025 16:56:12 +0100
Subject: [PATCH] fix: add turn action cancel logic
---
.../controller/hardware_controller.py | 1 +
.../jetracerros2/jetracer_controller.py | 31 +++++++++--
.../jetracerros2/turn_controller.py | 55 ++++++++++++++-----
3 files changed, 68 insertions(+), 19 deletions(-)
diff --git a/uppaal2jetracer/controller/hardware_controller.py b/uppaal2jetracer/controller/hardware_controller.py
index 5b12ba1..856a9ac 100644
--- a/uppaal2jetracer/controller/hardware_controller.py
+++ b/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]:
"""
diff --git a/uppaal2jetracer/jetracerros2/jetracerros2/jetracer_controller.py b/uppaal2jetracer/jetracerros2/jetracerros2/jetracer_controller.py
index 234bd99..97db157 100644
--- a/uppaal2jetracer/jetracerros2/jetracerros2/jetracer_controller.py
+++ b/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()
diff --git a/uppaal2jetracer/jetracerros2/jetracerros2/turn_controller.py b/uppaal2jetracer/jetracerros2/jetracerros2/turn_controller.py
index 6b0ccff..8ebc4e5 100644
--- a/uppaal2jetracer/jetracerros2/jetracerros2/turn_controller.py
+++ b/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()
--
GitLab