Added a client for the new action server to the main control node

Author: Isopod00

src/rovr_control/launch/main_no_joysticks_launch.py

@@ -77,6 +77,12 @@ def generate_launch_description():
         name="auto_offload_server",
     )
 
+    auto_dig_nav_offload_server = Node(
+        package="rovr_control",
+        executable="auto_dig_nav_offload_server",
+        name="auto_dig_nav_offload_server",
+    )
+
     ld.add_action(rovr_control)
     ld.add_action(motor_control)
     ld.add_action(drivetrain)
@@ -87,5 +93,6 @@ def generate_launch_description():
     ld.add_action(calibrate_field_coordinate_server)
     ld.add_action(auto_dig_server)
     ld.add_action(auto_offload_server)
+    ld.add_action(auto_dig_nav_offload_server)
 
     return ld

src/rovr_control/rovr_control/main_control_node.py

@@ -21,7 +21,7 @@
 
 # Import custom ROS 2 interfaces
 from rovr_interfaces.srv import SetPower, SetPosition
-from rovr_interfaces.action import CalibrateFieldCoordinates, AutoDig, AutoOffload
+from rovr_interfaces.action import CalibrateFieldCoordinates, AutoDig, AutoOffload, AutoDigNavOffload
 from std_srvs.srv import Trigger
 
 # Import Python Modules
@@ -134,10 +134,12 @@ def __init__(self) -> None:
         )
         self.act_auto_dig = ActionClient(self, AutoDig, "auto_dig")
         self.act_auto_offload = ActionClient(self, AutoOffload, "auto_offload")
+        self.act_auto_dig_nav_offload = ActionClient(self, AutoDigNavOffload, "auto_dig_nav_offload")
 
         self.field_calibrated_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
         self.auto_dig_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
         self.auto_offload_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
+        self.auto_dig_nav_offload_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
 
         # Current position of the lift motor in potentiometer units (0 to 1023)
         self.current_lift_position = None  # We don't know the current position yet
@@ -220,28 +222,56 @@ async def joystick_callback(self, msg: Joy) -> None:
 
         # THE CONTROLS BELOW ALWAYS WORK #
 
-        # Check if the Apriltag calibration button is pressed
+        # # Check if the Apriltag calibration button is pressed
+        # # TODO: This autonomous action needs to be tested on the physical robot!
+        # if msg.buttons[bindings.A_BUTTON] == 1 and buttons[bindings.A_BUTTON] == 0:
+        #     # Check if the field calibration process is not running
+        #     if self.field_calibrated_handle.status != GoalStatus.STATUS_EXECUTING:
+        #         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.stop_all_subsystems()
+        #         self.field_calibrated_handle = await self.act_calibrate_field_coordinates.send_goal_async(
+        #             CalibrateFieldCoordinates.Goal()
+        #         )
+        #         if not self.field_calibrated_handle.accepted:
+        #             self.get_logger().info("Field calibration Goal rejected")
+        #             return
+        #         self.field_calibrated_handle.get_result_async().add_done_callback(self.get_result_callback)
+        #         self.state = states["Autonomous"]
+        #     # Terminate the field calibration process
+        #     else:
+        #         self.get_logger().warn("Field Calibration Terminated")
+        #         # Cancel the goal
+        #         future = self.field_calibrated_handle.cancel_goal_async()
+        #         future.add_done_callback(self.cancel_done)
+
+        # Check if the Auto Dig Nav button is pressed
         # TODO: This autonomous action needs to be tested on the physical robot!
         if msg.buttons[bindings.A_BUTTON] == 1 and buttons[bindings.A_BUTTON] == 0:
-            # Check if the field calibration process is not running
-            if self.field_calibrated_handle.status != GoalStatus.STATUS_EXECUTING:
-                if not self.act_calibrate_field_coordinates.wait_for_server(timeout_sec=1.0):
-                    self.get_logger().error("Field calibration action not available")
+            # Check if the Auto Dig Nav Offload process is not running
+            if self.auto_dig_nav_offload_handle.status != GoalStatus.STATUS_EXECUTING:
+                if not self.act_auto_dig_nav_offload.wait_for_server(timeout_sec=1.0):
+                    self.get_logger().error("Auto Dig Nav Offload action not available")
                     return
                 self.stop_all_subsystems()
-                self.field_calibrated_handle = await self.act_calibrate_field_coordinates.send_goal_async(
-                    CalibrateFieldCoordinates.Goal()
+                self.auto_dig_nav_offload_handle = await self.act_auto_dig_nav_offload.send_goal_async(
+                    AutoDigNavOffload.Goal(
+                        lift_digging_start_position=self.lift_digging_start_position,
+                        digger_chain_power=self.digger_chain_power,
+                        backward_distance=0.2,  # meters # TODO: Tune this distance!
+                    )
                 )
-                if not self.field_calibrated_handle.accepted:
-                    self.get_logger().info("Field calibration Goal rejected")
+                if not self.auto_dig_nav_offload_handle.accepted:
+                    self.get_logger().info("Auto Dig Nav Offload Goal rejected")
                     return
-                self.field_calibrated_handle.get_result_async().add_done_callback(self.get_result_callback)
+                self.auto_dig_nav_offload_handle.get_result_async().add_done_callback(self.get_result_callback)
                 self.state = states["Autonomous"]
-            # Terminate the field calibration process
+            # Terminate the Auto Dig Nav Offload process
             else:
-                self.get_logger().warn("Field Calibration Terminated")
+                self.get_logger().warn("Auto Dig Nav Offload Terminated")
                 # Cancel the goal
-                future = self.field_calibrated_handle.cancel_goal_async()
+                future = self.auto_dig_nav_offload_handle.cancel_goal_async()
                 future.add_done_callback(self.cancel_done)
 
         # Check if the autonomous digging button is pressed