diff --git a/src/robot_interface/core/config.py b/src/robot_interface/core/config.py index 9e638b5..13d0027 100644 --- a/src/robot_interface/core/config.py +++ b/src/robot_interface/core/config.py @@ -17,6 +17,10 @@ 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 + :ivar face_detection_interval: Time between face detection events, defaults to 1000 ms. + :vartype face_detection_interval: int """ def __init__( self, @@ -25,12 +29,16 @@ class AgentSettings(object): main_receiver_port=None, video_sender_port=None, audio_sender_port=None, + face_detection_port=None, + face_detection_interval=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) + self.face_detection_interval = get_config(face_detection_interval, "AGENT__FACE_DETECTION_INTERVAL", 1000, int) class VideoConfig(object): diff --git a/src/robot_interface/endpoints/face_detector.py b/src/robot_interface/endpoints/face_detector.py new file mode 100644 index 0000000..b8485e5 --- /dev/null +++ b/src/robot_interface/endpoints/face_detector.py @@ -0,0 +1,93 @@ +# -*- coding: utf-8 -*- +""" +This program has been developed by students from the bachelor Computer Science at Utrecht +University within the Software Project course. +© Copyright Utrecht University (Department of Information and Computing Sciences) +""" +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): + """ + Face detection endpoint. + + Subscribes to and polls ALMemory["FaceDetected"], sends events to CB. + """ + + def __init__(self, zmq_context, port=settings.agent_settings.face_detection_port): + super(FaceDetectionSender, self).__init__("face") + + self.create_socket(zmq_context, zmq.PUB, port) + + self._face_service = None + self._memory_service = None + + self._face_thread = None + + def start_face_detection(self): + if not state.qi_session: + logging.warning("No Qi session available. Face detection not started.") + return + + self._face_service = state.qi_session.service("ALFaceDetection") + self._memory_service = state.qi_session.service("ALMemory") + + self._face_service.setTrackingEnabled(False) + self._face_service.setRecognitionEnabled(False) + + self._face_service.subscribe( + "FaceDetectionSender", + settings.agent_settings.face_detection_interval, + 0.0, + ) + + self._face_thread = threading.Thread(target=self._face_loop) + self._face_thread.start() + + logging.info("Face detection started.") + + def _face_loop(self): + """ + Continuously send face detected to the CB, at the interval set in the + ``start_face_detection`` method. + """ + while not state.exit_event.is_set(): + 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.socket.send(json.dumps({"face_detected": face_present}).encode("utf-8")) + except Exception: + logging.exception("Error reading FaceDetected") + + time.sleep(settings.agent_settings.face_detection_interval / 1000.0) + + def stop_face_detection(self): + try: + if self._face_service: + self._face_service.unsubscribe("FaceDetectionSender") + self._face_service.setTrackingEnabled(False) + logging.info("Face detection stopped.") + except Exception: + logging.warning("Error during face detection cleanup.") + + def close(self): + super(FaceDetectionSender, self).close() + self.stop_face_detection() diff --git a/src/robot_interface/endpoints/main_receiver.py b/src/robot_interface/endpoints/main_receiver.py index 2882970..29dba60 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): @@ -37,6 +38,7 @@ class MainReceiver(ReceiverBase): """ return {"endpoint": "ping", "data": message.get("data")} + @staticmethod def _handle_port_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] diff --git a/test/unit/test_face_detection_sender.py b/test/unit/test_face_detection_sender.py new file mode 100644 index 0000000..64661d8 --- /dev/null +++ b/test/unit/test_face_detection_sender.py @@ -0,0 +1,175 @@ +# -*- coding: utf-8 -*- +""" +This program has been developed by students from the bachelor Computer Science at Utrecht +University within the Software Project course. +© Copyright Utrecht University (Department of Information and Computing Sciences) +""" +from __future__ import unicode_literals + +import json +import mock +import pytest + +from robot_interface.endpoints.face_detector import FaceDetectionSender +from robot_interface.state import state + + +@pytest.fixture(autouse=True) +def initialized_state(monkeypatch): + """ + Fully initialize global state so __getattribute__ allows access. + """ + # Bypass the initialization guard + monkeypatch.setattr(state, "is_initialized", True, raising=False) + + # Install a controllable exit_event + exit_event = mock.Mock() + exit_event.is_set = mock.Mock(return_value=True) + monkeypatch.setattr(state, "exit_event", exit_event, raising=False) + + # Default qi_session is None unless overridden + monkeypatch.setattr(state, "qi_session", None, raising=False) + + yield + + +def test_start_face_detection_no_qi_session(): + """ + Returns early when qi_session is None. + """ + sender = FaceDetectionSender(mock.Mock()) + sender.start_face_detection() + + assert sender._face_thread is None + assert sender._face_service is None + assert sender._memory_service is None + + +def test_start_face_detection_happy_path(mocker): + """ + Initializes services and starts background thread. + """ + mock_face = mock.Mock() + mock_memory = mock.Mock() + + mock_qi = mock.Mock() + mock_qi.service.side_effect = lambda name: { + "ALFaceDetection": mock_face, + "ALMemory": mock_memory, + }[name] + + state.qi_session = mock_qi + + fake_thread = mock.Mock() + mocker.patch("threading.Thread", return_value=fake_thread) + + sender = FaceDetectionSender(mock.Mock()) + sender.start_face_detection() + + mock_face.setTrackingEnabled.assert_called_with(False) + mock_face.setRecognitionEnabled.assert_called_with(False) + mock_face.subscribe.assert_called_once() + fake_thread.start.assert_called_once() + + +def test_face_loop_face_detected_true(mocker): + """ + Sends face_detected=True when face data exists. + """ + sender = FaceDetectionSender(mock.Mock()) + + sender._memory_service = mock.Mock() + sender._memory_service.getData.return_value = [0, [[1]]] + sender.socket = mock.Mock() + + mocker.patch("time.sleep") + state.exit_event.is_set.side_effect = [False, True] + + sender._face_loop() + + sent = sender.socket.send.call_args[0][0] + payload = json.loads(sent.decode("utf-8")) + + assert payload["face_detected"] is True + + +def test_face_loop_face_detected_false(mocker): + """ + Sends face_detected=False when no face data exists. + """ + sender = FaceDetectionSender(mock.Mock()) + + sender._memory_service = mock.Mock() + sender._memory_service.getData.return_value = [] + sender.socket = mock.Mock() + + mocker.patch("time.sleep") + state.exit_event.is_set.side_effect = [False, True] + + sender._face_loop() + + sent = sender.socket.send.call_args[0][0] + payload = json.loads(sent.decode("utf-8")) + + assert not payload["face_detected"] + + +def test_face_loop_handles_exception(mocker): + """ + Exceptions inside loop are swallowed. + """ + sender = FaceDetectionSender(mock.Mock()) + + sender._memory_service = mock.Mock() + sender._memory_service.getData.side_effect = Exception("boom") + sender.socket = mock.Mock() + + mocker.patch("time.sleep") + state.exit_event.is_set.side_effect = [False, True] + + # Must not raise + sender._face_loop() + + +def test_stop_face_detection_happy_path(): + """ + Unsubscribes and disables tracking. + """ + sender = FaceDetectionSender(mock.Mock()) + + mock_face = mock.Mock() + sender._face_service = mock_face + + sender.stop_face_detection() + + mock_face.unsubscribe.assert_called_once() + mock_face.setTrackingEnabled.assert_called_with(False) + + +def test_stop_face_detection_exception(): + """ + stop_face_detection swallows service exceptions. + """ + sender = FaceDetectionSender(mock.Mock()) + + mock_face = mock.Mock() + mock_face.unsubscribe.side_effect = Exception("fail") + sender._face_service = mock_face + + sender.stop_face_detection() + + +def test_close_calls_stop_face_detection(mocker): + """ + close() calls parent close and stop_face_detection(). + """ + sender = FaceDetectionSender(mock.Mock()) + + mocker.patch.object(sender, "stop_face_detection") + mocker.patch( + "robot_interface.endpoints.face_detector.SocketBase.close" + ) + + sender.close() + + sender.stop_face_detection.assert_called_once() diff --git a/test/unit/test_main.py b/test/unit/test_main.py index f323630..afd4b05 100644 --- a/test/unit/test_main.py +++ b/test/unit/test_main.py @@ -55,6 +55,9 @@ class DummySender: def start(self): self.called = True + def start_face_detection(self): + self.called = True + def close(self): pass @@ -108,11 +111,13 @@ def patched_main_components(monkeypatch, fake_sockets, fake_poll): fake_act = FakeReceiver(act_sock) video_sender = DummySender() audio_sender = DummySender() + face_sender = DummySender() monkeypatch.setattr(main_mod, "MainReceiver", lambda ctx: fake_main) monkeypatch.setattr(main_mod, "ActuationReceiver", lambda ctx: fake_act) monkeypatch.setattr(main_mod, "VideoSender", lambda ctx: video_sender) monkeypatch.setattr(main_mod, "AudioSender", lambda ctx: audio_sender) + monkeypatch.setattr(main_mod, "FaceDetectionSender", lambda ctx: face_sender) # Register sockets for the fake poller fake_poll.registered = {main_sock: zmq.POLLIN, act_sock: zmq.POLLIN}