From 3b470c8f29ad337d47cee117526b770c43b29484 Mon Sep 17 00:00:00 2001 From: JobvAlewijk Date: Wed, 7 Jan 2026 17:56:21 +0100 Subject: [PATCH] feat: fully working face detection ref: N25B-397 --- src/robot_interface/core/config.py | 4 + .../endpoints/face_detector.py | 100 +++++++----------- .../endpoints/main_receiver.py | 26 +++++ src/robot_interface/main.py | 8 ++ 4 files changed, 75 insertions(+), 63 deletions(-) diff --git a/src/robot_interface/core/config.py b/src/robot_interface/core/config.py index 9e638b5..3caec21 100644 --- a/src/robot_interface/core/config.py +++ b/src/robot_interface/core/config.py @@ -17,6 +17,8 @@ class AgentSettings(object): :vartype video_sender_port: int :ivar audio_sender_port: Port used for sending audio data, defaults to 5558. :vartype audio_sender_port: int + :ivar face_detection_port: Port used for sending face detection events, defaults to 5559. + :vartype face_detection_port: int """ def __init__( self, @@ -25,12 +27,14 @@ class AgentSettings(object): main_receiver_port=None, video_sender_port=None, audio_sender_port=None, + face_detection_port=None, ): self.control_backend_host = get_config(control_backend_host, "AGENT__CONTROL_BACKEND_HOST", "localhost") self.actuation_receiver_port = get_config(actuation_receiver_port, "AGENT__ACTUATION_RECEIVER_PORT", 5557, int) self.main_receiver_port = get_config(main_receiver_port, "AGENT__MAIN_RECEIVER_PORT", 5555, int) self.video_sender_port = get_config(video_sender_port, "AGENT__VIDEO_SENDER_PORT", 5556, int) self.audio_sender_port = get_config(audio_sender_port, "AGENT__AUDIO_SENDER_PORT", 5558, int) + self.face_detection_port = get_config(face_detection_port, "AGENT__FACE_DETECTION_PORT", 5559, int) class VideoConfig(object): diff --git a/src/robot_interface/endpoints/face_detector.py b/src/robot_interface/endpoints/face_detector.py index b25f656..75e0930 100644 --- a/src/robot_interface/endpoints/face_detector.py +++ b/src/robot_interface/endpoints/face_detector.py @@ -1,52 +1,49 @@ from __future__ import unicode_literals + import logging import threading -import zmq +import time from robot_interface.endpoints.socket_base import SocketBase from robot_interface.state import state -from robot_interface.core.config import settings class FaceDetectionSender(SocketBase): """ - Face detection sender endpoint. + Minimal face detection sender with a shared flag. - Subscribes to ALFaceDetection and forwards face detection - events over ZeroMQ. + Polls ALMemory["FaceDetected"] and keeps a simple boolean + indicating if a face is currently detected. """ - def __init__(self, zmq_context, port=settings.agent_settings.face_sender_port): + def __init__(self, zmq_context, port=None): super(FaceDetectionSender, self).__init__("face") - self.create_socket(zmq_context, zmq.PUB, port) - + if port: + self.create_socket(zmq_context, None, port) # PUB not used here self._face_service = None self._memory_service = None - self._subscriber = None self._thread = None + # Shared status for MainReceiver polling + self.face_detected = False + self._last_seen_face_time = 0 + def start_face_detection(self): - """ - Initializes ALFaceDetection and starts listening for face events. - """ if not state.qi_session: logging.info("No Qi session available. Not starting face detection.") return - import qi # Lazy import (same pattern as rest of codebase) - + import qi self._face_service = state.qi_session.service("ALFaceDetection") self._memory_service = state.qi_session.service("ALMemory") - # Enable face detection - self._face_service.setTrackingEnabled(True) + # Enable minimal detection + self._face_service.setTrackingEnabled(False) self._face_service.setRecognitionEnabled(False) - # Subscribe to FaceDetected memory event - self._subscriber = self._memory_service.subscriber("FaceDetected") - self._subscriber.signal.connect(self._on_face_detected) + # Required to activate extractor + self._face_service.subscribe("FaceDetectionSender", 500, 0.0) - # Start keep-alive thread self._thread = threading.Thread(target=self._face_loop) self._thread.daemon = True self._thread.start() @@ -58,56 +55,33 @@ class FaceDetectionSender(SocketBase): Keeps the face detection alive until shutdown. """ while not state.exit_event.is_set(): - state.exit_event.wait(0.1) + try: + value = self._memory_service.getData("FaceDetected", 0) + face_present = ( + value + and len(value) > 1 + and value[1] + and value[1][0] + and len(value[1][0]) > 0 + ) - self._cleanup() + now = time.time() + if face_present: + self._last_seen_face_time = now - def _on_face_detected(self, value): - """ - Callback for ALMemory FaceDetected event. + # Consider face "lost" after 3s + self.face_detected = (now - self._last_seen_face_time) < 3 - :param value: Face detection data structure from NAOqi - """ - if not value or len(value) < 2: - return + except Exception: + logging.exception("Error reading FaceDetected") - timestamp = value[0] - faces = value[1] - - face_data = { - "timestamp": timestamp, - "face_count": len(faces), - "faces": [] - } - - for face in faces: - face_info = face[0] # Shape info - extra_info = face[1] # Extra info (ID, score, etc.) - - face_data["faces"].append({ - "alpha": face_info[1], - "beta": face_info[2], - "width": face_info[3], - "height": face_info[4], - "confidence": extra_info[1] if len(extra_info) > 1 else None - }) + time.sleep(0.1) + def stop_face_detection(self): try: - self.socket.send_json(face_data) - except Exception: - logging.warn("Failed to send face detection data.") - - def _cleanup(self): - """ - Cleanup subscriptions and disable face detection. - """ - try: - if self._subscriber: - self._subscriber.signal.disconnect(self._on_face_detected) - if self._face_service: + self._face_service.unsubscribe("FaceDetectionSender") self._face_service.setTrackingEnabled(False) - logging.info("Face detection stopped.") except Exception: - logging.warn("Error during face detection cleanup.") + logging.warning("Error during face detection cleanup.") diff --git a/src/robot_interface/endpoints/main_receiver.py b/src/robot_interface/endpoints/main_receiver.py index 2882970..0a479d9 100644 --- a/src/robot_interface/endpoints/main_receiver.py +++ b/src/robot_interface/endpoints/main_receiver.py @@ -4,6 +4,7 @@ from robot_interface.endpoints.receiver_base import ReceiverBase from robot_interface.state import state from robot_interface.core.config import settings +from robot_interface.endpoints.face_detector import FaceDetectionSender class MainReceiver(ReceiverBase): @@ -36,6 +37,29 @@ 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 def _handle_port_negotiation(message): @@ -86,6 +110,8 @@ 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) diff --git a/src/robot_interface/main.py b/src/robot_interface/main.py index 816e53b..4b32873 100644 --- a/src/robot_interface/main.py +++ b/src/robot_interface/main.py @@ -12,6 +12,8 @@ from robot_interface.endpoints.video_sender import VideoSender from robot_interface.state import state from robot_interface.core.config import settings from robot_interface.utils.timeblock import TimeBlock +from robot_interface.endpoints.face_detector import FaceDetectionSender + def main_loop(context): @@ -35,6 +37,12 @@ def main_loop(context): video_sender.start_video_rcv() audio_sender.start() + # --- Face detection sender --- + face_sender = FaceDetectionSender(context) + state.sockets.append(face_sender) + face_sender.start_face_detection() + + # Sockets that can run on the main thread. These sockets' endpoints should not block for long (say 50 ms at most). receivers = [main_receiver, actuation_receiver]