kyle/hermes-bot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Run the whole robot from one launch file and one boot-time unit

Browse Source 
hermes_bringup starts the face, camera, face detector and audio levels from one
launch file with their parameters in robot.yaml, each node its own respawned
process, the face first and the others after start_delay. robot.service runs it
at boot, so everything is ROS and the face is up whenever the robot is powered.
Reboot-tested: all services and nodes return by themselves, the face draws at
about 28 s on the boot clock, the mic gain survives, nothing throttles.

Cut the CPU: the camera runs at 15 fps (20% of a core, vs 40% at 30), and the
detector went from 131% to 40% by pinning OpenCV to one thread (its DNN burns 3-4x
its elapsed time across cores), decoding the JPEG at half size, and re-identifying
a tracked face at most once a second. The audio scope is published only while
someone is speaking or listening.

A standalone face unit that skipped `ros2 launch` saved only 3.7 s and was dropped:
the owner prefers everything in ROS and is fine with up to a minute. trim-boot.sh
disables the unused snapd and ModemManager.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs
This commit is contained in:
kyle
2026-10-05 00:10:09 -07:00
co-authored by Claude Sonnet 5.5
parent 5d1ad1eff7
commit d8db0a48f5
18 changed files with 332 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ros/hermes_audio/hermes_audio/audio_state_node.py
+4 -2
View File
@@ -47,8 +47,10 @@ class AudioStateNode(Node):
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:
# only while it is on the face: 30 idle messages a second would cost the face node CPU for nothing
for name, tap, db, wanted in (('mic', self.mic, mic_db, state == 'listening'),
('speaker', self.spk, spk_db, state == 'speaking')):
if wanted and 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()))
ros/hermes_bringup/config/robot.yaml
+32
View File
@@ -0,0 +1,32 @@
# Parameters for the robot's nodes, keyed by node name. The MCP token and other secrets do not
# belong here: they stay in an env file readable only by the service user.
camera:
ros__parameters:
width: 1296 # the OV5647's full-field binned mode: whole field of view
height: 972
format: RGB888
FrameDurationLimits: [66666, 66666] # 15 fps: ~20% of a core, vs ~40% at 30; plenty for 4 Hz faces
jpeg_quality: 80
hermes_face:
ros__parameters:
rotation: 270 # mounted pose, microSD end up (the panel is portrait)
x_offset: 0
y_offset: 80 # the 240x240 glass sits 80 rows into the controller's RAM
fps: 30.0
phosphor: P7
style: phosphor
initial_state: sleeping # no agent connected until hermes_mcp's heartbeat says so
agent_timeout_s: 10.0
gaze_follow: true
audio_drive: true
hermes_face_detector:
ros__parameters:
rate_hz: 4.0
recognize: true # only recognises people enrolled with hermes_vision.people
publish_debug: false
hermes_audio_state:
ros__parameters:
rate_hz: 30.0
ros/hermes_bringup/hermes_bringup/__init__.py
View File
ros/hermes_bringup/launch/robot.launch.py
+45
View File
@@ -0,0 +1,45 @@
"""Bring up the robot: camera, face, face detection, audio levels.
ros2 launch hermes_bringup robot.launch.py [camera:=false] [face:=false] [vision:=false] [audio:=false]
[start_delay:=6.0]
Every node is its own process and is respawned if it exits, so one crash cannot take the others
down: the face keeps running when the camera does not. Parameters are in config/robot.yaml.
The face starts at once; the camera, vision and audio nodes wait start_delay seconds, so the four
Python processes do not fight over the CPU while the face imports and draws its first frame.
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, TimerAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
# (launch argument, package, executable, node name in robot.yaml)
NODES = [
('camera', 'camera_ros', 'camera_node', 'camera'),
('face', 'hermes_face', 'face_node', 'hermes_face'),
('vision', 'hermes_vision', 'face_detector', 'hermes_face_detector'),
('audio', 'hermes_audio', 'audio_state', 'hermes_audio_state'),
]
DELAYED = {'camera', 'vision', 'audio'}
def generate_launch_description():
params = os.path.join(get_package_share_directory('hermes_bringup'), 'config', 'robot.yaml')
actions = [DeclareLaunchArgument(arg, default_value='true', description=f'run the {name} node')
for arg, _, _, name in NODES]
actions.append(DeclareLaunchArgument('start_delay', default_value='6.0',
description='seconds the camera, vision and audio wait for the face'))
for arg, package, executable, name in NODES:
node = Node(package=package, executable=executable, name=name, output='screen',
parameters=[params], respawn=True, respawn_delay=2.0,
condition=IfCondition(LaunchConfiguration(arg)))
actions.append(TimerAction(period=LaunchConfiguration('start_delay'), actions=[node])
if arg in DELAYED else node)
return LaunchDescription(actions)
ros/hermes_bringup/package.xml
+23
View File
@@ -0,0 +1,23 @@
<?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_bringup</name>
<version>0.1.0</version>
<description>Launch file and parameters that bring up the Hermes robot.</description>
<maintainer email="noreply@example.com">K. Isom</maintainer>
<license>UNLICENSED</license>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>camera_ros</exec_depend>
<exec_depend>hermes_face</exec_depend>
<exec_depend>hermes_vision</exec_depend>
<exec_depend>hermes_audio</exec_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
ros/hermes_bringup/resource/hermes_bringup
View File
ros/hermes_bringup/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/hermes_bringup
[install]
install_scripts=$base/lib/hermes_bringup
ros/hermes_bringup/setup.py
+24
View File
@@ -0,0 +1,24 @@
import os
from glob import glob
from setuptools import find_packages, setup
package_name = 'hermes_bringup'
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']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='K. Isom',
maintainer_email='noreply@example.com',
description='Launch file and parameters for the Hermes robot.',
license='UNLICENSED',
)
ros/hermes_bringup/test/test_launch.py
+34
View File
@@ -0,0 +1,34 @@
import os
import pytest
import yaml
pytest.importorskip('launch_ros')
HERE = os.path.dirname(__file__)
ROOT = os.path.dirname(HERE)
def test_launch_description_has_one_respawning_process_per_node():
import importlib.util
spec = importlib.util.spec_from_file_location('robot_launch', os.path.join(ROOT, 'launch', 'robot.launch.py'))
mod = importlib.util.module_from_spec(spec)
spec.loader.exec_module(mod)
from launch.actions import TimerAction
from launch_ros.actions import Node
ld = mod.generate_launch_description()
nodes = [e for e in ld.entities if isinstance(e, Node)]
delayed = [n for t in ld.entities if isinstance(t, TimerAction) for n in t.actions if isinstance(n, Node)]
assert len(nodes) + len(delayed) == len(mod.NODES) == 4
assert len(nodes) == 1 and 'face' in mod.NODES[1] and len(delayed) == 3 # the face starts at once
# respawn lives on a private attribute that differs between ROS releases, so check the source
src = open(os.path.join(ROOT, 'launch', 'robot.launch.py')).read()
assert 'respawn=True' in src and 'respawn_delay' in src
def test_every_node_in_the_launch_file_has_parameters_and_the_face_is_mounted_portrait():
cfg = yaml.safe_load(open(os.path.join(ROOT, 'config', 'robot.yaml')))
for name in ('camera', 'hermes_face', 'hermes_face_detector', 'hermes_audio_state'):
assert 'ros__parameters' in cfg[name], name
assert cfg['hermes_face']['ros__parameters']['rotation'] == 270
assert cfg['camera']['ros__parameters']['FrameDurationLimits'] == [66666, 66666]
ros/hermes_vision/hermes_vision/face_detector_node.py
+18 -9
View File
@@ -18,8 +18,8 @@ from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
from .detect import FaceDetector
from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer
from .detect import Face, FaceDetector
from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer, IdentityCache
class FaceDetectorNode(Node):
@@ -29,8 +29,14 @@ class FaceDetectorNode(Node):
topic = param('image_topic', '/camera/image_raw/compressed')
self.period = 1.0 / min(30.0, max(0.5, float(param('rate_hz', 4.0))))
self.debug = bool(param('publish_debug', False))
# OpenCV's DNN spins every core and burns 3-4x its elapsed time in CPU; one thread is the most
# efficient (measured on a Pi 4: YuNet 86 ms CPU on one thread, 406 ms on four).
cv2.setNumThreads(max(1, int(param('opencv_threads', 1))))
self.scale = 2 if bool(param('decode_half', True)) else 1 # JPEG decodes at half size ~2.5x faster
self._flag = cv2.IMREAD_REDUCED_COLOR_2 if self.scale == 2 else cv2.IMREAD_COLOR
self.detector = FaceDetector(param('model_path', ''), float(param('score_threshold', 0.6)),
int(param('min_size', 40)))
int(param('min_size', 40)) // self.scale, int(param('detect_max_side', 480)))
self.ids = IdentityCache(float(param('recognition_period_s', 1.0)))
recognition_model = os.path.expanduser(param('recognition_model', DEFAULT_MODEL)) # declare once
faces_db = param('faces_db', DEFAULT_DB)
threshold = float(param('recognition_threshold', COSINE_THRESHOLD))
@@ -52,16 +58,19 @@ class FaceDetectorNode(Node):
if now < self._next:
return
self._next = now + self.period
img = cv2.imdecode(np.frombuffer(msg.data, np.uint8), cv2.IMREAD_COLOR)
img = cv2.imdecode(np.frombuffer(msg.data, np.uint8), self._flag)
if img is None:
self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
return
faces = self.detector.detect(img)
faces = self.detector.detect(img) # in the (possibly half-size) decoded image
names = (self.ids.update(faces, now, lambda f: self.recognizer.identify(img, f)[0])
if self.recognizer else [''] * len(faces))
k = self.scale # published boxes are in full-size camera pixels
out = Detection2DArray(header=msg.header)
for f in faces:
for f, name in zip(faces, names):
f = Face(f.x * k, f.y * k, f.w * k, f.h * k, f.score)
d = Detection2D(header=msg.header)
if self.recognizer:
d.id = self.recognizer.identify(img, f)[0]
d.id = name
d.bbox.center.position.x = f.x + f.w / 2.0
d.bbox.center.position.y = f.y + f.h / 2.0
d.bbox.size_x, d.bbox.size_y = float(f.w), float(f.h)
@@ -72,7 +81,7 @@ class FaceDetectorNode(Node):
self.pub.publish(out)
if self.debug_pub:
for f in faces:
cv2.rectangle(img, (f.x, f.y), (f.x + f.w, f.y + f.h), (0, 255, 0), 2)
cv2.rectangle(img, (f.x, f.y), (f.x + f.w, f.y + f.h), (0, 255, 0), 2) # decoded-image pixels
ok, jpg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 80])
if ok:
self.debug_pub.publish(CompressedImage(header=msg.header, format='jpeg', data=jpg.tobytes()))
ros/hermes_vision/hermes_vision/recognize.py
+33
View File
@@ -98,3 +98,36 @@ class FaceRecognizer:
return '', 0.0
emb = self.embed(bgr, face)
return ('', 0.0) if emb is None else best_match(emb, self._db, self.threshold)
def iou(a: Face, b: Face) -> float:
iw = min(a.x + a.w, b.x + b.w) - max(a.x, b.x)
ih = min(a.y + a.h, b.y + b.h) - max(a.y, b.y)
if iw <= 0 or ih <= 0:
return 0.0
inter = iw * ih
return inter / (a.w * a.h + b.w * b.h - inter)
class IdentityCache:
"""Recognition is the expensive step, and a face that has barely moved is the same person: reuse
a tracked face's name for period_s before re-identifying it. Faces match by box overlap."""
def __init__(self, period_s: float = 1.0, iou_min: float = 0.4):
self.period_s, self.iou_min = period_s, iou_min
self._entries: list[tuple[Face, str, float]] = []
def update(self, faces, now: float, identify) -> list[str]:
"""Names for faces (same order). identify(face) -> name is called only for new or stale faces."""
names, nxt = [], []
for f in faces:
hit = max(((iou(f, pf), (pf, n, t)) for pf, n, t in self._entries), key=lambda x: x[0], default=None)
if hit and hit[0] >= self.iou_min and now - hit[1][2] < self.period_s:
_, name, t = hit[1]
nxt.append((f, name, t)) # keep the old stamp so it ages and gets rechecked
else:
name = identify(f)
nxt.append((f, name, now))
names.append(name)
self._entries = nxt
return names
ros/hermes_vision/test/test_identity_cache.py
+40
View File
@@ -0,0 +1,40 @@
from hermes_vision.detect import Face
from hermes_vision.recognize import IdentityCache, iou
def test_iou():
a = Face(0, 0, 100, 100)
assert iou(a, a) == 1.0
assert iou(a, Face(200, 200, 50, 50)) == 0.0
assert 0.3 < iou(a, Face(50, 0, 100, 100)) < 0.4
def test_a_tracked_face_is_identified_once_per_period():
calls = []
cache = IdentityCache(period_s=1.0)
ident = lambda f: (calls.append(f), 'kyle')[1]
assert cache.update([Face(100, 100, 80, 80)], 0.0, ident) == ['kyle']
for i in range(1, 8): # the face drifts a little, 4 times a second
assert cache.update([Face(100 + i, 100, 80, 80)], i * 0.125, ident) == ['kyle']
assert len(calls) == 1
cache.update([Face(108, 100, 80, 80)], 1.1, ident) # stale: rechecked
assert len(calls) == 2
def test_a_new_face_is_identified_immediately_and_unknown_is_cached_too():
calls = []
cache = IdentityCache()
ident = lambda f: (calls.append(f), '' if f.x > 400 else 'kyle')[1]
names = cache.update([Face(100, 100, 80, 80), Face(500, 100, 80, 80)], 0.0, ident)
assert names == ['kyle', ''] and len(calls) == 2
assert cache.update([Face(101, 100, 80, 80), Face(501, 100, 80, 80)], 0.3, ident) == ['kyle', '']
assert len(calls) == 2 # the stranger is not re-embedded every frame
def test_a_face_that_jumps_is_a_different_face():
calls = []
cache = IdentityCache()
ident = lambda f: (calls.append(f), 'kyle')[1]
cache.update([Face(100, 100, 80, 80)], 0.0, ident)
cache.update([Face(600, 300, 80, 80)], 0.1, ident)
assert len(calls) == 2