hermes-bot/ros/hermes_face/hermes_face/face_node.py

"""ROS node for the face. Draws on the BrainCraft HAT's display.

Subscribes:  /hermes/state (std_msgs/String): sleeping idle listening thinking looking speaking
             /joint_states (sensor_msgs/JointState): pan_joint, tilt_joint in radians. The sign
                 convention is the MCP tool's: positive pan = the robot's left, positive tilt = up.
             /vision/faces (vision_msgs/Detection2DArray): the eyes glance toward the best face (a named one
                 first: the name is results[1].class_id)
             /camera/camera_info (sensor_msgs/CameraInfo): the image size for that, if published
             /audio/activity (std_msgs/String): speaking | listening | quiet, from hermes_audio
             /audio/scope/{mic,speaker} (std_msgs/Float32MultiArray): the real waveform for the mouth
/hermes/state doubles as the agent's heartbeat: hermes_mcp republishes it about once a second, and
when it has not been heard for `agent_timeout_s` (default 10, 0 disables) the face goes to sleep, so
"sleeping" really means "no agent connected".
Which state shows is decided by hermes_face.arbiter: sleeping (agent only) > speaking > listening >
thinking > idle; room sound never wakes a sleeping face.
The face starts in `initial_state` (default `sleeping`), so it runs with nothing else up; whichever
node knows the agent is connected (hermes_mcp) publishes `idle` to wake it.
"""
import math
import time

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CameraInfo, JointState
from std_msgs.msg import Float32MultiArray, String
from vision_msgs.msg import Detection2DArray

from .arbiter import Heartbeat, resolve_state
from .gaze import Target, gaze_from_pixel, pick_target
from .render import STATES, FaceRenderer


class FaceNode(Node):
    def __init__(self):
        super().__init__('hermes_face')
        param = lambda name, default: self.declare_parameter(name, default).value  # noqa: E731
        self.fps = min(60.0, max(1.0, float(param('fps', 30.0))))   # a timer period of 1/fps
        output = param('output', 'st7789')              # 'st7789' or 'none' (render only)
        self.pan_joint = param('pan_joint', 'pan_joint')
        self.tilt_joint = param('tilt_joint', 'tilt_joint')
        initial = param('initial_state', 'sleeping')
        self.agent_state = initial if initial in STATES else 'sleeping'
        self.audio_activity = 'quiet'
        self._logged_state = ''
        self.heartbeat = Heartbeat(float(param('agent_timeout_s', 10.0)))
        self.audio_drive = bool(param('audio_drive', True))
        self.follow = bool(param('gaze_follow', True))
        self.image_size = (float(param('camera_width', 1296)), float(param('camera_height', 972)))

        self.renderer = FaceRenderer(
            phosphor=param('phosphor', 'P7'), style=param('style', 'phosphor'),
            persist_ms=float(param('persistence_ms', 70.0)), focus=float(param('focus', 0.35)),
            jitter=float(param('jitter', 0.3)), intensity=float(param('intensity', 1.0)),
            state=initial if initial in STATES else 'sleeping', now=time.monotonic(),
            rotation=int(param('rotation', 270)))   # 270: the mounted pose, microSD end up

        self.display = None
        if output == 'st7789':
            from .display import AsyncDisplay, St7789
            self.display = AsyncDisplay(St7789(
                x_offset=int(param('x_offset', 0)), y_offset=int(param('y_offset', 80)),
                baudrate=int(param('baudrate', 48_000_000))))

        self.create_subscription(String, '/hermes/state', self._on_state, 10)
        self.create_subscription(JointState, '/joint_states', self._on_joints, 10)
        if self.audio_drive:
            self.create_subscription(String, '/audio/activity', self._on_activity, 10)
            for kind in ('mic', 'speaker'):
                self.create_subscription(Float32MultiArray, f'/audio/scope/{kind}',
                                         lambda m, k=kind: self.renderer.set_scope(k, m.data, time.monotonic()), 2)
        if self.follow:
            self.create_subscription(Detection2DArray, '/vision/faces', self._on_faces, 10)
            self.create_subscription(CameraInfo, '/camera/camera_info', self._on_camera_info, 1)
        self._last = time.monotonic()
        self._frames, self._stat_t = 0, self._last
        self.create_timer(1.0 / self.fps, self._tick)
        self.get_logger().info(f'face up: {output}, {self.fps:g} fps, state {self.renderer.state}')
        self._resolve(time.monotonic())

    def _on_state(self, msg: String) -> None:
        if msg.data not in STATES:
            self.get_logger().warn(f'ignoring unknown state {msg.data!r}; known: {", ".join(STATES)}')
            return
        now = time.monotonic()
        self.heartbeat.beat(now)
        if msg.data == 'looking':          # a timed event: the renderer squints, flashes, then goes back
            self.renderer.set_state('looking', now)
            return
        self.agent_state = msg.data
        self._resolve(now)

    def _on_activity(self, msg: String) -> None:
        self.audio_activity = msg.data
        self._resolve(time.monotonic())

    def _resolve(self, now: float) -> None:
        if self.agent_state != 'sleeping' and not self.heartbeat.alive(now):
            self.get_logger().info(f'no /hermes/state for {self.heartbeat.timeout_s:g} s: agent gone, sleeping')
            self.agent_state = 'sleeping'
        if self.renderer.state != 'looking':
            self.renderer.set_state(resolve_state(self.agent_state, self.audio_activity if self.audio_drive else 'quiet'), now)
        if self.renderer.state != self._logged_state:
            self._logged_state = self.renderer.state
            self.get_logger().info(f'state: {self._logged_state} (agent={self.agent_state}, audio={self.audio_activity})')

    def _on_joints(self, msg: JointState) -> None:
        pos = dict(zip(msg.name, msg.position))
        if self.pan_joint in pos or self.tilt_joint in pos:
            self.renderer.set_pose(math.degrees(pos.get(self.pan_joint, 0.0)),
                                   math.degrees(pos.get(self.tilt_joint, 0.0)))

    def _on_camera_info(self, msg: CameraInfo) -> None:
        if msg.width and msg.height:
            self.image_size = (float(msg.width), float(msg.height))

    def _on_faces(self, msg: Detection2DArray) -> None:
        target = pick_target([
            Target(d.bbox.center.position.x, d.bbox.center.position.y, d.bbox.size_x, d.bbox.size_y,
                   d.results[0].hypothesis.score if d.results else 1.0,
                   d.results[1].hypothesis.class_id if len(d.results) > 1 else '')   # the name, if recognised
            for d in msg.detections])
        if target:
            gx, gy = gaze_from_pixel(target.cx, target.cy, *self.image_size)
            self.renderer.set_gaze(gx, gy, time.monotonic())

    def _tick(self) -> None:
        now = time.monotonic()
        self._resolve(now)                 # also settles the state a finished `looking` returned to
        rgb = self.renderer.frame(now, now - self._last, 'rgb565')
        self._last = now
        if self.display:
            self.display.show(rgb)
        self._frames += 1
        if now - self._stat_t >= 30.0:
            self.get_logger().debug(f'{self._frames / (now - self._stat_t):.1f} fps')
            self._frames, self._stat_t = 0, now

    def destroy_node(self):
        if self.display:
            self.display.close()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = FaceNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()