fix: add result check in succeeded

uppaal2jetracer/jetracerros2/jetracerros2/jetracer_controller.py

@@ -71,14 +71,14 @@ class JetRacerController(Node):
             goal_handle.succeed()
             return Turn.Result()
 
-        self._execute_turn(request.theta, goal_handle)
+        result_turned: float = self._execute_turn(request.theta, goal_handle)
 
         result = Turn.Result()
-        result.turned = request.theta
+        result.turned = result_turned
 
         return result
 
-    def _execute_turn(self, theta: float, goal_handle):
+    def _execute_turn(self, theta: float, goal_handle) -> float:
         old_speed = self._speed
 
         if self._speed == 0:
@@ -105,7 +105,7 @@ class JetRacerController(Node):
                 self._steering_angle = 0.0
                 self._publish_to_cmd_vel()
                 goal_handle.canceled()
-                return
+                return 0.0
 
         self._steering_angle = 0.0
         if self._speed != 0.0:
@@ -114,6 +114,7 @@ class JetRacerController(Node):
         self._publish_to_cmd_vel()
 
         goal_handle.succeed()
+        return theta
 
 
     def _cancel_turn_callback(self, goal_handle):

uppaal2jetracer/jetracerros2/jetracerros2/turn_controller.py

@@ -135,7 +135,9 @@ class TurnActionClientAsync(Node):
 
     def _get_result_callback(self, future):
         self._result = future.result().result
-        self._status = GoalStatus.STATUS_SUCCEEDED
+
+        if self._result.turned != 0.0:
+            self._status = GoalStatus.STATUS_SUCCEEDED
         logger.debug("Turn request marked as succeeded!")
 
     def _cancel_response_callback(self, future):
@@ -181,6 +183,10 @@ class TurnController:
         :type theta: float
         """
 
+        if theta == 0.0:
+            self._turn_action_client.cleanup()
+            return
+
         self._turn_action_client.send_turn_goal_async(theta)
 
         while self._turn_action_client.is_turn_goal_active():