From bb0c1bd3838b4b88397259820d2536abdd99096b Mon Sep 17 00:00:00 2001 From: JobvAlewijk Date: Thu, 8 Jan 2026 13:02:32 +0100 Subject: [PATCH] feat: cb can communicate face with ri ref: N25B-397 --- .../communication/ri_communication_agent.py | 24 ++++++ .../agents/perception/face_rec_agent | 53 ------------- .../agents/perception/face_rec_agent.py | 74 +++++++++++++++++++ src/control_backend/api/v1/router.py | 2 +- 4 files changed, 99 insertions(+), 54 deletions(-) delete mode 100644 src/control_backend/agents/perception/face_rec_agent create mode 100644 src/control_backend/agents/perception/face_rec_agent.py diff --git a/src/control_backend/agents/communication/ri_communication_agent.py b/src/control_backend/agents/communication/ri_communication_agent.py index 9aaf0db..058dc6e 100644 --- a/src/control_backend/agents/communication/ri_communication_agent.py +++ b/src/control_backend/agents/communication/ri_communication_agent.py @@ -268,6 +268,30 @@ class RICommunicationAgent(BaseAgent): if self.pub_socket is not None: await self.pub_socket.send_multipart([topic, data]) await asyncio.sleep(settings.behaviour_settings.sleep_s) + case "face": + # Ask the RI for current face status + request = {"endpoint": "face", "data": {}} + try: + assert self._req_socket is not None + await self._req_socket.send_json(request) + response = await asyncio.wait_for( + self._req_socket.recv_json(), timeout=2.0 + ) + + # Expect response: {"endpoint": "face", "data": True/False} + face_status = response.get("data", False) + + topic = b"face" + data = json.dumps(face_status).encode() + + if self.pub_socket is not None: + await self.pub_socket.send_multipart([topic, data]) + + self.logger.debug(f"Face status: {face_status}") + + except Exception as e: + self.logger.warning(f"Failed to get face status: {e}") + case _: self.logger.debug( "Received message with topic different than ping, while ping expected." diff --git a/src/control_backend/agents/perception/face_rec_agent b/src/control_backend/agents/perception/face_rec_agent deleted file mode 100644 index ddb7433..0000000 --- a/src/control_backend/agents/perception/face_rec_agent +++ /dev/null @@ -1,53 +0,0 @@ -import asyncio -import zmq -import zmq.asyncio as azmq - -from control_backend.agents import BaseAgent -from control_backend.core.config import settings - - -class FacePerceptionAgent(BaseAgent): - """ - Receives and processes face detection events from Pepper. - """ - - def __init__(self, name, address, bind=False): - super().__init__(name) - self._address = address - self._bind = bind - self._socket: azmq.Socket | None = None - - async def setup(self): - self.logger.info("Starting FacePerceptionAgent") - - ctx = azmq.Context.instance() - self._socket = ctx.socket(zmq.SUB) - self._socket.setsockopt_string(zmq.SUBSCRIBE, "") - - if self._bind: - self._socket.bind(self._address) - else: - self._socket.connect(self._address) - - self.add_behavior(self._listen_loop()) - - async def _listen_loop(self): - while self._running: - try: - msg = await self._socket.recv_json() - await self._process_face_data(msg) - except Exception: - self.logger.exception("Error receiving face data") - - async def _process_face_data(self, data: dict): - """ - Central place to handle face perception. - """ - face_count = data.get("face_count", 0) - - if face_count > 0: - self.logger.debug("Detected %d face(s)", face_count) - - #Post belief - else: - self.logger.debug("No faces detected") diff --git a/src/control_backend/agents/perception/face_rec_agent.py b/src/control_backend/agents/perception/face_rec_agent.py new file mode 100644 index 0000000..8201197 --- /dev/null +++ b/src/control_backend/agents/perception/face_rec_agent.py @@ -0,0 +1,74 @@ +import zmq +import zmq.asyncio as azmq + +from control_backend.agents import BaseAgent + + +class FacePerceptionAgent(BaseAgent): + """ + Receives and processes face detection / recognition events + coming from Pepper (via a NAOqi -> ZMQ bridge). + """ + + def __init__(self, name: str): + super().__init__(name) + self._address = "tcp://127.0.0.1:5559" + self._socket: azmq.Socket | None = None + + async def setup(self): + self.logger.info("Starting FacePerceptionAgent") + + ctx = azmq.Context.instance() + self._socket = ctx.socket(zmq.SUB) + self._socket.setsockopt_string(zmq.SUBSCRIBE, "") + + self._socket.connect(self._address) + + self.add_behavior(self._listen_loop()) + + async def _listen_loop(self): + while self._running: + try: + msg = await self._socket.recv_json() + await self._process_face_data(msg) + except Exception: + self.logger.exception("Error receiving face data") + + async def _process_face_data(self, data: dict): + """ + Processes NAOqi FaceDetected-derived data. + """ + + faces = data.get("faces", []) + new_recognitions = data.get("new_recognitions", []) + + if not faces: + self.logger.debug("No faces detected") + return + + self.logger.debug("Detected %d face(s)", len(faces)) + + for face in faces: + face_id = face.get("face_id") + alpha = face.get("alpha") + beta = face.get("beta") + # size_x = face.get("size_x") + # size_y = face.get("size_y") + + recognized = face.get("recognized", False) + label = face.get("label") + score = face.get("score", 0.0) + + if recognized: + self.logger.info("Recognized %s (score=%.2f, id=%s)", label, score, face_id) + else: + self.logger.debug( + "Unrecognized face id=%s at (α=%.2f, β=%.2f)", face_id, alpha, beta + ) + + # Temporal-filtered recognition (important!) + for name in new_recognitions: + self.logger.info("New person recognized: %s", name) + + # 🔮 Example belief posting hook + # await self.post_belief("person_present", name=name) diff --git a/src/control_backend/api/v1/router.py b/src/control_backend/api/v1/router.py index ebba0db..88ec30d 100644 --- a/src/control_backend/api/v1/router.py +++ b/src/control_backend/api/v1/router.py @@ -8,7 +8,7 @@ api_router.include_router(message.router, tags=["Messages"]) api_router.include_router(sse.router, tags=["SSE"]) -api_router.include_router(robot.router, prefix="/robot", tags=["Pings", "Commands"]) +api_router.include_router(robot.router, prefix="/robot", tags=["Pings", "Commands", "Face"]) api_router.include_router(logs.router, tags=["Logs"])