pepperplus/pepperplus-ri
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

feat: fully working face detection

Browse Source 
ref: N25B-397
This commit is contained in:
JobvAlewijk
2026-01-07 17:56:21 +01:00
parent b8f71f6bee
commit 3b470c8f29
4 changed files with 75 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/robot_interface/core/config.py
View File

@@ -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):

100
src/robot_interface/endpoints/face_detector.py
View File

@@ -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.")

26
src/robot_interface/endpoints/main_receiver.py
View File

@@ -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)

8
src/robot_interface/main.py
View File

@@ -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]