chore: modified into req reply socket on 5559
This commit is contained in:
@@ -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:
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user