feat: cb can communicate face with ri
ref: N25B-397
This commit is contained in:
@@ -268,6 +268,30 @@ class RICommunicationAgent(BaseAgent):
|
|||||||
if self.pub_socket is not None:
|
if self.pub_socket is not None:
|
||||||
await self.pub_socket.send_multipart([topic, data])
|
await self.pub_socket.send_multipart([topic, data])
|
||||||
await asyncio.sleep(settings.behaviour_settings.sleep_s)
|
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 _:
|
case _:
|
||||||
self.logger.debug(
|
self.logger.debug(
|
||||||
"Received message with topic different than ping, while ping expected."
|
"Received message with topic different than ping, while ping expected."
|
||||||
|
|||||||
@@ -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")
|
|
||||||
74
src/control_backend/agents/perception/face_rec_agent.py
Normal file
74
src/control_backend/agents/perception/face_rec_agent.py
Normal file
@@ -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)
|
||||||
@@ -8,7 +8,7 @@ api_router.include_router(message.router, tags=["Messages"])
|
|||||||
|
|
||||||
api_router.include_router(sse.router, tags=["SSE"])
|
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"])
|
api_router.include_router(logs.router, tags=["Logs"])
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user