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_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