chore: modified into req reply socket on 5559

This commit is contained in:
JobvAlewijk
2026-01-17 14:01:32 +01:00
parent 4e9afbaaf5
commit 83099a2810
2 changed files with 60 additions and 47 deletions

View File

@@ -1,65 +1,78 @@
from __future__ import unicode_literals from __future__ import unicode_literals
import json
import logging import logging
import threading import threading
import time import time
import zmq
from robot_interface.endpoints.socket_base import SocketBase from robot_interface.endpoints.socket_base import SocketBase
from robot_interface.state import state from robot_interface.state import state
from robot_interface.core.config import settings
class FaceDetectionSender(SocketBase): class FaceDetectionSender(SocketBase):
""" """
Minimal face detection sender with a shared flag. Face detection REP endpoint.
Polls ALMemory["FaceDetected"] and keeps a simple boolean - Polls ALMemory["FaceDetected"]
indicating if a face is currently detected. - 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") super(FaceDetectionSender, self).__init__("face")
if port: 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._face_service = None
self._memory_service = None self._memory_service = None
self._thread = None
# Shared status for MainReceiver polling
self.face_detected = False self.face_detected = False
self._last_seen_face_time = 0 self._last_seen_face_time = 0
self._face_thread = None
self._rep_thread = None
def endpoint_description(self): def endpoint_description(self):
return None return "face"
# ------------------------------------------------------------------
# Face detection logic (runs independently)
# ------------------------------------------------------------------
def start_face_detection(self): def start_face_detection(self):
if not state.qi_session: 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 return
import qi import qi
self._face_service = state.qi_session.service("ALFaceDetection") self._face_service = state.qi_session.service("ALFaceDetection")
self._memory_service = state.qi_session.service("ALMemory") self._memory_service = state.qi_session.service("ALMemory")
# Enable minimal detection
self._face_service.setTrackingEnabled(False) self._face_service.setTrackingEnabled(False)
self._face_service.setRecognitionEnabled(False) self._face_service.setRecognitionEnabled(False)
# Required to activate extractor
self._face_service.subscribe("FaceDetectionSender", 500, 0.0) self._face_service.subscribe("FaceDetectionSender", 500, 0.0)
self._thread = threading.Thread(target=self._face_loop) self._face_thread = threading.Thread(target=self._face_loop, daemon=True)
self._thread.daemon = True self._face_thread.start()
self._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): def _face_loop(self):
""" """
Keeps the face detection alive until shutdown. Continuously updates `self.face_detected`.
""" """
while not state.exit_event.is_set(): while not state.exit_event.is_set():
try: try:
value = self._memory_service.getData("FaceDetected", 0) value = self._memory_service.getData("FaceDetected", 0)
face_present = ( face_present = (
value value
and len(value) > 1 and len(value) > 1
@@ -72,7 +85,7 @@ class FaceDetectionSender(SocketBase):
if face_present: if face_present:
self._last_seen_face_time = now 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 self.face_detected = (now - self._last_seen_face_time) < 3
except Exception: except Exception:
@@ -80,6 +93,35 @@ class FaceDetectionSender(SocketBase):
time.sleep(0.1) 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): def stop_face_detection(self):
try: try:
if self._face_service: if self._face_service:

View File

@@ -37,28 +37,6 @@ class MainReceiver(ReceiverBase):
:rtype: dict[str, str | list[dict]] :rtype: dict[str, str | list[dict]]
""" """
return {"endpoint": "ping", "data": message.get("data")} 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 @staticmethod
@@ -74,12 +52,7 @@ class MainReceiver(ReceiverBase):
:return: A response dictionary with endpoint descriptions as data. :return: A response dictionary with endpoint descriptions as data.
:rtype: dict[str, list[dict]] :rtype: dict[str, list[dict]]
""" """
endpoints = [ endpoints = [socket.endpoint_description() for socket in state.sockets]
socket.endpoint_description()
for socket in state.sockets
if socket.endpoint_description() is not None
]
return {"endpoint": "negotiate/ports", "data": endpoints} return {"endpoint": "negotiate/ports", "data": endpoints}
@@ -115,8 +88,6 @@ class MainReceiver(ReceiverBase):
""" """
if message["endpoint"] == "ping": if message["endpoint"] == "ping":
return self._handle_ping(message) return self._handle_ping(message)
elif message["endpoint"] == "face":
return self._handle_face(message)
elif message["endpoint"].startswith("negotiate"): elif message["endpoint"].startswith("negotiate"):
return self._handle_negotiation(message) return self._handle_negotiation(message)