Make the eyes follow detected faces; start the face at power-up

hermes_face now subscribes to /vision/faces and glances toward the best face (a
recognised one first, then the biggest), smoothed, holding 2.5 s and drifting
back to centre when none is seen. Sleeping ignores gaze. Image x flips on the
display: the camera looks the way the robot faces, so a person on the image's
right is on the robot's right, the viewer's left.

Fix the pan sign carried over from the prototype: positive pan is the robot's
left (the MCP contract), which is the viewer's right on the display.

robot-face.service starts the face at boot and restarts it on failure, with a
private working directory because lgpio drops a notification pipe in its cwd and
systemd's default / is not writable. robot-cpu-performance.service holds the CPU
at full clock, which halves the face's CPU cost.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs

ros/hermes_face/hermes_face/face_node.py

@@ -1,7 +1,10 @@
 """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 (REP 103)
+             /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
+             /camera/camera_info (sensor_msgs/CameraInfo): the image size for that, if published
 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.
 """
@@ -10,9 +13,11 @@ import time
 
 import rclpy
 from rclpy.node import Node
-from sensor_msgs.msg import JointState
+from sensor_msgs.msg import CameraInfo, JointState
 from std_msgs.msg import String
+from vision_msgs.msg import Detection2DArray
 
+from .gaze import Target, gaze_from_pixel, pick_target
 from .render import STATES, FaceRenderer
 
 
@@ -25,6 +30,8 @@ class FaceNode(Node):
         self.pan_joint = param('pan_joint', 'pan_joint')
         self.tilt_joint = param('tilt_joint', 'tilt_joint')
         initial = param('initial_state', 'sleeping')
+        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'),
@@ -42,6 +49,9 @@ class FaceNode(Node):
 
         self.create_subscription(String, '/hermes/state', self._on_state, 10)
         self.create_subscription(JointState, '/joint_states', self._on_joints, 10)
+        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)
@@ -59,6 +69,19 @@ class FaceNode(Node):
             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.id)
+            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()
         rgb = self.renderer.frame(now, now - self._last, 'rgb565')

ros/hermes_face/hermes_face/gaze.py

@@ -0,0 +1,31 @@
+"""Where the eyes should look, given where faces are in the camera image. No ROS imports."""
+from __future__ import annotations
+
+from typing import NamedTuple, Optional, Sequence
+
+
+class Target(NamedTuple):
+    cx: float          # face centre, pixels
+    cy: float
+    w: float           # face size, pixels
+    h: float
+    score: float = 1.0
+    name: str = ''     # who, if recognised
+
+
+def pick_target(faces: Sequence[Target]) -> Optional[Target]:
+    """A recognised face wins over an unknown one; otherwise the biggest, most confident face."""
+    if not faces:
+        return None
+    return max(faces, key=lambda f: (bool(f.name), f.w * f.h * f.score))
+
+
+def gaze_from_pixel(cx: float, cy: float, width: float, height: float) -> tuple[float, float]:
+    """Image position -> eye direction (+x the viewer's right, +y down), each in [-1, 1].
+
+    The camera looks the way the robot faces, so a person on the right of the image is on the
+    robot's right, which is the viewer's left on the display: x flips, y does not.
+    """
+    gx = (cx - width / 2.0) / (width / 2.0)
+    gy = (cy - height / 2.0) / (height / 2.0)
+    return -max(-1.0, min(1.0, gx)), max(-1.0, min(1.0, gy))

ros/hermes_face/hermes_face/render.py

@@ -30,6 +30,8 @@ LOOK_HOLD_S = 1.2            # how long `looking` holds before returning to the
 SQUINT_S = 0.18              # squint before the shutter flash
 FLASH_AT_S = 0.16            # the shutter bloom lands this long after entering `looking`
 SLEEP_OPEN = 0.07            # eye openness while asleep: a thin line
+GAZE_HOLD_S = 2.5            # a glance toward a detected person lasts this long without a refresh
+GAZE_RANGE = (12.0, 9.0)     # eye travel in pixels for a gaze of +-1 (display x, y)
 
 
 def _approach(cur: float, target: float, rate: float, dt: float) -> float:
