From c91b9991048ae0840f54e5dbb847333295958d14 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Otgaar?= <b.r.c.otgaar@students.uu.nl>
Date: Thu, 8 Jan 2026 15:31:44 +0100
Subject: [PATCH] chore: fix bugs and make sure connected robots work

---
 .../agents/actuation/robot_gesture_agent.py   |  2 ++
 .../communication/ri_communication_agent.py   | 20 ++++++++++++++++++-
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/src/control_backend/agents/actuation/robot_gesture_agent.py b/src/control_backend/agents/actuation/robot_gesture_agent.py
index 4f5dd79..5e40c04 100644
--- a/src/control_backend/agents/actuation/robot_gesture_agent.py
+++ b/src/control_backend/agents/actuation/robot_gesture_agent.py
@@ -83,6 +83,8 @@ class RobotGestureAgent(BaseAgent):
             self.subsocket.close()
         if self.pubsocket:
             self.pubsocket.close()
+        if self.repsocket:
+            self.repsocket.close()
         await super().stop()
 
     async def handle_message(self, msg: InternalMessage):
diff --git a/src/control_backend/agents/communication/ri_communication_agent.py b/src/control_backend/agents/communication/ri_communication_agent.py
index 45e3511..7fcd07b 100644
--- a/src/control_backend/agents/communication/ri_communication_agent.py
+++ b/src/control_backend/agents/communication/ri_communication_agent.py
@@ -48,6 +48,8 @@ class RICommunicationAgent(BaseAgent):
         self._req_socket: azmq.Socket | None = None
         self.pub_socket: azmq.Socket | None = None
         self.connected = False
+        self.gesture_agent: RobotGestureAgent | None = None
+        self.speech_agent: RobotSpeechAgent | None = None
 
     async def setup(self):
         """
@@ -141,6 +143,7 @@ class RICommunicationAgent(BaseAgent):
 
             # At this point, we have a valid response
             try:
+                self.logger.debug("Negotiation successful. Handling rn")
                 await self._handle_negotiation_response(received_message)
                 # Let UI know that we're connected
                 topic = b"ping"
@@ -189,6 +192,7 @@ class RICommunicationAgent(BaseAgent):
                         address=addr,
                         bind=bind,
                     )
+                    self.speech_agent = robot_speech_agent
                     robot_gesture_agent = RobotGestureAgent(
                         settings.agent_settings.robot_gesture_name,
                         address=addr,
@@ -196,6 +200,7 @@ class RICommunicationAgent(BaseAgent):
                         gesture_data=gesture_data,
                         single_gesture_data=single_gesture_data,
                     )
+                    self.gesture_agent = robot_gesture_agent
                     await robot_speech_agent.start()
                     await asyncio.sleep(0.1)  # Small delay
                     await robot_gesture_agent.start()
@@ -226,6 +231,7 @@ class RICommunicationAgent(BaseAgent):
         while self._running:
             if not self.connected:
                 await asyncio.sleep(settings.behaviour_settings.sleep_s)
+                self.logger.debug("Not connected, skipping ping loop iteration.")
                 continue
 
             # We need to listen and send pings.
@@ -289,15 +295,27 @@ class RICommunicationAgent(BaseAgent):
         # Tell UI we're disconnected.
         topic = b"ping"
         data = json.dumps(False).encode()
+        self.logger.debug("1")
         if self.pub_socket:
             try:
+                self.logger.debug("2")
                 await asyncio.wait_for(self.pub_socket.send_multipart([topic, data]), 5)
             except TimeoutError:
+                self.logger.debug("3")
                 self.logger.warning("Connection ping for router timed out.")
 
         # Try to reboot/renegotiate
+        if self.gesture_agent is not None:
+            await self.gesture_agent.stop()
+
+        if self.speech_agent is not None:
+            await self.speech_agent.stop()
+
+        if self.pub_socket is not None:
+            self.pub_socket.close()
+
         self.logger.debug("Restarting communication negotiation.")
-        if await self._negotiate_connection(max_retries=1):
+        if await self._negotiate_connection(max_retries=2):
             self.connected = True
 
     async def handle_message(self, msg: InternalMessage):