From 778e9753f747e4cf7c8044c54969d748a018d8b6 Mon Sep 17 00:00:00 2001
From: andrew-aj <andrew.knee.j@gmail.com>
Date: Fri, 8 Nov 2024 07:01:02 -0500
Subject: [PATCH] switched out ball launcher service

---
 .../navigator_missions/docking.py             |  2 ++
 .../navigator_missions/navigator.py           | 33 +++++++++++--------
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py b/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py
index 12a4b1e36..5b0340c56 100644
--- a/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py
+++ b/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py
@@ -260,6 +260,8 @@ async def run(self, args):
                 await self.fire_launcher()
                 await self.nh.sleep(2)
 
+            await self.stop_launcher()
+
         await self.move.backward(10).go(blind=True)
         await self.pcodar_save(SetBoolRequest(False))
 
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py
index 10256c954..59ea23f97 100644
--- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py
+++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py
@@ -28,8 +28,10 @@
 from roboteq_msgs.msg import Command
 from ros_alarms import TxAlarmListener
 from sensor_msgs.msg import CameraInfo, Image
-from std_msgs.msg import Bool, Empty
+from std_msgs.msg import Bool
+from std_srvs.srv import Empty as Emptysrv
 from std_srvs.srv import (
+    EmptyRequest,
     SetBool,
     SetBoolRequest,
     SetBoolResponse,
@@ -211,6 +213,16 @@ def enu_odom_set(odom):
             cls._grind_motor_pub.setup(),
         )
 
+        cls._ball_spin_srv = cls.nh.get_service_client("/ball_launcher/spin", SetBool)
+        cls._ball_launch_srv = cls.nh.get_service_client(
+            "/ball_launcher/drop_ball",
+            Emptysrv,
+        )
+        await asyncio.gather(
+            cls._ball_spin_srv.setup(),
+            cls._ball_launch_srv.setup(),
+        )
+
         try:
             cls._actuator_client = cls.nh.get_service_client(
                 "/actuator_driver/actuate",
@@ -267,12 +279,6 @@ def enu_odom_set(odom):
             "Odom listener",
         )
 
-        cls._ball_launcher_pub = cls.nh.advertise(
-            "/wamv/shooters/ball_shooter/fire",
-            Empty,
-        )
-        await cls._ball_launcher_pub.setup()
-
         if not cls.sim:
             await util.wrap_time_notice(
                 cls._ecef_odom_sub.get_next_message(),
@@ -317,6 +323,8 @@ async def _shutdown_not_vrx(cls):
             cls.front_right_camera_info_sub.shutdown(),
             cls.yolo_objects.shutdown(),
             cls.stc_objects.shutdown(),
+            cls._ball_spin_srv.shutdown(),
+            cls._ball_launch_srv.shutdown(),
         )
 
     @classmethod
@@ -541,17 +549,16 @@ async def reload_launcher(self):
         self.launcher_state = "inactive"
 
     async def start_launcher(self):
-        await self.nh.sleep(0.5)
+        await self._ball_spin_srv(SetBoolRequest(data=True))
+
+    async def stop_launcher(self):
+        await self._ball_spin_srv(SetBoolRequest(data=False))
 
     async def fire_launcher(self):
         if self.launcher_state != "inactive":
             raise Exception(f"Launcher is {self.launcher_state}")
         self.launcher_state = "firing"
-        if self.sim:
-            pass
-            # await self._ball_launcher_pub.publish(Empty())
-        else:
-            await self.set_valve("LAUNCHER_FIRE", True)
+        await self._ball_launch_srv(EmptyRequest())
         await self.nh.sleep(0.5)
         self.launcher_state = "inactive"