Code completed

src/rovr_control/rovr_control/calibrate_field_coordinates.py

@@ -3,6 +3,8 @@
 from nav2_msgs.action import Spin
 import rclpy
 from rclpy.action import ActionServer, ActionClient
+from rclpy.action.client import ClientGoalHandle
+from rclpy.action.server import ServerGoalHandle
 from rclpy.client import Future
 from rclpy.node import Node
 
@@ -27,7 +29,7 @@ def __init__(self):
         self.cli_spin = ActionClient(self, Spin, "spin")
         self.future_spin = Future()
 
-    def execute_callback(self, goal_handle: CalibrateFieldCoordinates.Goal):
+    def execute_callback(self, goal_handle: ServerGoalHandle):
         """This method rotates until we can see apriltag(s) and then sets the map -> odom tf."""
         self.get_logger().info("Executing goal...")
         result = CalibrateFieldCoordinates.Result()
@@ -45,7 +47,13 @@ def execute_callback(self, goal_handle: CalibrateFieldCoordinates.Goal):
         for i in range(1, 3):
             # Spin around to find the apriltag
             spin_goal = Spin.Goal(target_yaw=(i * math.pi))
-            self.future_spin = self.cli_spin.send_goal_async(spin_goal)
+            spin_request = self.cli_spin.send_goal_async(spin_goal)
+            rclpy.spin_until_future_complete(self, spin_request)
+            spin_handle:ClientGoalHandle = spin_request.result()
+            if not spin_handle.accepted:
+                self.get_logger().error("Spin request was rejected")
+                return result
+            self.future_spin: Future = spin_handle.get_result_async()
             self.future_odom = Future()
             # Keep trying to set the map -> odom TF until it is successful
             while (
@@ -63,18 +71,28 @@ def execute_callback(self, goal_handle: CalibrateFieldCoordinates.Goal):
                 self.get_logger().info("map -> odom TF published!")
                 self.future_spin.cancel()
                 break
-            elif i == 1:
+            elif i == 2:
                 self.get_logger().error("Failed to find apriltag")
                 return result
-
-        self.future_spin = self.cli_spin.send_goal_async(
-            Spin.Goal(target_yaw=(math.pi))
-        )
+        # Stop spinning since apriltag was found
+        self.future_spin.cancel()
+        if not self.cli_spin.wait_for_server(timeout_sec=1.0):
+            self.get_logger().error("Apriltag odom service not available")
+            return result
+        spin_goal = Spin.Goal(target_yaw=(i * math.pi))
+        spin_request = self.cli_spin.send_goal_async(spin_goal)
+        rclpy.spin_until_future_complete(self, spin_request)
+        spin_handle:ClientGoalHandle = spin_request.result()
+        if not spin_handle.accepted:
+            self.get_logger().error("Spin request was rejected")
+            return result
+        self.future_spin: Future = spin_handle.get_result_async()
         rclpy.spin_until_future_complete(self, self.future_spin)
         if self.future_spin.cancelled():
             self.get_logger().error("Final spin to field Spin canceled")
             return result
         result.success = True
+        goal_handle.succeed()
         return result
 
     def cancel_callback(self, cancel_request):

src/rovr_control/rovr_control/main_control_node.py

@@ -484,6 +484,9 @@ def joystick_callback(self, msg: Joy) -> None:
         if msg.buttons[START_BUTTON] == 1 and buttons[START_BUTTON] == 0:
             # Start the field calibration process
             if self.field_calibrated.done():
+                if not self.act_calibrate_field_coordinates.wait_for_server(timeout_sec=1.0):
+                    self.get_logger().error("Field calibration action not available")
+                    return
                 self.field_calibrated = self.act_calibrate_field_coordinates.send_goal_async(
                     CalibrateFieldCoordinates.Goal()
                 )