diff --git a/pi/install-face-service.sh b/pi/install-face-service.sh
new file mode 100755
index 0000000..474c8f6
--- /dev/null
+++ b/pi/install-face-service.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+# Make the face start whenever the robot is powered. Run as root from this directory. Re-runnable.
+set -euo pipefail
+cd "$(dirname "$0")"
+install -m 644 robot-cpu-performance.service /etc/systemd/system/
+install -m 644 robot-face.service /etc/systemd/system/
+systemctl daemon-reload
+systemctl enable --now robot-cpu-performance.service
+systemctl enable robot-face.service
+systemctl restart robot-face.service
+sleep 8
+systemctl --no-pager --lines=4 status robot-face.service
diff --git a/pi/robot-cpu-performance.service b/pi/robot-cpu-performance.service
new file mode 100644
index 0000000..62dc565
--- /dev/null
+++ b/pi/robot-cpu-performance.service
@@ -0,0 +1,10 @@
+[Unit]
+Description=Hold the CPU at full clock (the face costs about half the CPU at 1.8 GHz as under ondemand)
+
+[Service]
+Type=oneshot
+RemainAfterExit=yes
+ExecStart=/bin/sh -c 'for g in /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor; do echo performance > "$g"; done'
+
+[Install]
+WantedBy=multi-user.target
diff --git a/pi/robot-face.service b/pi/robot-face.service
new file mode 100644
index 0000000..f7f32f1
--- /dev/null
+++ b/pi/robot-face.service
@@ -0,0 +1,16 @@
+[Unit]
+Description=Robot face (hermes_face): the display is always on while the robot is powered
+After=local-fs.target robot-cpu-performance.service
+
+[Service]
+User=hermes
+# lgpio (the GPIO backend for the display) drops a notification pipe, .lgd-nfy*, in its working
+# directory, which must be writable: systemd's default, /, is not.
+RuntimeDirectory=robot-face
+WorkingDirectory=/run/robot-face
+ExecStart=/bin/bash -c 'source /home/hermes/robot-env.sh && exec ros2 run hermes_face face_node'
+Restart=always
+RestartSec=2
+
+[Install]
+WantedBy=multi-user.target
diff --git a/ros/hermes_face/hermes_face/face_node.py b/ros/hermes_face/hermes_face/face_node.py
index d544678..b1e3178 100644
--- a/ros/hermes_face/hermes_face/face_node.py
+++ b/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')
diff --git a/ros/hermes_face/hermes_face/gaze.py b/ros/hermes_face/hermes_face/gaze.py
new file mode 100644
index 0000000..0b8c38d
--- /dev/null
+++ b/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))
diff --git a/ros/hermes_face/hermes_face/render.py b/ros/hermes_face/hermes_face/render.py
index d0d9d28..55ab90b 100644
--- a/ros/hermes_face/hermes_face/render.py
+++ b/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
diff --git a/ros/hermes_face/package.xml b/ros/hermes_face/package.xml
index 48028f2..57db6c8 100644
--- a/ros/hermes_face/package.xml
+++ b/ros/hermes_face/package.xml
@@ -10,6 +10,7 @@
rclpy
std_msgs
sensor_msgs
+ vision_msgs
python3-numpy
python3-pil
diff --git a/ros/hermes_face/test/test_gaze.py b/ros/hermes_face/test/test_gaze.py
new file mode 100644
index 0000000..3301973
--- /dev/null
+++ b/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
diff --git a/ros/hermes_face/test/test_render.py b/ros/hermes_face/test/test_render.py
index 8b06d75..776de97 100644
--- a/ros/hermes_face/test/test_render.py
+++ b/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)