Merge remote-tracking branch 'origin/feat/reset-experiment-and-phase' into feat/visual-emotion-recognition

src/control_backend/agents/communication/ri_communication_agent.py

@@ -3,6 +3,7 @@ import json
 
 import zmq
 import zmq.asyncio as azmq
+from pydantic import ValidationError
 from zmq.asyncio import Context
 
 from control_backend.agents import BaseAgent
@@ -11,6 +12,8 @@ from control_backend.agents.perception.visual_emotion_recognition_agent.visual_e
     VisualEmotionRecognitionAgent,
 )
 from control_backend.core.config import settings
+from control_backend.schemas.internal_message import InternalMessage
+from control_backend.schemas.ri_message import PauseCommand
 
 from ..actuation.robot_speech_agent import RobotSpeechAgent
 from ..perception import VADAgent
@@ -50,6 +53,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):
         """
@@ -143,6 +148,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"
@@ -191,6 +197,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,
@@ -198,6 +205,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()
@@ -235,6 +243,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.
@@ -258,7 +267,8 @@ class RICommunicationAgent(BaseAgent):
                     self._req_socket.recv_json(), timeout=seconds_to_wait_total / 2
                 )
 
-                self.logger.debug(f'Received message "{message}" from RI.')
+                if "endpoint" in message and message["endpoint"] != "ping":
+                    self.logger.debug(f'Received message "{message}" from RI.')
                 if "endpoint" not in message:
                     self.logger.warning("No received endpoint in message, expected ping endpoint.")
                     continue
@@ -298,13 +308,33 @@ 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):
+        try:
+            pause_command = PauseCommand.model_validate_json(msg.body)
+            self._req_socket.send_json(pause_command.model_dump())
+            self.logger.debug(self._req_socket.recv_json())
+        except ValidationError:
+            self.logger.warning("Incorrect message format for PauseCommand.")