@@ -148,10 +150,13 @@ class FaceRenderer:
         self._state_t0 = now
         self._return_state = 'idle'
         self._flash_pending = state == 'looking'
-        self.pan = 0.0     # degrees, from the servos
+        self.pan = 0.0     # degrees, from the servos: positive pan = robot's left, tilt = looks up
         self.tilt = 0.0
+        self._gaze = (0.0, 0.0)      # where a person is, as eye direction: +x viewer's right, +y down
+        self._gaze_t = -1e9
         self.a = dict(think=0.0, open=SLEEP_OPEN if state == 'sleeping' else 1.0, eye_scale=1.0,
                       mouth_speak=0.0, mouth_listen=0.0, smile=1.0, pan_x=0.0, tilt_y=0.0,
+                      gaze_x=0.0, gaze_y=0.0,
                       sleep=1.0 if state == 'sleeping' else 0.0)
         self._next_blink = now + 2.5
         self._blink_t = -1.0
@@ -187,6 +192,12 @@ class FaceRenderer:
     def set_pose(self, pan_deg: float, tilt_deg: float) -> None:
         self.pan, self.tilt = pan_deg, tilt_deg
 
+    def set_gaze(self, gx: float, gy: float, now: float) -> None:
+        """Glance toward a person: gx, gy in [-1, 1] as eye direction (+x is the viewer's right, +y
+        down). Held for GAZE_HOLD_S, then the eyes drift back to centre unless it is refreshed."""
+        self._gaze = (min(1.0, max(-1.0, float(gx))), min(1.0, max(-1.0, float(gy))))
+        self._gaze_t = now
+
     # ------------------------------------------------------------ beam
     def _queue(self, x, y, e) -> None:
         x = np.atleast_1d(np.asarray(x, np.float64))
@@ -254,7 +265,8 @@ class FaceRenderer:
             breath = 0.5 + 0.5 * math.sin(2 * math.pi * 0.22 * T)
             e *= 1.0 - sleep * (0.55 - 0.18 * breath)
 
-        ex, ey = a['pan_x'], a['tilt_y'] + 8.0 * sleep
+        ex = a['pan_x'] + a['gaze_x'] * GAZE_RANGE[0]
+        ey = a['tilt_y'] + a['gaze_y'] * GAZE_RANGE[1] + 8.0 * sleep
         eyes = [(82 + ex, 98 + ey), (158 + ex, 98 + ey)]
         rx = 21 * a['eye_scale']
         ry = 25 * a['eye_scale'] * a['open']
@@ -319,8 +331,13 @@ class FaceRenderer:
         if in_look:
             scale_t = 0.92 if tl < SQUINT_S else 1.14
         a['eye_scale'] = _approach(a['eye_scale'], scale_t, 12, dt)
-        a['pan_x'] = _approach(a['pan_x'], -self.pan * 0.22, 7, dt)   # +pan = robot's left = viewer's right
-        a['tilt_y'] = _approach(a['tilt_y'], -self.tilt * 0.3, 7, dt)
+        a['pan_x'] = _approach(a['pan_x'], self.pan * 0.22, 7, dt)   # +pan = robot's left = viewer's right
+        a['tilt_y'] = _approach(a['tilt_y'], -self.tilt * 0.3, 7, dt)   # +tilt = looks up = display up
+        glancing = st != 'sleeping' and T - self._gaze_t < GAZE_HOLD_S
+        gx, gy = self._gaze if glancing else (0.0, 0.0)
+        rate = 6.0 if glancing else 1.5                               # follow briskly, drift back slowly
+        a['gaze_x'] = _approach(a['gaze_x'], gx, rate, dt)
+        a['gaze_y'] = _approach(a['gaze_y'], gy, rate, dt)
 
         # blink, and the squint before a photo; none while thinking or asleep
         open_t = SLEEP_OPEN if st == 'sleeping' else 1.0

ros/hermes_face/package.xml

