Add hermes_face ROS package and verify the display on the Pi
The Pi (hermes-bot) is flashed, upgraded and running ROS 2 Jazzy. The ST7789 works through Blinka on lgpio: pass cs=None (the kernel owns CE0), and the glass sits at y_offset=80 in the controller RAM for every rotation, since the driver rotates in software. The mounted pose needs rotation=270. hermes_face is a numpy port of the flat face from docs/face-prototype.html, with phosphor and vector styles, plus a new `sleeping` pose for "no agent connected". The renderer has no ROS or hardware imports. The tone map is a lookup table and the glow runs at half resolution, so the face reaches ~28 fps on the panel at 24% of a core (20 fps) to 40% (30 fps) with the CPU at full clock. CONTEXT.md now records the Pi, the cloud-init first-boot-only gotcha, and what is left. pi/50-cloud-init.yaml is a gitignored Wi-Fi template. Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs
This commit is contained in:
Claude Sonnet 5.5
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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]
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>hermes_face</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Oscilloscope-phosphor face for the Hermes robot's ST7789 display.</description>
|
||||
<maintainer email="noreply@example.com">K. Isom</maintainer>
|
||||
<license>UNLICENSED</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-pil</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/hermes_face
|
||||
[install]
|
||||
install_scripts=$base/lib/hermes_face
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user