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)