Merge branch 'feat/visual-emotion-recognition' of https://git.science.uu.nl/ics/sp/2025/n25b/pepperplus-cb into feat/face-recognition

src/control_backend/agents/communication/ri_communication_agent.py

@@ -7,6 +7,9 @@ from zmq.asyncio import Context
 
 from control_backend.agents import BaseAgent
 from control_backend.agents.actuation.robot_gesture_agent import RobotGestureAgent
+from control_backend.agents.perception.visual_emotion_recognition_agent.visual_emotion_recognition_agent import (  # noqa
+    VisualEmotionRecognitionAgent,
+)
 from control_backend.core.config import settings
 
 from ..actuation.robot_speech_agent import RobotSpeechAgent
@@ -47,6 +50,9 @@ 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
+        self.visual_emotion_recognition_agent: VisualEmotionRecognitionAgent | None = None
 
     async def setup(self):
         """
@@ -140,6 +146,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"
@@ -188,6 +195,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,
@@ -195,12 +203,21 @@ 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()
                 case "audio":
                     vad_agent = VADAgent(audio_in_address=addr, audio_in_bind=bind)
                     await vad_agent.start()
+                case "video":
+                    visual_emotion_agent = VisualEmotionRecognitionAgent(
+                        settings.agent_settings.visual_emotion_recognition_name,
+                        socket_address=addr,
+                        bind=bind,
+                    )
+                    self.visual_emotion_recognition_agent = visual_emotion_agent
+                    await visual_emotion_agent.start()
                 case "face":
                     self.logger.warn("yup we here")
                     face_agent = FacePerceptionAgent(
@@ -233,6 +250,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.
@@ -256,7 +274,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
@@ -296,13 +315,28 @@ 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.visual_emotion_recognition_agent is not None:
+            await self.visual_emotion_recognition_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