diff --git a/.gitignore b/.gitignore index 288b655..b48de44 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ __pycache__/ *.orig *.rej .venv/ +pi/50-cloud-init.yaml diff --git a/CONTEXT.md b/CONTEXT.md index 51af791..65e2cd0 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -4,43 +4,83 @@ Last updated 2026-10-04. `HANDOFF.md` is the brief; this file tracks where the w ## Hardware on hand -Raspberry Pi 4, BrainCraft HAT, Arducam UC-346 camera. Not yet: servos, USB-C splitter, printed -enclosure, speakers. Work until those arrive is limited to what the Pi, HAT and camera can do. +Raspberry Pi 4 (8 GB), BrainCraft HAT, Arducam UC-346 camera. Not yet: servos, USB-C splitter, +printed enclosure, speakers. Work until those arrive is limited to what the Pi, HAT and camera can do. + +## The Pi (`hermes-bot`) + +- Ubuntu 24.04.5 server, kernel `6.8.0-1065-raspi` (fully upgraded), flashed from `talos`. User + `hermes` (key-only; passwordless sudo), SSH keys for `kyle@vade` (talos) and `kyle@straylight`. +- On Wi-Fi at `192.168.88.152` (MAC `d8:3a:dd:0d:66:f5`); `eth0` is not cabled. The owner says it has + a static IP on the rack switch; that address is not recorded here yet. `avahi-daemon` is + installed, but mDNS is off in `systemd-resolved` on talos, so `hermes-bot.local` doesn't resolve + there; use the IP. +- **cloud-init applies `network-config` and `user-data` on the first boot only.** To change Wi-Fi + later, edit `/etc/netplan/50-cloud-init.yaml` on the Pi, or bump `instance_id` in `meta-data` on + `system-boot`. `pi/50-cloud-init.yaml` is a template for the Wi-Fi block; it is gitignored + because it holds credentials. +- Installed: ROS 2 Jazzy (`ros-base`, `ros-dev-tools`, colcon, `robot_state_publisher`, `xacro`, + `camera_ros`); `python3-numpy/pil/spidev/lgpio/rpi-lgpio`; a venv `~/venv` (system site packages + visible) with `adafruit-circuitpython-rgb-display` and Blinka. Workspace: `~/ros_ws`, source synced + from `ros/` with `rsync -a --delete --exclude __pycache__ ros/ hermes@:~/ros_ws/src/`, then + `colcon build --symlink-install`. +- CPU governor is `ondemand`; at full clock the face costs about half as much CPU. Set + `performance` in the installer (package 5). It is not persistent yet. ## Package 0a (platform proof on Ubuntu 24.04) -- **Step 1 done.** `host/check-pi-platform.sh` passes: Ubuntu's `6.8.0-1065-raspi` kernel ships - every driver and overlay needed (WM8960 audio, OV5647, Unicam, PWM, SPI, dwc2 gadget with - NCM/ECM/ACM). The script needed a fix for uncompressed `data.tar` (applied to `host/` and the - duplicate in `tools/`; committed). -- **Step 2 in progress: flashing the SD card.** Image chosen: - `ubuntu-24.04.5-preinstalled-server-arm64+raspi.img.xz`, SHA256 - `b23371a5c8d02f612c26e7c18cacb81c5a48f968c7c66a2645391320aa94351e` (from - cdimage.ubuntu.com/releases/24.04/release/SHA256SUMS). It was downloaded to the session - scratchpad under `/tmp`, which a reboot likely cleared; re-download and re-verify. -- The Pi is not running yet. No card has been written. `straylight` (NixOS; has `xz`, `dd`, - `udisksctl`, no `rpi-imager`) never detected the Framework SD expansion card: no kernel event - in two bays, so the flashing moves to **`talos`**, where the owner confirmed the card mounts. - Continue the session there: clone the repo, read this file, re-download and re-verify the - image (the copy on straylight is in its `/tmp`). Check talos's OS and available tools first. -- **Open questions for the owner before flashing:** Wi-Fi credentials or Ethernet for first boot; - hostname (proposed `hermes-robot`); username; SSH key (default `~/.ssh/id_ed25519.pub`). - Plan: write the image after the owner confirms the device from `lsblk`, then edit cloud-init - on the `system-boot` partition (user, key, hostname, SSH, network). -- Remaining 0a steps after boot: ROS 2 Jazzy, audio (`wm8960-soundcard` overlay + mixer), - camera (`cam -l`, `camera_ros`), display test pattern. PWM test waits for the servos. Ask - before rebooting the Pi. +- Step 1 done: `host/check-pi-platform.sh` passes. +- Step 2 done: flashed, booted, upgraded; ROS installed; `ros2 topic list` works. +- **Step 5 (display) done.** ST7789 works on `lgpio` through Blinka (`rpi-lgpio`, no `RPi.GPIO`). + Two gotchas: pass `cs=None` (the kernel drives CE0; claiming GPIO8 raises "GPIO busy"), and the + 240x240 glass sits at `y_offset=80` in the controller's RAM, for every rotation (the driver + rotates in software and leaves the scan direction fixed). Mounted pose (microSD end up, USB-C edge + on the left) needs content turned 90 degrees clockwise: `rotation=270`. Full-frame push is + ~23 fps through the driver; the face pushes raw RGB565 with `_block` and reaches ~27-28 fps. +- Not done: audio (`wm8960-soundcard` overlay + mixer), camera (`cam -l`, `camera_ros`), PWM + (waits for servos). Ask before rebooting the Pi. + +## Face (`ros/hermes_face`) + +- `render.py`: numpy port of `docs/face-prototype.html`, **flat face only** (head and lowpoly not + ported). Phosphor and vector styles. States: `sleeping idle listening thinking looking speaking`. + `sleeping` (new) is the "no agent connected" pose: eyes closed to thin lines, mouth short, dim and + slowly breathing. `looking` returns to the state it interrupted. Tone mapping is a lookup table + (RGB and RGB565 tables), glow is computed at half resolution, rotation is a free `np.rot90`. +- `display.py`: ST7789 driver plus an async pusher (render overlaps the SPI transfer). +- `face_node.py`: node `hermes_face`; subscribes `/hermes/state` (`std_msgs/String`) and + `/joint_states` (`pan_joint`, `tilt_joint`, radians); starts in `initial_state` (`sleeping`), so it + runs with nothing else up. Parameters: `rotation` (270), `x_offset` (0), `y_offset` (80), + `baudrate`, `fps`, `output` (`st7789` or `none`), `phosphor` (P7), `style`, `persistence_ms`, + `focus`, `jitter`, `intensity`, `initial_state`. +- `demo.py`: no ROS. `python -m hermes_face.demo --sequence` cycles the states on the panel; + `--png-dir DIR` writes a PNG per state without hardware. +- Tests: `python3 -m pytest test` in `ros/hermes_face` (23 pass on the Pi). +- Measured on the Pi 4: ~9 ms per frame in isolation. With the CPU at full clock: 24% of a core at + 20 fps and 40% at 30 fps (target was under 40% at 20 fps or more). Under `ondemand` the same + load reads 40% at 20 fps and ~67% at 30 fps. No C++ rewrite was needed. +- Not yet: audio levels for the mouth (`/audio/levels`), `hermes_mcp` publishing states, launch + file in `hermes_bringup`, head/lowpoly shapes, DotStar LEDs, button mute. +- Open: IPS black level. The backlight is on at full brightness, so black reads dark gray on + camera. Dimming it needs a software PWM on GPIO26 (not a hardware PWM pin). + +## Next + +1. Package 0b: USB gadget Ethernet (`dwc2` peripheral, NCM + ACM, `usb0` at `10.55.0.2/30`). Needs a + reboot and a USB-C data cable to a host; the splitter has not arrived, so a bench test would + power the Pi from the host's port (fine without servos). Keep Wi-Fi up throughout. +2. Audio (package 1), camera (package 3), then the rest of the packages in `HANDOFF.md`. ## Repository -Git repo initialised on `main` with one commit ("Initial commit"). `.gitignore` covers -`.remember/`, caches, `.venv/`, `*.orig`, `*.rej`. The earlier zip and patch files were deleted. -`tools/check-pi-platform.sh` duplicates `host/check-pi-platform.sh` and can probably go. +Branch `master`. `.gitignore` covers `.remember/`, caches, `.venv/`, `*.orig`, `*.rej` and +`pi/50-cloud-init.yaml`. `tools/check-pi-platform.sh` duplicates `host/check-pi-platform.sh` and can +probably go. ## Security review notes (parked until the USB link is up) An automated review of the initial commit flagged three items. All stem from the code predating -the USB-gadget design (HANDOFF.md package 0b); none is exposed until the Pi runs. +the USB-gadget design (HANDOFF.md package 0b); none is exposed until the Pi runs the services. - `pi/robot_mcp_server.py`: plain HTTP, `ROBOT_MCP_BIND` defaults to `0.0.0.0`. Plan: default to `10.55.0.2`, start after `usb0`. diff --git a/pi/display_test.py b/pi/display_test.py new file mode 100644 index 0000000..d79bc44 --- /dev/null +++ b/pi/display_test.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +"""Draw a test pattern on the BrainCraft HAT's ST7789 (240x240, SPI CE0, DC=GPIO25, BL=GPIO26). + + display_test.py [ROTATION] one pattern, then a frame-rate probe + +The driver rotates in software (PIL, counter-clockwise), and the panel's 80-row RAM offset belongs +to the glass, so y_offset=80 holds for every rotation. 270 is the mounted pose (microSD end up). +A correct picture has the frame complete on all four edges, no noise, the white triangle in the +top-left corner as you look at the panel in its mounted pose, and upright text. +""" +import sys +import time + +import board +import digitalio +from adafruit_rgb_display import st7789 +from PIL import Image, ImageDraw, ImageFont + +W = H = 240 + + +def make_display(rotation, x_offset, y_offset, baudrate=48_000_000): + return st7789.ST7789( + board.SPI(), + cs=None, # CE0 is driven by the kernel SPI driver; claiming GPIO8 fails with 'GPIO busy' + dc=digitalio.DigitalInOut(board.D25), + rst=None, + baudrate=baudrate, + width=W, + height=H, + x_offset=x_offset, + y_offset=y_offset, + rotation=rotation, + ) + + +def pattern(label): + img = Image.new("RGB", (W, H), "black") + d = ImageDraw.Draw(img) + for i, c in enumerate(["red", "lime", "blue", "yellow", "cyan", "magenta"]): + d.rectangle([i * 40, 90, i * 40 + 39, 150], fill=c) + d.rectangle([0, 0, W - 1, H - 1], outline="white") # edge frame: shows offset/clipping + d.rectangle([3, 3, W - 4, H - 4], outline="gray") + d.polygon([(8, 8), (58, 8), (8, 58)], fill="white") # top-left marker: shows rotation + d.text((70, 12), label[0], fill="white", font=ImageFont.load_default(size=48)) + d.text((70, 66), label[1], fill="white", font=ImageFont.load_default(size=14)) + for x in range(0, W, 30): + d.line([(x, 160), (x, H - 8)], fill="gray") + for y in range(160, H - 8, 20): + d.line([(0, y), (W, y)], fill="gray") + return img + + +backlight = digitalio.DigitalInOut(board.D26) +backlight.switch_to_output(value=True) + +rotation = int(sys.argv[1]) if len(sys.argv) > 1 else 270 +disp = make_display(rotation, 0, 80) +img = pattern((str(rotation), f"rot={rotation} y_off=80")) +disp.image(img) +t0, n = time.monotonic(), 30 +for _ in range(n): + disp.image(img) +print(f"pattern shown; full-frame push rate {n / (time.monotonic() - t0):.1f} fps at 48 MHz") diff --git a/ros/hermes_face/hermes_face/__init__.py b/ros/hermes_face/hermes_face/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_face/hermes_face/demo.py b/ros/hermes_face/hermes_face/demo.py new file mode 100644 index 0000000..5eb2602 --- /dev/null +++ b/ros/hermes_face/hermes_face/demo.py @@ -0,0 +1,94 @@ +"""Run the face without ROS: on the Pi's display, or to PNG files for checking the look. + + python -m hermes_face.demo --sequence # cycle through every state on the panel + python -m hermes_face.demo --state speaking --seconds 10 + python -m hermes_face.demo --png-dir /tmp/face # one PNG per state, no hardware +""" +from __future__ import annotations + +import argparse +import os +import time + +import numpy as np + +from .render import STATES, FaceRenderer + +SEQUENCE = (('sleeping', 4.0), ('idle', 3.5), ('listening', 3.6), ('thinking', 2.4), + ('looking', 1.3), ('thinking', 1.4), ('speaking', 6.5)) + + +def write_pngs(args) -> None: + from PIL import Image + os.makedirs(args.png_dir, exist_ok=True) + for state in STATES: + r = FaceRenderer(args.phosphor, args.style, state=state, seed=1, rotation=args.rotation) + r.set_pose(args.pan, args.tilt) + dt, t = 1.0 / args.fps, 0.0 + settle = 5.0 if state in ('sleeping', 'thinking') else 3.0 + for _ in range(int(settle * args.fps)): + t += dt + frame = r.frame(t, dt) + Image.fromarray(frame).resize((480, 480), Image.NEAREST).save( + os.path.join(args.png_dir, f'{args.style}-{state}.png')) + print('wrote', state) + + +def run(args) -> None: + from .display import AsyncDisplay, St7789 + panel = AsyncDisplay(St7789(args.x_offset, args.y_offset, args.baud)) + r = FaceRenderer(args.phosphor, args.style, state=args.state, now=time.monotonic(), + rotation=args.rotation) + r.set_pose(args.pan, args.tilt) + period = 1.0 / args.fps + start = last = time.monotonic() + seq_i, seq_t0 = 0, start + if args.sequence: + r.set_state(SEQUENCE[0][0], start) + frames, stat_t, cpu0, render_s = 0, start, time.process_time(), 0.0 + try: + while args.seconds <= 0 or time.monotonic() - start < args.seconds: + now = time.monotonic() + if args.sequence and now - seq_t0 >= SEQUENCE[seq_i][1]: + seq_i, seq_t0 = (seq_i + 1) % len(SEQUENCE), now + r.set_state(SEQUENCE[seq_i][0], now) + t0 = time.perf_counter() + rgb = r.frame(now, now - last, 'rgb565') + render_s += time.perf_counter() - t0 + last = now + panel.show(rgb) + frames += 1 + if now - stat_t >= 5.0: + wall = now - stat_t + print(f'{r.state:>9}: render {frames / wall:5.1f} fps ({1000 * render_s / frames:4.1f} ms/frame), ' + f'panel {panel.pushed / wall:5.1f} fps, cpu {100 * (time.process_time() - cpu0) / wall:3.0f}%') + frames, stat_t, cpu0, render_s, panel.pushed = 0, now, time.process_time(), 0.0, 0 + time.sleep(max(0.0, period - (time.monotonic() - now))) + except KeyboardInterrupt: + pass + finally: + panel.close() + + +def main() -> None: + p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument('--state', choices=STATES, default='idle') + p.add_argument('--sequence', action='store_true', help='cycle through the states') + p.add_argument('--seconds', type=float, default=0, help='stop after this long (0 = until Ctrl-C)') + p.add_argument('--fps', type=float, default=30) + p.add_argument('--phosphor', default='P7', choices=['P31', 'P1', 'P7', 'P3']) + p.add_argument('--style', default='phosphor', choices=['phosphor', 'vector']) + p.add_argument('--pan', type=float, default=0, help='degrees') + p.add_argument('--tilt', type=float, default=0, help='degrees') + p.add_argument('--rotation', type=int, default=270, choices=[0, 90, 180, 270], + help='degrees counter-clockwise; 270 is the mounted pose (microSD end up)') + p.add_argument('--x-offset', type=int, default=0) + p.add_argument('--y-offset', type=int, default=80) + p.add_argument('--baud', type=int, default=48_000_000) + p.add_argument('--png-dir', help='write one PNG per state here and exit (no hardware)') + args = p.parse_args() + write_pngs(args) if args.png_dir else run(args) + + +if __name__ == '__main__': + main() diff --git a/ros/hermes_face/hermes_face/display.py b/ros/hermes_face/hermes_face/display.py new file mode 100644 index 0000000..966dc2a --- /dev/null +++ b/ros/hermes_face/hermes_face/display.py @@ -0,0 +1,81 @@ +"""BrainCraft HAT display: ST7789, 240x240, SPI CE0, DC=GPIO25, backlight=GPIO26. + +Hardware imports are deferred so the renderer and tests run on hosts without a Pi. +""" +from __future__ import annotations + +import threading + +import numpy as np + + +class St7789: + """Frames arrive already rotated (FaceRenderer(rotation=...)), so the driver's own rotation is + left at 0. The 80-row offset belongs to the panel, not to a rotation: a 240x240 glass sits in a + 240x320 controller RAM, and the driver rotates in software, leaving the scan direction fixed.""" + + def __init__(self, x_offset: int = 0, y_offset: int = 80, baudrate: int = 48_000_000): + import board + import digitalio + from adafruit_rgb_display import st7789 + self._backlight = digitalio.DigitalInOut(board.D26) + self._backlight.switch_to_output(value=True) + self._disp = st7789.ST7789( + board.SPI(), + cs=None, # the kernel drives CE0; claiming GPIO8 fails with 'GPIO busy' + dc=digitalio.DigitalInOut(board.D25), + rst=None, + baudrate=baudrate, + width=240, height=240, + x_offset=x_offset, y_offset=y_offset, # 240x240 panel in a 240x320 controller + rotation=0, + ) + + def show(self, frame) -> None: + """Push a frame: big-endian RGB565 bytes (fast path), or an (H, W, 3) uint8 array.""" + if not isinstance(frame, (bytes, bytearray)): + from PIL import Image + self._disp.image(Image.fromarray(frame, 'RGB')) + return + self._disp._block(0, 0, 239, 239, frame) # the driver's own windowing, minus its PIL conversion + + def close(self) -> None: + self.show(bytes(240 * 240 * 2)) + self._backlight.value = False + + +class AsyncDisplay: + """Pushes frames on a worker thread so rendering the next one overlaps the SPI transfer. + Only the newest pending frame is kept; if the panel is slower than the renderer, frames drop.""" + + def __init__(self, display: St7789): + self._display = display + self._latest = None + self._cv = threading.Condition() + self._stop = False + self.pushed = 0 + self._thread = threading.Thread(target=self._run, name='display-push', daemon=True) + self._thread.start() + + def show(self, frame) -> None: + with self._cv: + self._latest = frame + self._cv.notify() + + def _run(self) -> None: + while True: + with self._cv: + while self._latest is None and not self._stop: + self._cv.wait() + if self._stop: + return + frame, self._latest = self._latest, None + self._display.show(frame) + self.pushed += 1 + + def close(self) -> None: + with self._cv: + self._stop = True + self._cv.notify() + self._thread.join(timeout=2) + self._display.close() diff --git a/ros/hermes_face/hermes_face/face_node.py b/ros/hermes_face/hermes_face/face_node.py new file mode 100644 index 0000000..2354e3e --- /dev/null +++ b/ros/hermes_face/hermes_face/face_node.py @@ -0,0 +1,93 @@ +"""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) +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 JointState +from std_msgs.msg import String + +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 = float(param('fps', 30.0)) + 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.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) + 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}') + + 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 + self.renderer.set_state(msg.data, time.monotonic()) + + 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 _tick(self) -> None: + now = time.monotonic() + 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() diff --git a/ros/hermes_face/hermes_face/render.py b/ros/hermes_face/hermes_face/render.py new file mode 100644 index 0000000..21e0d0c --- /dev/null +++ b/ros/hermes_face/hermes_face/render.py @@ -0,0 +1,386 @@ +"""Oscilloscope-phosphor face: a numpy port of docs/face-prototype.html (flat face). + +No ROS or hardware imports, so it runs on any host and in tests. The model is the prototype's: +a 240x240 float32 energy buffer that decays each frame, a beam traced at constant speed that +deposits energy with bilinear splatting, two box-blur passes for glow, and a tone map tinted +with the phosphor colour. Only the flat shape is ported so far (head and lowpoly are not). + +Beyond the prototype: a `sleeping` state for "no agent connected", and `looking` returns to the +state it interrupted instead of always going to idle. +""" +from __future__ import annotations + +import math +from dataclasses import dataclass + +import numpy as np + +W = H = 240 +N = W * H + +PHOSPHORS = { + 'P31': (0.30, 1.00, 0.45), # bright green, the usual scope tube + 'P1': (0.45, 1.00, 0.25), # yellower green, longer-glowing + 'P7': (0.62, 0.80, 1.00), # blue-white flash, radar screens + 'P3': (1.00, 0.68, 0.18), # amber +} + +STATES = ('sleeping', 'idle', 'listening', 'thinking', 'looking', 'speaking') +LOOK_HOLD_S = 1.2 # how long `looking` holds before returning to the prior state +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 + + +def _approach(cur: float, target: float, rate: float, dt: float) -> float: + return cur + (target - cur) * (1.0 - math.exp(-rate * dt)) + + +# ---------------------------------------------------------------- synthetic voices +def _hash(i: float, seed: float) -> float: + x = math.sin(i * 127.1 + seed * 311.7) * 43758.5453 + return x - math.floor(x) + + +_VOWELS = ((730, 1090), (270, 2290), (300, 870), (530, 1840), (570, 840), (660, 1720)) +_K = np.arange(1, 10, dtype=np.float64) + + +@dataclass +class Voice: + env: float + f0: float + w: np.ndarray + norm: float + noise: float + + +def voice(T: float, seed: float, base_f0: float, rate: float) -> Voice: + u = T * rate + i = math.floor(u) + frac = u - i + pause = _hash(i, seed) < 0.16 + amp = 0.0 if pause else 0.55 + 0.45 * _hash(i + 7, seed) + env = amp * math.sin(math.pi * frac) ** 0.7 + v0 = _VOWELS[int(math.floor(_hash(i, seed + 3) * len(_VOWELS)))] + v1 = _VOWELS[int(math.floor(_hash(i + 1, seed + 3) * len(_VOWELS)))] + m = frac * frac * (3 - 2 * frac) + f1 = v0[0] + (v1[0] - v0[0]) * m + f2 = v0[1] + (v1[1] - v0[1]) * m + f0 = base_f0 * (1 + 0.12 * math.sin(T * 1.7 + seed) + 0.06 * math.sin(T * 4.3)) + f = _K * f0 + d1 = (f - f1) / 160.0 + d2 = (f - f2) / 260.0 + a = np.exp(-d1 * d1) + 0.65 * np.exp(-d2 * d2) + np.where(_K == 1, 0.35, 0.04) + noise = (0.14 - frac) * 3.2 if frac < 0.14 and not pause else 0.0 + return Voice(env, f0, a, float(a.sum()), noise) + + +def voice_sample(v: Voice, t: np.ndarray, phase: float) -> np.ndarray: + """The voice's waveform at local times t (seconds) inside a trigger-locked window.""" + arg = 2 * math.pi * _K[:, None] * v.f0 * t[None, :] + phase * _K[:, None] + return (v.w[:, None] * np.sin(arg)).sum(axis=0) / v.norm + + +# ---------------------------------------------------------------- blur +def box_blur(a: np.ndarray, r: int) -> np.ndarray: + """Separable box blur, zero beyond the edges, via cumulative sums (conserves total energy).""" + k = 2 * r + 1 + for axis in (1, 0): + n = a.shape[axis] + shape = list(a.shape) + shape[axis] = n + 2 * r + 1 + p = np.zeros(shape, np.float32) + body = [slice(None), slice(None)] + tail = [slice(None), slice(None)] + last = [slice(None), slice(None)] + lo = [slice(None), slice(None)] + hi = [slice(None), slice(None)] + body[axis] = slice(r + 1, r + 1 + n) + tail[axis] = slice(r + 1 + n, None) + last[axis] = slice(r + n, r + n + 1) + lo[axis] = slice(0, n) + hi[axis] = slice(k, k + n) + np.cumsum(a, axis=axis, dtype=np.float32, out=p[tuple(body)]) + p[tuple(tail)] = p[tuple(last)] + a = (p[tuple(hi)] - p[tuple(lo)]) * np.float32(1.0 / k) + return a + + +_LUT_SCALE = 512 # tone-map table entries per unit of energy +_LUT_SIZE = 8 * _LUT_SCALE # covers energy 0..8, far beyond any overdriven spot + + +def _rgb565_be(rgb: np.ndarray) -> np.ndarray: + r, g, b = (rgb[..., i].astype(np.uint16) for i in range(3)) + return (((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3)).astype('>u2') # big-endian for SPI + + +class FaceRenderer: + def __init__(self, phosphor: str = 'P7', style: str = 'phosphor', persist_ms: float = 70.0, + intensity: float = 1.0, focus: float = 0.35, jitter: float = 0.3, + state: str = 'idle', now: float = 0.0, seed: int | None = None, rotation: int = 0): + if style not in ('phosphor', 'vector'): + raise ValueError(f'unknown style {style!r}') + if rotation not in (0, 90, 180, 270): + raise ValueError(f'rotation must be 0, 90, 180 or 270, not {rotation!r}') + self.rotation = rotation # output turned this many degrees counter-clockwise + self.rgb = PHOSPHORS[phosphor] + self.style = style + self.persist_ms = persist_ms + self.intensity = intensity + self.focus = focus + self.jitter = jitter + self.rng = np.random.default_rng(seed) + + self.buf = np.zeros((H, W), np.float32) + self._xi: list[np.ndarray] = [] + self._yi: list[np.ndarray] = [] + self._w: list[np.ndarray] = [] + self.beam = (120.0, 120.0) + self.flash = 0.0 + + self.state = state + self._state_t0 = now + self._return_state = 'idle' + self._flash_pending = state == 'looking' + self.pan = 0.0 # degrees, from the servos + self.tilt = 0.0 + 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, + sleep=1.0 if state == 'sleeping' else 0.0) + self._next_blink = now + 2.5 + self._blink_t = -1.0 + self._build_lut() + + def _build_lut(self) -> None: + """Tone curve as a table indexed by energy, so a frame is one gather, not exp/clip/stack.""" + v = np.arange(_LUT_SIZE, dtype=np.float64) / _LUT_SCALE + pr = np.array(self.rgb) + if self.style == 'vector': + c = pr + (1 - pr) * 0.35 # pale the tint so lines read as light, not phosphor + out = np.minimum(1.0, c[None, :] * v[:, None]) + else: + base = 1 - np.exp(-v * 1.6) + hot = np.clip((v - 1.1) * 0.35, 0, 0.75) # overdriven spots whiten + out = np.minimum(1.0, pr[None, :] * base[:, None] + hot[:, None]) + self._lut_rgb = (out * 255).astype(np.uint8) + self._lut_565 = _rgb565_be(self._lut_rgb) + + # ------------------------------------------------------------ inputs + def set_state(self, s: str, now: float) -> None: + if s not in STATES: + raise ValueError(f'unknown state {s!r}') + if s == self.state and s != 'looking': + return + if s == 'looking' and self.state != 'looking': + self._return_state = self.state + self.state = s + self._state_t0 = now + if s == 'looking': + self._flash_pending = True + + def set_pose(self, pan_deg: float, tilt_deg: float) -> None: + self.pan, self.tilt = pan_deg, tilt_deg + + # ------------------------------------------------------------ beam + def _queue(self, x, y, e) -> None: + x = np.atleast_1d(np.asarray(x, np.float64)) + y = np.atleast_1d(np.asarray(y, np.float64)) + if self.jitter > 0 and self.style == 'phosphor': + x = x + (self.rng.random(x.shape) - 0.5) * 2 * self.jitter + y = y + (self.rng.random(y.shape) - 0.5) * 2 * self.jitter + xi = np.floor(x).astype(np.int64) + yi = np.floor(y).astype(np.int64) + ok = (xi >= 0) & (yi >= 0) & (xi < W - 1) & (yi < H - 1) + e = np.broadcast_to(np.asarray(e, np.float64), x.shape) + self._xi.append(xi[ok]) + self._yi.append(yi[ok]) + fx = (x - xi)[ok] + fy = (y - yi)[ok] + self._w.append(np.stack([e[ok] * (1 - fx) * (1 - fy), e[ok] * fx * (1 - fy), + e[ok] * (1 - fx) * fy, e[ok] * fx * fy])) + + def _flush(self) -> None: + if not self._xi: + return + xi = np.concatenate(self._xi) + yi = np.concatenate(self._yi) + w = np.concatenate(self._w, axis=1) + idx = yi * W + xi + acc = np.bincount(np.concatenate([idx, idx + 1, idx + W, idx + W + 1]), + weights=w.reshape(-1), minlength=N) + self.buf += acc.reshape(H, W).astype(np.float32) + self._xi.clear() + self._yi.clear() + self._w.clear() + + def _stroke(self, x: np.ndarray, y: np.ndarray, e: float) -> None: + if self.style == 'vector' and len(x) > 1: # keep line weight even: fill the gaps + gaps = np.floor(np.hypot(np.diff(x), np.diff(y)) / 0.7).astype(np.int64) + reps = gaps + 1 + seg = np.repeat(np.arange(len(reps)), reps) + within = np.arange(len(seg)) - np.repeat(np.cumsum(reps) - reps, reps) + t = within / np.repeat(reps, reps) + x, y = (np.append(x[seg] + (x[seg + 1] - x[seg]) * t, x[-1]), + np.append(y[seg] + (y[seg + 1] - y[seg]) * t, y[-1])) + self._queue(x, y, e) + + def _jump_to(self, x: float, y: float, e: float) -> None: + if self.style == 'phosphor': # a vector display blanks the beam on retrace + bx, by = self.beam + n = max(4, round(math.hypot(x - bx, y - by) / 1.5)) + j = np.arange(1, n + 1) / n + self._queue(bx + (x - bx) * j, by + (y - by) * j, e * 0.045) + self.beam = (x, y) + + def _trace(self, fn, n: int, e: float) -> None: + x, y = fn(np.linspace(0.0, 1.0, n + 1)) + self._jump_to(float(x[0]), float(y[0]), e) + self._stroke(x, y, e) + self.beam = (float(x[-1]), float(y[-1])) + + # ------------------------------------------------------------ the face + def _draw(self, T: float, dt: float) -> None: + a = self.a + sleep = a['sleep'] + e = 0.45 * (dt * 30) * (1.0 if self.style == 'vector' else + self.intensity * (70.0 / self.persist_ms) ** 0.6) + if sleep > 0.001: # asleep: dim, with a slow breathing swell + 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 + eyes = [(82 + ex, 98 + ey), (158 + ex, 98 + ey)] + rx = 21 * a['eye_scale'] + ry = 25 * a['eye_scale'] * a['open'] + listening = self.state == 'listening' + lv = voice(T, 9, 190, 4.2) if listening or a['mouth_listen'] > 0.01 else None + listen_pulse = 0.12 * lv.env if listening else 0.0 + + # eyes: an ellipse morphing to a rotating 3:2 Lissajous figure while thinking + k = a['think'] + for idx, (cx, cy) in enumerate(eyes): + ph = T * 1.6 + idx * math.pi * 0.5 + + def eye(u, cx=cx, cy=cy, ph=ph): + th = u * 2 * math.pi + ox, oy = rx * np.cos(th), ry * np.sin(th) + lx, ly = rx * 0.95 * np.sin(3 * th + ph), ry * 0.95 * np.sin(2 * th) + return cx + ox + (lx - ox) * k, cy + oy + (ly - oy) * k + self._trace(eye, 260 + round(160 * k), e * (1 + listen_pulse)) + + # mouth: a smile at rest, the live waveform when speaking or listening + mx0, mx1 = 70 + ex * 0.6, 170 + ex * 0.6 + my = 166 + ey * 0.6 + 4.0 * sleep + sv = voice(T, 1, 118, 4.6) if a['mouth_speak'] > 0.01 else None + win_s, win_l = (2.4 / sv.f0 if sv else 0.0), (3.2 / lv.f0 if lv else 0.0) + speak_amp = 26 * a['mouth_speak'] * sv.env if sv else 0.0 + listen_amp = 12 * a['mouth_listen'] * lv.env if lv else 0.0 + smile = 7 * a['smile'] * (1 - a['mouth_speak']) * (1 - k * 0.8) + mouth_w = (mx1 - mx0) * (1 - 0.45 * k) * (1 - 0.5 * sleep) + m_start = (mx0 + mx1) / 2 - mouth_w / 2 + + def mouth(u): + x = m_start + u * mouth_w + c = 2 * u - 1 + y = my + smile * (1 - c * c) - smile * 0.4 + if speak_amp > 0.05: + y = y - speak_amp * voice_sample(sv, u * win_s, 0.0) + if sv.noise: + y = y - sv.noise * 5 * a['mouth_speak'] * (np.sin(u * 97 + T * 53) * + np.sin(u * 61 - T * 31)) + if listen_amp > 0.05: + y = y - listen_amp * voice_sample(lv, u * win_l, 0.4) + return x, y + self._trace(mouth, 320, e * 0.65 * (0.95 - 0.3 * a['mouth_listen'])) + self._jump_to(eyes[0][0] - rx, eyes[0][1], e) + + # ------------------------------------------------------------ per-frame update + def _update(self, T: float, dt: float) -> None: + a = self.a + if self.state == 'looking' and T - self._state_t0 > LOOK_HOLD_S: + self.set_state(self._return_state, T) + st = self.state + in_look = st == 'looking' + tl = T - self._state_t0 + + a['sleep'] = _approach(a['sleep'], 1.0 if st == 'sleeping' else 0.0, 2.5, dt) + a['think'] = _approach(a['think'], 1.0 if st == 'thinking' else 0.0, 6, dt) + a['mouth_speak'] = _approach(a['mouth_speak'], 1.0 if st == 'speaking' else 0.0, 10, dt) + a['mouth_listen'] = _approach(a['mouth_listen'], 1.0 if st == 'listening' else 0.0, 8, dt) + a['smile'] = _approach(a['smile'], {'idle': 1.0, 'speaking': 0.6, 'sleeping': 0.0}.get(st, 0.25), + 4, dt) + scale_t = 1.08 if st == 'listening' else 1.0 + 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) + + # blink, and the squint before a photo; none while thinking or asleep + open_t = SLEEP_OPEN if st == 'sleeping' else 1.0 + if in_look and tl < SQUINT_S: + open_t = 0.35 + if st not in ('thinking', 'sleeping') and not in_look: + if self._blink_t < 0 and T >= self._next_blink: + self._blink_t = T + if self._blink_t >= 0: + b = (T - self._blink_t) / 0.17 + if b >= 1: + self._blink_t = -1.0 + self._next_blink = T + 2.2 + float(self.rng.random()) * 4 + else: + open_t = max(0.06, abs(1 - 2 * b)) + if self._blink_t >= 0: + a['open'] = open_t + else: + rate = 3.0 if st == 'sleeping' else (6.0 if a['sleep'] > 0.1 else 18.0) # drift shut, wake gently + a['open'] = _approach(a['open'], open_t, rate, dt) + + if self._flash_pending and tl > FLASH_AT_S: # shutter: bloom the whole tube once + if self.style == 'phosphor': + self.buf += 0.55 + else: + self.flash = 0.6 + self._flash_pending = False + self.flash = max(0.0, self.flash - dt * 2.5) + + # ------------------------------------------------------------ tone map + def _glow(self, r: int) -> np.ndarray: + """Two blur passes, at half resolution (the glow is smooth) and repeated back up.""" + b = self.buf + half = (b[0::2, 0::2] + b[1::2, 0::2] + b[0::2, 1::2] + b[1::2, 1::2]) * np.float32(0.25) + rh = max(1, int(r / 2 + 0.5)) + g = box_blur(box_blur(half, rh), rh) + return np.repeat(np.repeat(g, 2, axis=0), 2, axis=1) + + def _energy_index(self) -> np.ndarray: + if self.style == 'vector': + k = np.float32(2.6 * self.intensity) + glow = box_blur(self.buf, 1) + v = (np.minimum(1, self.buf * k) * np.float32(0.9) + np.minimum(1, glow * k) * np.float32(0.25) + + np.float32(self.flash * 0.35)) + else: + r = 1 + round(self.focus * 5) + v = self.buf * np.float32(1.15 - self.focus * 0.55) + self._glow(r) * np.float32(0.6 + self.focus * 2.2) + v *= np.float32(_LUT_SCALE) + np.minimum(v, _LUT_SIZE - 1, out=v) + idx = v.astype(np.int32) + return np.rot90(idx, self.rotation // 90) if self.rotation else idx # free: the gather re-packs it + + def frame(self, T: float, dt: float, fmt: str = 'rgb'): + """Advance by dt seconds (clock at T). Returns an (H, W, 3) uint8 array for fmt='rgb', or + the frame as big-endian RGB565 bytes (what the ST7789 wants) for fmt='rgb565'.""" + dt = min(dt, 0.1) + if self.style == 'vector': + self.buf.fill(0) + else: + self.buf *= np.float32(math.exp(-(dt * 1000) / self.persist_ms)) + self._update(T, dt) + self._draw(T, dt) + self._flush() + idx = self._energy_index() + if fmt == 'rgb565': + return self._lut_565[idx].tobytes() + return self._lut_rgb[idx] diff --git a/ros/hermes_face/package.xml b/ros/hermes_face/package.xml new file mode 100644 index 0000000..48028f2 --- /dev/null +++ b/ros/hermes_face/package.xml @@ -0,0 +1,21 @@ + + + + hermes_face + 0.1.0 + Oscilloscope-phosphor face for the Hermes robot's ST7789 display. + K. Isom + UNLICENSED + + rclpy + std_msgs + sensor_msgs + python3-numpy + python3-pil + + python3-pytest + + + ament_python + + diff --git a/ros/hermes_face/resource/hermes_face b/ros/hermes_face/resource/hermes_face new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_face/setup.cfg b/ros/hermes_face/setup.cfg new file mode 100644 index 0000000..06c9f0f --- /dev/null +++ b/ros/hermes_face/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/hermes_face +[install] +install_scripts=$base/lib/hermes_face diff --git a/ros/hermes_face/setup.py b/ros/hermes_face/setup.py new file mode 100644 index 0000000..6091831 --- /dev/null +++ b/ros/hermes_face/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'hermes_face' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='K. Isom', + maintainer_email='noreply@example.com', + description='Oscilloscope-phosphor face for the Hermes robot.', + license='UNLICENSED', + entry_points={ + 'console_scripts': [ + 'face_node = hermes_face.face_node:main', + 'demo = hermes_face.demo:main', + ], + }, +) diff --git a/ros/hermes_face/test/test_render.py b/ros/hermes_face/test/test_render.py new file mode 100644 index 0000000..665ac63 --- /dev/null +++ b/ros/hermes_face/test/test_render.py @@ -0,0 +1,101 @@ +import numpy as np +import pytest + +from hermes_face.render import H, STATES, W, FaceRenderer, box_blur + + +def run(r, seconds, t0=0.0, fps=30): + t, frame = t0, None + for _ in range(int(seconds * fps)): + t += 1 / fps + frame = r.frame(t, 1 / fps) + return frame, t + + +def brightness(frame): + return float(frame.astype(np.float32).mean()) + + +@pytest.mark.parametrize('state', STATES) +@pytest.mark.parametrize('style', ['phosphor', 'vector']) +def test_every_state_draws_something(state, style): + frame, _ = run(FaceRenderer(style=style, state=state, seed=0), 3) + assert frame.shape == (H, W, 3) and frame.dtype == np.uint8 + assert frame.max() > 100 + + +def test_sleeping_is_dimmer_than_idle(): + idle, _ = run(FaceRenderer(state='idle', seed=0), 4) + asleep, _ = run(FaceRenderer(state='sleeping', seed=0), 4) + assert brightness(asleep) < 0.8 * brightness(idle) + + +def test_wakes_up(): + r = FaceRenderer(state='sleeping', seed=0) + _, t = run(r, 2) + r.set_state('idle', t) + frame, _ = run(r, 3, t) + idle, _ = run(FaceRenderer(state='idle', seed=0), 3) + assert r.a['open'] > 0.9 and abs(brightness(frame) - brightness(idle)) < 0.2 * brightness(idle) + + +def test_looking_returns_to_the_state_it_interrupted(): + r = FaceRenderer(state='thinking', seed=0) + _, t = run(r, 1) + r.set_state('looking', t) + assert r.state == 'looking' + run(r, 2, t) + assert r.state == 'thinking' + + +def test_unknown_state_rejected(): + with pytest.raises(ValueError): + FaceRenderer().set_state('dancing', 0.0) + + +def test_pan_moves_the_eyes(): + def centroid_x(pan): + r = FaceRenderer(state='idle', seed=0, jitter=0) + r.set_pose(pan, 0) + 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 + + +def test_box_blur_conserves_energy(): + b = np.zeros((H, W), np.float32) + b[120, 120] = 1.0 + out = box_blur(b, 2) + assert abs(out.sum() - 1.0) < 1e-3 + assert np.allclose(out[118:123, 118:123], 1 / 25, atol=1e-5) + + +def test_rgb565_frame_matches_rgb_frame(): + a = FaceRenderer(state='idle', seed=3) + b = FaceRenderer(state='idle', seed=3) + rgb, raw = None, None + for i in range(1, 40): + rgb = a.frame(i / 30, 1 / 30) + raw = b.frame(i / 30, 1 / 30, 'rgb565') + assert len(raw) == H * W * 2 + px = np.frombuffer(raw, '>u2').reshape(H, W) + assert np.array_equal(px >> 11, rgb[..., 0] >> 3) + assert np.array_equal((px >> 5) & 0x3F, rgb[..., 1] >> 2) + assert np.array_equal(px & 0x1F, rgb[..., 2] >> 3) + + +@pytest.mark.parametrize('rotation', [90, 180, 270]) +def test_rotation_turns_the_frame_counter_clockwise(rotation): + base = FaceRenderer(state='idle', seed=5, rotation=0) + turned = FaceRenderer(state='idle', seed=5, rotation=rotation) + a = b = None + for i in range(1, 30): + a = base.frame(i / 30, 1 / 30) + b = turned.frame(i / 30, 1 / 30) + assert np.array_equal(b, np.rot90(a, rotation // 90)) + + +def test_bad_rotation_rejected(): + with pytest.raises(ValueError): + FaceRenderer(rotation=45)