@@ -10,6 +10,7 @@
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>vision_msgs</depend>
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-pil</exec_depend>
 

ros/hermes_face/test/test_gaze.py

@@ -0,0 +1,27 @@
+from hermes_face.gaze import Target, gaze_from_pixel, pick_target
+
+
+def test_centre_is_straight_ahead():
+    assert gaze_from_pixel(648, 486, 1296, 972) == (0.0, 0.0)
+
+
+def test_person_on_the_image_right_is_the_viewers_left_on_the_display():
+    gx, gy = gaze_from_pixel(1296, 486, 1296, 972)
+    assert gx == -1.0 and gy == 0.0
+
+
+def test_person_high_in_the_image_is_up_on_the_display():
+    assert gaze_from_pixel(648, 0, 1296, 972) == (0.0, -1.0)
+
+
+def test_off_image_positions_are_clamped():
+    assert gaze_from_pixel(5000, -5000, 1296, 972) == (-1.0, -1.0)
+
+
+def test_pick_target_prefers_a_recognised_face_then_the_biggest():
+    small_known = Target(100, 100, 50, 60, 0.9, 'owner')
+    big_unknown = Target(500, 400, 300, 400, 0.9, '')
+    mid_unknown = Target(300, 300, 200, 250, 0.9, '')
+    assert pick_target([big_unknown, small_known]) is small_known
+    assert pick_target([mid_unknown, big_unknown]) is big_unknown
+    assert pick_target([]) is None

ros/hermes_face/test/test_render.py

@@ -60,7 +60,7 @@ def test_pan_moves_the_eyes():
         frame, _ = run(r, 2)
         lum = frame.astype(np.float32).sum(axis=(0, 2))
         return float((lum * np.arange(W)).sum() / lum.sum())
-    assert centroid_x(-60) > centroid_x(0) > centroid_x(60)   # +pan = robot's left = viewer's right
+    assert centroid_x(60) > centroid_x(0) > centroid_x(-60)   # +pan = robot's left = viewer's right
 
 
 def test_box_blur_conserves_energy():
@@ -110,3 +110,37 @@ def test_wild_parameters_are_clamped():
 def test_unknown_phosphor_rejected():
     with pytest.raises(ValueError):
         FaceRenderer(phosphor='P99')
+
+
+def eye_centroid(r, seconds, t0=0.0):
+    frame, t = run(r, seconds, t0)
+    lum = frame[:130].astype(np.float32).sum(axis=(0, 2))         # the eyes live in the top half
+    rows = frame[:130].astype(np.float32).sum(axis=(1, 2))
+    return float((lum * np.arange(W)).sum() / lum.sum()), float((rows * np.arange(130)).sum() / rows.sum()), t
+
+
+def test_gaze_moves_the_eyes_toward_the_person_and_drifts_back():
+    r = FaceRenderer(state='idle', seed=0, jitter=0)
+    cx0, cy0, t = eye_centroid(r, 2)
+    r.set_gaze(1.0, 0.0, t)
+    cx1, _, t = eye_centroid(r, 1, t)           # held, so still glancing
+    assert cx1 > cx0 + 5
+    r.set_gaze(0.0, -1.0, t)
+    _, cy2, t = eye_centroid(r, 1, t)
+    assert cy2 < cy0 - 3                         # up on the display
+    cx3, cy3, _ = eye_centroid(r, 8, t)          # no refresh: hold expires, eyes return
+    assert abs(cx3 - cx0) < 2 and abs(cy3 - cy0) < 2
+
+
+def test_sleeping_ignores_gaze():
+    r = FaceRenderer(state='sleeping', seed=0, jitter=0)
+    cx0, _, t = eye_centroid(r, 4)
+    r.set_gaze(1.0, 0.0, t)
+    cx1, _, _ = eye_centroid(r, 2, t)
+    assert abs(cx1 - cx0) < 1.5
+
+
+def test_gaze_is_clamped():
+    r = FaceRenderer(seed=0)
+    r.set_gaze(50, -50, 0.0)
+    assert r._gaze == (1.0, -1.0)