Add hermes_audio and draw the mouth from the real waveform
audio_state taps the microphone and the speaker monitor with parec and publishes levels, a speaking/listening/quiet activity, and trigger-locked waveform windows; audio itself stays out of ROS. The activity detector calibrates its noise floor at start-up and absorbs steady noise, so a room at -43 dBFS does not read as "listening" forever, and it ignores the mic while the robot speaks. The face arbitrates the state it shows: sleeping (agent side only, so room sound never wakes a face that means "no agent connected") > speaking > listening > thinking > idle, and draws the mouth from the real samples while fresh scope data exists, auto-scaled so a quiet voice is still visible. Checked: a tone played to the sink shows as `speaking` with the mic ignored. The microphone-to-mouth path with a live voice is not yet confirmed. Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs
This commit is contained in:
+24
@@ -151,6 +151,30 @@ printed enclosure, speakers. Work until those arrive is limited to what the Pi,
|
||||
- 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).
|
||||
|
||||
## Audio levels and the literal voice trace (`ros/hermes_audio`, face integration)
|
||||
|
||||
- `hermes_audio` node `audio_state` taps the default source and the default sink's monitor with
|
||||
`parec` (16 kHz mono) and publishes `/audio/levels` ([mic_dbfs, speaker_dbfs]), `/audio/activity`
|
||||
(`speaking` | `listening` | `quiet`) and trigger-locked waveform windows `/audio/scope/{mic,speaker}`
|
||||
(256 samples). Audio itself never goes through ROS. `hermes_audio.levels.Activity` debounces and
|
||||
keeps an adaptive noise floor: a 3 s start-up calibration, then it drops fast and rises slowly, and
|
||||
anything 'listening' without a gap for 8 s is absorbed as ambient noise. The microphone is ignored
|
||||
while speaking (it would hear the robot).
|
||||
- The face arbitrates (`hermes_face.arbiter`): sleeping (agent side only) > speaking > listening >
|
||||
thinking > idle. **Room sound never wakes a sleeping face**: sleeping means "no agent connected".
|
||||
`looking` stays a timed event owned by the renderer. While fresh (< 0.35 s) scope data exists, the
|
||||
mouth draws the real waveform (speaker when speaking, mic when listening), auto-scaled.
|
||||
- **WirePlumber resets the codec's capture gain** to 100% (+30 dB) when it takes the device,
|
||||
overriding `alsactl`'s saved level. Fix: `pactl set-source-volume @DEFAULT_SOURCE@ 63%` (PipeWire's
|
||||
scale is cubic: 63% is -12 dB from the +30 dB maximum, so ALSA `Capture` lands at 46, about +17
|
||||
dB). WirePlumber remembers it; recheck after a reboot. The quiet-room floor at this gain is about
|
||||
-43 dBFS (preamp and mic noise).
|
||||
- Verified on the Pi: the `speaking` path (a tone played to the sink, with no speaker attached, shows
|
||||
on the monitor at -13.5 dBFS, `speaking` for its length, mic ignored); levels and scope publish;
|
||||
47 face tests, 8 audio tests. **Not yet verified:** the microphone-to-mouth path with a real voice
|
||||
(one live attempt saw no speech: the cue and the talking did not overlap), and the audio node as a
|
||||
service (it was run as a transient unit).
|
||||
|
||||
## Face recognition (opt-in)
|
||||
|
||||
`hermes_vision.recognize` / `people`: SFace embeddings on YuNet's landmarks. Nothing is stored until
|
||||
|
||||
@@ -0,0 +1,75 @@
|
||||
"""ROS node: how loud the robot's microphone and speaker are, and whether it is speaking or
|
||||
listening. Audio itself never goes through ROS; this reads PipeWire with `parec` for levels only.
|
||||
|
||||
Publishes: /audio/levels (std_msgs/Float32MultiArray): [mic_dbfs, speaker_dbfs]
|
||||
/audio/activity (std_msgs/String): speaking | listening | quiet (on change, and 2 Hz)
|
||||
/audio/scope/mic, /audio/scope/speaker (std_msgs/Float32MultiArray): a trigger-locked
|
||||
window of `scope_samples` samples at 16 kHz in [-1, 1], while there is sound: the face
|
||||
draws the mouth from it, so the trace is the real signal
|
||||
"""
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float32MultiArray, String
|
||||
|
||||
from .capture import PulseTap
|
||||
from .levels import Activity, rms_dbfs, trigger_window
|
||||
|
||||
RATE = 16000
|
||||
|
||||
|
||||
class AudioStateNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('hermes_audio_state')
|
||||
param = lambda name, default: self.declare_parameter(name, default).value # noqa: E731
|
||||
self.mic = PulseTap(param('mic_source', '@DEFAULT_SOURCE@'), RATE)
|
||||
self.spk = PulseTap(param('speaker_monitor', '@DEFAULT_MONITOR@'), RATE)
|
||||
self.n_scope = int(min(2048, max(64, int(param('scope_samples', 256)))))
|
||||
self.scope_floor_db = float(param('scope_floor_db', -70.0))
|
||||
self.activity = Activity(spk_db=float(param('speaker_threshold_db', -55.0)),
|
||||
margin_db=float(param('mic_margin_db', 9.0)))
|
||||
self.levels_pub = self.create_publisher(Float32MultiArray, '/audio/levels', 5)
|
||||
self.activity_pub = self.create_publisher(String, '/audio/activity', 5)
|
||||
self.scope_pub = {'mic': self.create_publisher(Float32MultiArray, '/audio/scope/mic', 2),
|
||||
'speaker': self.create_publisher(Float32MultiArray, '/audio/scope/speaker', 2)}
|
||||
self._last_state, self._last_pub = '', 0.0
|
||||
self._t = self.get_clock().now().nanoseconds / 1e9
|
||||
self.create_timer(1.0 / min(60.0, max(5.0, float(param('rate_hz', 30.0)))), self._tick)
|
||||
self.get_logger().info('audio state up: mic and speaker monitor via parec')
|
||||
|
||||
def _tick(self) -> None:
|
||||
now = self.get_clock().now().nanoseconds / 1e9
|
||||
dt, self._t = now - self._t, now
|
||||
win = RATE // 10 # 100 ms for the levels
|
||||
mic_db = rms_dbfs(self.mic.latest(win)) if self.mic.alive() else -100.0
|
||||
spk_db = rms_dbfs(self.spk.latest(win)) if self.spk.alive() else -100.0
|
||||
state = self.activity.update(mic_db, spk_db, now, dt)
|
||||
self.levels_pub.publish(Float32MultiArray(data=[float(mic_db), float(spk_db)]))
|
||||
if state != self._last_state or now - self._last_pub > 0.5:
|
||||
self.activity_pub.publish(String(data=state))
|
||||
self._last_state, self._last_pub = state, now
|
||||
for name, tap, db in (('mic', self.mic, mic_db), ('speaker', self.spk, spk_db)):
|
||||
if db > self.scope_floor_db:
|
||||
w = trigger_window(tap.latest(self.n_scope + 512), self.n_scope, 512)
|
||||
self.scope_pub[name].publish(Float32MultiArray(data=w.tolist()))
|
||||
|
||||
def destroy_node(self):
|
||||
self.mic.close()
|
||||
self.spk.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = AudioStateNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,58 @@
|
||||
"""Tap a PipeWire/PulseAudio source with `parec`, keeping the newest samples. Used only for levels
|
||||
and the face's waveform: audio itself never goes through ROS."""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class PulseTap:
|
||||
def __init__(self, device: str, rate: int = 16000, history_s: float = 2.0):
|
||||
self.device, self.rate = device, rate
|
||||
self._buf = np.zeros(int(rate * history_s), np.float32)
|
||||
self._lock = threading.Lock()
|
||||
self._stop = threading.Event()
|
||||
self._proc: subprocess.Popen | None = None
|
||||
self.last_data = 0.0
|
||||
self._thread = threading.Thread(target=self._run, name=f'tap-{device}', daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _run(self) -> None:
|
||||
env = dict(os.environ)
|
||||
env.setdefault('XDG_RUNTIME_DIR', f'/run/user/{os.getuid()}') # services do not get one
|
||||
cmd = ['parec', '-d', self.device, '--format=s16le', f'--rate={self.rate}', '--channels=1',
|
||||
'--latency-msec=20', '--raw']
|
||||
while not self._stop.is_set():
|
||||
try:
|
||||
self._proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, env=env)
|
||||
while not self._stop.is_set():
|
||||
raw = self._proc.stdout.read(self.rate // 25 * 2) # ~40 ms
|
||||
if not raw:
|
||||
break
|
||||
x = np.frombuffer(raw[: len(raw) // 2 * 2], np.int16).astype(np.float32) / 32768.0
|
||||
with self._lock:
|
||||
self._buf = np.concatenate([self._buf[len(x):], x])
|
||||
self.last_data = time.monotonic()
|
||||
except OSError:
|
||||
pass
|
||||
finally:
|
||||
if self._proc and self._proc.poll() is None:
|
||||
self._proc.kill()
|
||||
self._stop.wait(2.0) # PipeWire not up yet: retry
|
||||
|
||||
def latest(self, n: int) -> np.ndarray:
|
||||
with self._lock:
|
||||
return self._buf[-n:].copy()
|
||||
|
||||
def alive(self, within_s: float = 1.0) -> bool:
|
||||
return time.monotonic() - self.last_data < within_s
|
||||
|
||||
def close(self) -> None:
|
||||
self._stop.set()
|
||||
if self._proc and self._proc.poll() is None:
|
||||
self._proc.kill()
|
||||
self._thread.join(timeout=2)
|
||||
@@ -0,0 +1,100 @@
|
||||
"""Audio level maths and the speaking/listening decision. No ROS or audio-device imports."""
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
FLOOR_DB = -100.0
|
||||
|
||||
|
||||
def rms_dbfs(x: np.ndarray) -> float:
|
||||
"""RMS level of float samples in [-1, 1], in dB relative to full scale."""
|
||||
if len(x) == 0:
|
||||
return FLOOR_DB
|
||||
r = float(np.sqrt(np.mean(np.square(x, dtype=np.float64))))
|
||||
return max(FLOOR_DB, 20.0 * np.log10(max(r, 1e-9)))
|
||||
|
||||
|
||||
def trigger_window(buf: np.ndarray, n: int = 256, lookback: int = 512) -> np.ndarray:
|
||||
"""n samples starting at a rising zero crossing near the newest end of buf, so a periodic
|
||||
sound draws standing still, like a triggered oscilloscope. Falls back to the newest n samples
|
||||
when there is no crossing (silence, or a DC offset). Zero-pads a short buffer on the left."""
|
||||
need = n + lookback
|
||||
if len(buf) < need:
|
||||
buf = np.concatenate([np.zeros(need - len(buf), buf.dtype), buf])
|
||||
seg = buf[-need:]
|
||||
head = seg[:lookback + 1]
|
||||
crossings = np.nonzero((head[:-1] < 0) & (head[1:] >= 0))[0]
|
||||
start = int(crossings[-1]) + 1 if len(crossings) else lookback
|
||||
return seg[start:start + n].astype(np.float32)
|
||||
|
||||
|
||||
class Activity:
|
||||
"""Decides 'speaking', 'listening' or 'quiet' from the speaker monitor and microphone levels.
|
||||
|
||||
speaking: the speaker monitor is above spk_db for on_s (so a click is not speech); it lets go
|
||||
after hold_s of quiet.
|
||||
listening: the microphone is above the adaptive threshold while not speaking, for on_s; it lets
|
||||
go after hold_s. The threshold is margin_db above a noise floor that drops quickly
|
||||
to new lows and rises slowly (so a long conversation does not become the floor).
|
||||
For its first calibrate_s the floor follows the microphone quickly in both directions (a room
|
||||
that is already noisy must not read as "listening"), and anything that stays 'listening' for
|
||||
ambient_s without a gap is treated as ambient noise and absorbed into the floor: people pause.
|
||||
The microphone is ignored while speaking: it would hear the robot's own voice.
|
||||
"""
|
||||
|
||||
def __init__(self, spk_db: float = -55.0, margin_db: float = 9.0, min_mic_db: float = -62.0,
|
||||
on_s: float = 0.10, speak_on_s: float = 0.08, hold_s: float = 0.40, speak_hold_s: float = 0.25,
|
||||
calibrate_s: float = 3.0, ambient_s: float = 8.0):
|
||||
self.spk_db, self.margin_db, self.min_mic_db = spk_db, margin_db, min_mic_db
|
||||
self.on_s, self.speak_on_s, self.hold_s, self.speak_hold_s = on_s, speak_on_s, hold_s, speak_hold_s
|
||||
self.calibrate_s, self.ambient_s = calibrate_s, ambient_s
|
||||
self._t0: Optional[float] = None
|
||||
self._listening_since: Optional[float] = None
|
||||
self.floor_db = min_mic_db
|
||||
self.state = 'quiet'
|
||||
self._since = {'speaking': None, 'listening': None}
|
||||
self._last = {'speaking': -1e9, 'listening': -1e9}
|
||||
|
||||
@property
|
||||
def mic_threshold_db(self) -> float:
|
||||
return max(self.floor_db + self.margin_db, self.min_mic_db)
|
||||
|
||||
def _track_floor(self, mic_db: float, dt: float, now: float) -> None:
|
||||
if self._t0 is not None and now - self._t0 < self.calibrate_s:
|
||||
self.floor_db += (mic_db - self.floor_db) * min(1.0, 2.0 * dt)
|
||||
elif self._listening_since is not None and now - self._listening_since > self.ambient_s:
|
||||
self.floor_db += (mic_db - 0.5 * self.margin_db - self.floor_db) * min(1.0, 0.5 * dt)
|
||||
elif mic_db < self.floor_db:
|
||||
self.floor_db += (mic_db - self.floor_db) * min(1.0, 8.0 * dt)
|
||||
else:
|
||||
self.floor_db += (mic_db - self.floor_db) * min(1.0, 0.04 * dt)
|
||||
|
||||
def update(self, mic_db: float, spk_db: float, now: float, dt: float = 0.05) -> str:
|
||||
if self._t0 is None:
|
||||
self._t0 = now
|
||||
if self.state != 'speaking':
|
||||
self._track_floor(mic_db, dt, now)
|
||||
for key, active, on_s, hold_s in (
|
||||
('speaking', spk_db > self.spk_db, self.speak_on_s, self.speak_hold_s),
|
||||
('listening', mic_db > self.mic_threshold_db and self.state != 'speaking', self.on_s, self.hold_s)):
|
||||
if active:
|
||||
if self._since[key] is None:
|
||||
self._since[key] = now
|
||||
if now - self._since[key] >= on_s:
|
||||
self._last[key] = now
|
||||
else:
|
||||
self._since[key] = None
|
||||
if now - self._last['speaking'] <= self.speak_hold_s:
|
||||
self.state = 'speaking'
|
||||
elif now - self._last['listening'] <= self.hold_s:
|
||||
self.state = 'listening'
|
||||
else:
|
||||
self.state = 'quiet'
|
||||
if self.state == 'listening':
|
||||
if self._listening_since is None:
|
||||
self._listening_since = now
|
||||
else:
|
||||
self._listening_since = None
|
||||
return self.state
|
||||
@@ -0,0 +1,19 @@
|
||||
<?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_audio</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Microphone and speaker levels, and speaking/listening state, for the Hermes robot.</description>
|
||||
<maintainer email="noreply@example.com">K. Isom</maintainer>
|
||||
<license>UNLICENSED</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<exec_depend>python3-numpy</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_audio
|
||||
[install]
|
||||
install_scripts=$base/lib/hermes_audio
|
||||
@@ -0,0 +1,24 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'hermes_audio'
|
||||
|
||||
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='Audio levels and speaking/listening state for the Hermes robot.',
|
||||
license='UNLICENSED',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'audio_state = hermes_audio.audio_state_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,87 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from hermes_audio.levels import FLOOR_DB, Activity, rms_dbfs, trigger_window
|
||||
|
||||
|
||||
def sine(f, n, rate=16000, amp=0.5, phase=0.0):
|
||||
return (amp * np.sin(2 * np.pi * f * np.arange(n) / rate + phase)).astype(np.float32)
|
||||
|
||||
|
||||
def test_rms_dbfs_known_values():
|
||||
assert rms_dbfs(np.ones(100, np.float32)) == pytest.approx(0.0, abs=0.01)
|
||||
assert rms_dbfs(sine(440, 16000, amp=1.0)) == pytest.approx(-3.01, abs=0.05)
|
||||
assert rms_dbfs(np.zeros(100, np.float32)) == FLOOR_DB or rms_dbfs(np.zeros(100, np.float32)) < -100 + 1
|
||||
assert rms_dbfs(np.array([], np.float32)) == FLOOR_DB
|
||||
|
||||
|
||||
def test_trigger_window_starts_on_a_rising_zero_crossing_and_is_phase_locked():
|
||||
a = trigger_window(sine(200, 2000, phase=0.3), 256, 512)
|
||||
b = trigger_window(sine(200, 2000, phase=2.1), 256, 512) # a different arrival phase
|
||||
assert len(a) == 256 and a[0] >= 0 and abs(a[0]) < 0.2 # begins near a rising zero crossing
|
||||
assert np.allclose(a, b, atol=0.12) # so both draw the same picture
|
||||
|
||||
|
||||
def test_trigger_window_handles_silence_and_short_buffers():
|
||||
assert trigger_window(np.zeros(2000, np.float32)).shape == (256,)
|
||||
short = trigger_window(sine(200, 100), 256, 512)
|
||||
assert short.shape == (256,)
|
||||
|
||||
|
||||
def run_activity(act, mic_db, spk_db, seconds, t0=0.0, dt=0.05):
|
||||
t = t0
|
||||
for _ in range(int(seconds / dt)):
|
||||
t += dt
|
||||
act.update(mic_db, spk_db, t, dt)
|
||||
return t
|
||||
|
||||
|
||||
def test_listens_to_a_voice_and_lets_go_after_the_hold():
|
||||
a = Activity()
|
||||
t = run_activity(a, -62, -100, 3) # a quiet room sets the floor
|
||||
assert a.state == 'quiet'
|
||||
t = run_activity(a, -35, -100, 0.5, t)
|
||||
assert a.state == 'listening'
|
||||
run_activity(a, -62, -100, 0.2, t)
|
||||
assert a.state == 'listening' # still holding
|
||||
run_activity(a, -62, -100, 1.0, t + 0.2)
|
||||
assert a.state == 'quiet'
|
||||
|
||||
|
||||
def test_a_click_is_not_speech():
|
||||
a = Activity()
|
||||
t = run_activity(a, -62, -100, 2)
|
||||
t = run_activity(a, -30, -100, 0.05, t) # one 50 ms burst, shorter than on_s
|
||||
run_activity(a, -62, -100, 0.1, t)
|
||||
assert a.state == 'quiet'
|
||||
|
||||
|
||||
def test_speaking_wins_and_ignores_the_mic():
|
||||
a = Activity()
|
||||
t = run_activity(a, -62, -100, 2)
|
||||
t = run_activity(a, -30, -40, 0.5, t) # loud mic AND loud speaker monitor
|
||||
assert a.state == 'speaking'
|
||||
run_activity(a, -30, -100, 0.1, t)
|
||||
assert a.state == 'speaking' # held
|
||||
t2 = run_activity(a, -30, -100, 1.0, t + 0.1)
|
||||
assert a.state == 'listening' # speaker stopped; the mic is real speech now
|
||||
assert t2 > t
|
||||
|
||||
|
||||
def test_a_room_that_is_already_noisy_calibrates_instead_of_listening_forever():
|
||||
a = Activity()
|
||||
run_activity(a, -43, -100, 5) # a steady -43 dBFS noise floor from the start
|
||||
assert a.state == 'quiet' and a.mic_threshold_db > -40
|
||||
t = run_activity(a, -25, -100, 0.5, 5.0) # a voice 18 dB above it
|
||||
assert a.state == 'listening'
|
||||
run_activity(a, -43, -100, 1.0, t)
|
||||
assert a.state == 'quiet'
|
||||
|
||||
|
||||
def test_a_new_steady_noise_is_absorbed_after_a_while():
|
||||
a = Activity()
|
||||
t = run_activity(a, -60, -100, 5) # quiet room
|
||||
t = run_activity(a, -35, -100, 1.0, t)
|
||||
assert a.state == 'listening' # something started
|
||||
run_activity(a, -35, -100, 20, t) # ...and never stops: a hum, not speech
|
||||
assert a.state == 'quiet'
|
||||
@@ -0,0 +1,22 @@
|
||||
"""Which state the face shows, from what the agent says and what the microphone/speaker hear.
|
||||
|
||||
Priority, highest first: sleeping > speaking > listening > thinking > idle. `sleeping` means "no
|
||||
agent connected", so only the agent side can set or clear it: room sound never wakes the face, or it
|
||||
would claim the agent hears you. `looking` is a timed event the renderer owns, not decided here.
|
||||
No ROS imports.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
def resolve_state(agent: str, audio: str) -> str:
|
||||
"""agent: the last /hermes/state (sleeping idle listening thinking speaking);
|
||||
audio: /audio/activity (speaking listening quiet)."""
|
||||
if agent == 'sleeping':
|
||||
return 'sleeping'
|
||||
if audio == 'speaking' or agent == 'speaking':
|
||||
return 'speaking'
|
||||
if audio == 'listening' or agent == 'listening':
|
||||
return 'listening'
|
||||
if agent == 'thinking':
|
||||
return 'thinking'
|
||||
return 'idle'
|
||||
@@ -5,6 +5,10 @@ Subscribes: /hermes/state (std_msgs/String): sleeping idle listening thinking l
|
||||
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
|
||||
/audio/activity (std_msgs/String): speaking | listening | quiet, from hermes_audio
|
||||
/audio/scope/{mic,speaker} (std_msgs/Float32MultiArray): the real waveform for the mouth
|
||||
Which state shows is decided by hermes_face.arbiter: sleeping (agent only) > speaking > listening >
|
||||
thinking > idle; room sound never wakes a sleeping face.
|
||||
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.
|
||||
"""
|
||||
@@ -14,9 +18,10 @@ import time
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import CameraInfo, JointState
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float32MultiArray, String
|
||||
from vision_msgs.msg import Detection2DArray
|
||||
|
||||
from .arbiter import resolve_state
|
||||
from .gaze import Target, gaze_from_pixel, pick_target
|
||||
from .render import STATES, FaceRenderer
|
||||
|
||||
@@ -30,6 +35,9 @@ 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.agent_state = initial if initial in STATES else 'sleeping'
|
||||
self.audio_activity = 'quiet'
|
||||
self.audio_drive = bool(param('audio_drive', True))
|
||||
self.follow = bool(param('gaze_follow', True))
|
||||
self.image_size = (float(param('camera_width', 1296)), float(param('camera_height', 972)))
|
||||
|
||||
@@ -49,6 +57,11 @@ 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.audio_drive:
|
||||
self.create_subscription(String, '/audio/activity', self._on_activity, 10)
|
||||
for kind in ('mic', 'speaker'):
|
||||
self.create_subscription(Float32MultiArray, f'/audio/scope/{kind}',
|
||||
lambda m, k=kind: self.renderer.set_scope(k, m.data, time.monotonic()), 2)
|
||||
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)
|
||||
@@ -61,7 +74,20 @@ class FaceNode(Node):
|
||||
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())
|
||||
now = time.monotonic()
|
||||
if msg.data == 'looking': # a timed event: the renderer squints, flashes, then goes back
|
||||
self.renderer.set_state('looking', now)
|
||||
return
|
||||
self.agent_state = msg.data
|
||||
self._resolve(now)
|
||||
|
||||
def _on_activity(self, msg: String) -> None:
|
||||
self.audio_activity = msg.data
|
||||
self._resolve(time.monotonic())
|
||||
|
||||
def _resolve(self, now: float) -> None:
|
||||
if self.renderer.state != 'looking':
|
||||
self.renderer.set_state(resolve_state(self.agent_state, self.audio_activity if self.audio_drive else 'quiet'), now)
|
||||
|
||||
def _on_joints(self, msg: JointState) -> None:
|
||||
pos = dict(zip(msg.name, msg.position))
|
||||
@@ -84,6 +110,7 @@ class FaceNode(Node):
|
||||
|
||||
def _tick(self) -> None:
|
||||
now = time.monotonic()
|
||||
self._resolve(now) # also settles the state a finished `looking` returned to
|
||||
rgb = self.renderer.frame(now, now - self._last, 'rgb565')
|
||||
self._last = now
|
||||
if self.display:
|
||||
|
||||
@@ -30,6 +30,7 @@ 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
|
||||
SCOPE_FRESH_S = 0.35 # a waveform window older than this is ignored (the synthetic voice returns)
|
||||
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)
|
||||
|
||||
@@ -152,6 +153,8 @@ class FaceRenderer:
|
||||
self._flash_pending = state == 'looking'
|
||||
self.pan = 0.0 # degrees, from the servos: positive pan = robot's left, tilt = looks up
|
||||
self.tilt = 0.0
|
||||
self._scope = {'mic': (None, -1e9), 'speaker': (None, -1e9)} # newest real waveform per source
|
||||
self._scope_peak = {'mic': 0.05, 'speaker': 0.05}
|
||||
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,
|
||||
@@ -192,6 +195,25 @@ class FaceRenderer:
|
||||
def set_pose(self, pan_deg: float, tilt_deg: float) -> None:
|
||||
self.pan, self.tilt = pan_deg, tilt_deg
|
||||
|
||||
def set_scope(self, kind: str, samples, now: float) -> None:
|
||||
"""The real waveform for the mouth: a trigger-locked window of samples in [-1, 1] from the
|
||||
'mic' or the 'speaker'. While it is fresh and the face is listening/speaking, the mouth draws
|
||||
it instead of the synthetic voice, auto-scaled so a quiet voice is still visible."""
|
||||
if kind not in self._scope:
|
||||
raise ValueError(f'unknown scope {kind!r}')
|
||||
x = np.asarray(samples, np.float32)
|
||||
if len(x) < 8:
|
||||
return
|
||||
peak = float(np.abs(x).max())
|
||||
self._scope_peak[kind] = max(peak, self._scope_peak[kind] * 0.97, 0.02) # fast attack, slow release
|
||||
self._scope[kind] = (x, now)
|
||||
|
||||
def _real_wave(self, kind: str, T: float):
|
||||
x, t = self._scope[kind]
|
||||
if x is None or T - t > SCOPE_FRESH_S:
|
||||
return None
|
||||
return x / self._scope_peak[kind]
|
||||
|
||||
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."""
|
||||
@@ -297,16 +319,26 @@ class FaceRenderer:
|
||||
mouth_w = (mx1 - mx0) * (1 - 0.45 * k) * (1 - 0.5 * sleep)
|
||||
m_start = (mx0 + mx1) / 2 - mouth_w / 2
|
||||
|
||||
real_s = self._real_wave('speaker', T) if self.state == 'speaking' else None
|
||||
real_l = self._real_wave('mic', T) if self.state == 'listening' else None
|
||||
|
||||
def real(wave, u):
|
||||
return np.interp(u * (len(wave) - 1), np.arange(len(wave)), wave)
|
||||
|
||||
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:
|
||||
if real_s is not None: # the actual outgoing audio
|
||||
y = y - 26 * a['mouth_speak'] * real(real_s, u)
|
||||
elif 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:
|
||||
if real_l is not None: # the actual incoming audio, dimmer and smaller
|
||||
y = y - 12 * a['mouth_listen'] * real(real_l, u)
|
||||
elif 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']))
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
import pytest
|
||||
|
||||
from hermes_face.arbiter import resolve_state
|
||||
|
||||
|
||||
@pytest.mark.parametrize('agent,audio,expected', [
|
||||
('sleeping', 'quiet', 'sleeping'),
|
||||
('sleeping', 'listening', 'sleeping'), # room sound never wakes the face
|
||||
('sleeping', 'speaking', 'sleeping'),
|
||||
('idle', 'quiet', 'idle'),
|
||||
('idle', 'listening', 'listening'),
|
||||
('idle', 'speaking', 'speaking'),
|
||||
('thinking', 'quiet', 'thinking'),
|
||||
('thinking', 'listening', 'listening'), # a voice interrupts the thinking face
|
||||
('listening', 'quiet', 'listening'),
|
||||
('speaking', 'quiet', 'speaking'), # the agent says it is speaking before the audio shows it
|
||||
('thinking', 'speaking', 'speaking'),
|
||||
])
|
||||
def test_priority(agent, audio, expected):
|
||||
assert resolve_state(agent, audio) == expected
|
||||
@@ -144,3 +144,41 @@ def test_gaze_is_clamped():
|
||||
r = FaceRenderer(seed=0)
|
||||
r.set_gaze(50, -50, 0.0)
|
||||
assert r._gaze == (1.0, -1.0)
|
||||
|
||||
|
||||
def mouth_rows(frame):
|
||||
"""Row extent of lit pixels in the mouth band."""
|
||||
band = frame[140:215].astype(np.float32).sum(axis=(1, 2))
|
||||
rows = np.nonzero(band > 0.15 * band.max())[0]
|
||||
return int(rows.max() - rows.min()) if len(rows) else 0
|
||||
|
||||
|
||||
def run_scope(kind, state, amp):
|
||||
r = FaceRenderer(state=state, seed=0, jitter=0)
|
||||
t, frame = 0.0, None
|
||||
for _ in range(60):
|
||||
t += 1 / 30
|
||||
r.set_scope(kind, amp * np.sin(np.linspace(0, 8 * np.pi, 256)), t) # refreshed every frame
|
||||
frame = r.frame(t, 1 / 30)
|
||||
return frame
|
||||
|
||||
|
||||
def test_the_mouth_draws_the_real_waveform_and_autoscales_it():
|
||||
big, small = run_scope('speaker', 'speaking', 0.8), run_scope('speaker', 'speaking', 0.02)
|
||||
assert mouth_rows(big) > 25 and mouth_rows(small) > 25 # a quiet voice is still visible
|
||||
assert abs(mouth_rows(big) - mouth_rows(small)) < 8
|
||||
|
||||
|
||||
def test_the_mic_waveform_is_drawn_only_while_listening_and_stale_data_is_ignored():
|
||||
listening = mouth_rows(run_scope('mic', 'listening', 0.5))
|
||||
idle = mouth_rows(run_scope('mic', 'idle', 0.5))
|
||||
assert listening > idle + 6
|
||||
r = FaceRenderer(state='speaking', seed=0, jitter=0)
|
||||
r.set_scope('speaker', np.sin(np.linspace(0, 6, 128)), 0.0)
|
||||
frame, _ = run(r, 2) # the scope goes stale: synthetic voice, no crash
|
||||
assert frame.max() > 100
|
||||
|
||||
|
||||
def test_unknown_scope_kind_rejected():
|
||||
with pytest.raises(ValueError):
|
||||
FaceRenderer().set_scope('radio', [0.0] * 16, 0.0)
|
||||
|
||||
Reference in New Issue
Block a user