diff --git a/src/robot_interface/endpoints/face_detector.py b/src/robot_interface/endpoints/face_detector.py index ca53798..b45d088 100644 --- a/src/robot_interface/endpoints/face_detector.py +++ b/src/robot_interface/endpoints/face_detector.py @@ -1,65 +1,78 @@ from __future__ import unicode_literals +import json import logging import threading import time +import zmq from robot_interface.endpoints.socket_base import SocketBase from robot_interface.state import state +from robot_interface.core.config import settings class FaceDetectionSender(SocketBase): """ - Minimal face detection sender with a shared flag. + Face detection REP endpoint. - Polls ALMemory["FaceDetected"] and keeps a simple boolean - indicating if a face is currently detected. + - Polls ALMemory["FaceDetected"] + - Maintains a rolling boolean state + - Responds to REQ polling over ZMQ REP """ - def __init__(self, zmq_context, port=None): + def __init__(self, zmq_context, port=settings.agent_settings.face_detection_port): super(FaceDetectionSender, self).__init__("face") + if port: - self.create_socket(zmq_context, None, port) # PUB not used here + self.create_socket(zmq_context, zmq.REP, port) + self._face_service = None self._memory_service = None - self._thread = None - # Shared status for MainReceiver polling self.face_detected = False self._last_seen_face_time = 0 + self._face_thread = None + self._rep_thread = None + def endpoint_description(self): - return None + return "face" + + # ------------------------------------------------------------------ + # Face detection logic (runs independently) + # ------------------------------------------------------------------ def start_face_detection(self): if not state.qi_session: - logging.info("No Qi session available. Not starting face detection.") + logging.warning("No Qi session available. Face detection not started.") return import qi + self._face_service = state.qi_session.service("ALFaceDetection") self._memory_service = state.qi_session.service("ALMemory") - # Enable minimal detection self._face_service.setTrackingEnabled(False) self._face_service.setRecognitionEnabled(False) - # Required to activate extractor self._face_service.subscribe("FaceDetectionSender", 500, 0.0) - self._thread = threading.Thread(target=self._face_loop) - self._thread.daemon = True - self._thread.start() + self._face_thread = threading.Thread(target=self._face_loop, daemon=True) + self._face_thread.start() - logging.info("Face detection started.") + self._rep_thread = threading.Thread(target=self._rep_loop, daemon=True) + self._rep_thread.start() + + logging.info("Face detection + REP endpoint started.") def _face_loop(self): """ - Keeps the face detection alive until shutdown. + Continuously updates `self.face_detected`. """ while not state.exit_event.is_set(): try: value = self._memory_service.getData("FaceDetected", 0) + face_present = ( value and len(value) > 1 @@ -72,7 +85,7 @@ class FaceDetectionSender(SocketBase): if face_present: self._last_seen_face_time = now - # Consider face "lost" after 3s + # Face considered lost after 3 seconds self.face_detected = (now - self._last_seen_face_time) < 3 except Exception: @@ -80,6 +93,35 @@ class FaceDetectionSender(SocketBase): time.sleep(0.1) + # ------------------------------------------------------------------ + # REP loop (THIS WAS THE MISSING PIECE) + # ------------------------------------------------------------------ + + def _rep_loop(self): + """ + Handles ZMQ REQ/REP polling from the Control Backend. + """ + while not state.exit_event.is_set(): + try: + # Receive request (content ignored) + _ = self.socket.recv() + + # Respond with current face state + response = { + "endpoint": "face", + "data": self.face_detected, + } + + self.socket.send_json(response) + + except Exception: + logging.exception("Error in face detection REP loop") + time.sleep(0.1) + + # ------------------------------------------------------------------ + # Cleanup + # ------------------------------------------------------------------ + def stop_face_detection(self): try: if self._face_service: diff --git a/src/robot_interface/endpoints/main_receiver.py b/src/robot_interface/endpoints/main_receiver.py index c5341fd..29dba60 100644 --- a/src/robot_interface/endpoints/main_receiver.py +++ b/src/robot_interface/endpoints/main_receiver.py @@ -37,28 +37,6 @@ class MainReceiver(ReceiverBase): :rtype: dict[str, str | list[dict]] """ return {"endpoint": "ping", "data": message.get("data")} - - @staticmethod - def _handle_face(message): - """ - Handles sending face data to the cb - Sends if it sees a face or not - - :param message: face data. - :type message: int - - :return: A response to CB containing the amount of faces - :rtype: int - """ - # Poll the FaceDetectionSender status - face_sender = next( - (s for s in state.sockets if isinstance(s, FaceDetectionSender)), - None - ) - if face_sender: - return {"endpoint": "face", "data": face_sender.face_detected} - else: - return {"endpoint": "face", "data": False} @staticmethod @@ -74,12 +52,7 @@ class MainReceiver(ReceiverBase): :return: A response dictionary with endpoint descriptions as data. :rtype: dict[str, list[dict]] """ - endpoints = [ - socket.endpoint_description() - for socket in state.sockets - if socket.endpoint_description() is not None - ] - + endpoints = [socket.endpoint_description() for socket in state.sockets] return {"endpoint": "negotiate/ports", "data": endpoints} @@ -115,8 +88,6 @@ class MainReceiver(ReceiverBase): """ if message["endpoint"] == "ping": return self._handle_ping(message) - elif message["endpoint"] == "face": - return self._handle_face(message) elif message["endpoint"].startswith("negotiate"): return self._handle_negotiation(message)