Add hermes_vision, build camera_ros on the fork, and bring up WM8960 audio

hermes_vision wraps OpenCV YuNet (Haar fallback) as a plain module plus a node
that publishes /vision/faces as vision_msgs/Detection2DArray. It replaces the
standalone detect_faces.py. pi/robot-env.sh puts the Raspberry Pi libcamera fork
ahead of ROS's own copy, the venv's OpenCV 4.10 ahead of the apt one, and fixes
the ROS domain and subnet-only discovery. camera_ros 0.7.0 now runs against the
fork and publishes raw and compressed images.

The stock wm8960-soundcard overlay fails on Ubuntu 24.04 with "No MCLK
configured": it hangs the clock on the sound card's codec link, but the codec
driver reads mclk from its own node. hermes-wm8960.dts adds it there, and
capture opens at 8-48 kHz. The microphones still read digital zeros; see
CONTEXT.md for what has been ruled out.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs

ros/hermes_vision/hermes_vision/detect.py

@@ -0,0 +1,95 @@
+"""Face detection on BGR images: OpenCV YuNet when its model loads, else the Haar cascades.
+
+No ROS imports. YuNet 2023mar needs OpenCV 4.8+ (the apt python3-opencv is 4.6, so the Pi runs
+this from ~/venv, which carries opencv-python-headless 4.10) and input sides that are multiples
+of 32, so frames are resized to about 640 wide and the boxes scaled back.
+"""
+from __future__ import annotations
+
+import os
+from dataclasses import dataclass
+
+import cv2
+
+HAAR_DIR = '/usr/share/opencv4/haarcascades'
+DEFAULT_MODEL = os.path.expanduser('~/models/face_detection_yunet_2023mar.onnx')
+
+
+@dataclass(frozen=True)
+class Face:
+    x: int
+    y: int
+    w: int
+    h: int
+    score: float = 1.0
+
+
+def yunet_input_size(w: int, h: int, max_side: int = 640) -> tuple[int, int]:
+    """The size to feed YuNet: scaled so the long side is at most max_side, sides rounded to 32."""
+    scale = min(1.0, max_side / max(w, h))
+    return max(32, round(w * scale / 32) * 32), max(32, round(h * scale / 32) * 32)
+
+
+def merge(faces: list[Face], overlap: float = 0.4) -> list[Face]:
+    """Drop boxes that mostly overlap an earlier one (frontal and profile cascades can both fire)."""
+    kept: list[Face] = []
+    for f in faces:
+        for k in kept:
+            iw = min(f.x + f.w, k.x + k.w) - max(f.x, k.x)
+            ih = min(f.y + f.h, k.y + k.h) - max(f.y, k.y)
+            if iw > 0 and ih > 0 and iw * ih > overlap * min(f.w * f.h, k.w * k.h):
+                break
+        else:
+            kept.append(f)
+    return kept
+
+
+class FaceDetector:
+    def __init__(self, model_path: str | None = None, score_threshold: float = 0.6,
+                 min_size: int = 40, max_side: int = 640):
+        self.score_threshold = score_threshold
+        self.min_size = min_size
+        self.max_side = max_side
+        self._yunet: dict[tuple[int, int], cv2.FaceDetectorYN] = {}
+        self._model = os.path.expanduser(model_path) if model_path else DEFAULT_MODEL
+        self.backend = 'yunet' if self._load_yunet(640, 480) is not None else 'haar'
+        if self.backend == 'haar':
+            self._frontal = cv2.CascadeClassifier(os.path.join(HAAR_DIR, 'haarcascade_frontalface_alt2.xml'))
+            self._profile = cv2.CascadeClassifier(os.path.join(HAAR_DIR, 'haarcascade_profileface.xml'))
+
+    def _load_yunet(self, w: int, h: int):
+        if (w, h) in self._yunet:
+            return self._yunet[(w, h)]
+        if not os.path.exists(self._model) or not hasattr(cv2, 'FaceDetectorYN'):
+            return None
+        try:   # OpenCV 4.6 cannot change the input size after create(), so keep one per size
+            det = cv2.FaceDetectorYN.create(self._model, '', (w, h), self.score_threshold, 0.3, 5000)
+            det.detect(__import__('numpy').zeros((h, w, 3), 'uint8'))   # old OpenCV parses but cannot run it
+        except cv2.error:
+            return None
+        self._yunet[(w, h)] = det
+        return det
+
+    def detect(self, bgr) -> list[Face]:
+        return self._detect_yunet(bgr) if self.backend == 'yunet' else self._detect_haar(bgr)
+
+    def _detect_yunet(self, bgr) -> list[Face]:
+        h0, w0 = bgr.shape[:2]
+        tw, th = yunet_input_size(w0, h0, self.max_side)
+        small = cv2.resize(bgr, (tw, th), interpolation=cv2.INTER_AREA)
+        _, found = self._load_yunet(tw, th).detect(small)
+        if found is None:
+            return []
+        fx, fy = w0 / tw, h0 / th
+        return [Face(int(f[0] * fx), int(f[1] * fy), int(f[2] * fx), int(f[3] * fy), float(f[14]))
+                for f in found if f[2] * fx >= self.min_size]
+
+    def _detect_haar(self, bgr) -> list[Face]:
+        gray = cv2.equalizeHist(cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY))
+        ms = (self.min_size, self.min_size)
+        boxes = [Face(*map(int, b)) for b in self._frontal.detectMultiScale(gray, 1.1, 5, minSize=ms)]
+        for flip in (False, True):   # the profile cascade only sees one side
+            g = cv2.flip(gray, 1) if flip else gray
+            for x, y, w, h in self._profile.detectMultiScale(g, 1.1, 5, minSize=ms):
+                boxes.append(Face(int(gray.shape[1] - x - w if flip else x), int(y), int(w), int(h)))
+        return merge(boxes)

ros/hermes_vision/hermes_vision/detect_cli.py

@@ -0,0 +1,39 @@
+"""Detect faces in image files and write annotated copies.
+
+  python -m hermes_vision.detect_cli IMAGE [IMAGE ...] [--out DIR]
+"""
+import argparse
+import os
+
+import cv2
+
+from .detect import FaceDetector
+
+
+def main():
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument('images', nargs='+')
+    p.add_argument('--out', default='.')
+    p.add_argument('--model')
+    p.add_argument('--min-size', type=int, default=40)
+    args = p.parse_args()
+    os.makedirs(args.out, exist_ok=True)
+    det = FaceDetector(args.model, min_size=args.min_size)
+    for path in args.images:
+        img = cv2.imread(path)
+        if img is None:
+            print(f'{path}: unreadable')
+            continue
+        faces = det.detect(img)
+        for f in faces:
+            cv2.rectangle(img, (f.x, f.y), (f.x + f.w, f.y + f.h), (0, 255, 0), 2)
+            cv2.putText(img, f'{f.score:.2f}', (f.x, max(12, f.y - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
+                        (0, 255, 0), 2)
+        dest = os.path.join(args.out, 'faces-' + os.path.basename(path).rsplit('.', 1)[0] + '.jpg')
+        cv2.imwrite(dest, img)
+        print(f'{path}: {len(faces)} face(s) [{det.backend}] '
+              f'{[(f.x, f.y, f.w, f.h, round(f.score, 2)) for f in faces]} -> {dest}')
+
+
+if __name__ == '__main__':
+    main()

ros/hermes_vision/hermes_vision/face_detector_node.py

@@ -0,0 +1,80 @@
+"""ROS node: faces in the camera image.
+
+Subscribes:  image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
+Publishes:   /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in pixels
+             (center and size), class_id 'face', score = detector confidence; header from the image.
+             /vision/faces/debug/compressed (CompressedImage), only if publish_debug is true.
+Work is rate-limited to rate_hz, so a busy Pi 4 skips frames rather than falling behind.
+"""
+import time
+
+import cv2
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import CompressedImage
+from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
+
+from .detect import FaceDetector
+
+
+class FaceDetectorNode(Node):
+    def __init__(self):
+        super().__init__('hermes_face_detector')
+        param = lambda name, default: self.declare_parameter(name, default).value  # noqa: E731
+        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))
+        self.detector = FaceDetector(param('model_path', ''), float(param('score_threshold', 0.6)),
+                                     int(param('min_size', 40)))
+        self.pub = self.create_publisher(Detection2DArray, '/vision/faces', 10)
+        self.debug_pub = (self.create_publisher(CompressedImage, '/vision/faces/debug/compressed', 1)
+                          if self.debug else None)
+        self.create_subscription(CompressedImage, topic, self._on_image, 1)
+        self._next = 0.0
+        self.get_logger().info(f'face detector up: {self.detector.backend}, {1 / self.period:g} Hz, {topic}')
+
+    def _on_image(self, msg: CompressedImage) -> None:
+        now = time.monotonic()
+        if now < self._next:
+            return
+        self._next = now + self.period
+        img = cv2.imdecode(np.frombuffer(msg.data, np.uint8), cv2.IMREAD_COLOR)
+        if img is None:
+            self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
+            return
+        faces = self.detector.detect(img)
+        out = Detection2DArray(header=msg.header)
+        for f in faces:
+            d = Detection2D(header=msg.header)
+            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)
+            hyp = ObjectHypothesisWithPose()
+            hyp.hypothesis.class_id, hyp.hypothesis.score = 'face', f.score
+            d.results.append(hyp)
+            out.detections.append(d)
+        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)
+            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()))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FaceDetectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros/hermes_vision/package.xml

@@ -0,0 +1,20 @@
+<?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_vision</name>
+  <version>0.1.0</version>
+  <description>Face detection on the Hermes robot's camera image.</description>
+  <maintainer email="noreply@example.com">K. Isom</maintainer>
+  <license>UNLICENSED</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>vision_msgs</depend>
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros/hermes_vision/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/hermes_vision
+[install]
+install_scripts=$base/lib/hermes_vision

ros/hermes_vision/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'hermes_vision'
+
+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='Face detection for the Hermes robot.',
+    license='UNLICENSED',
+    entry_points={
+        'console_scripts': [
+            'face_detector = hermes_vision.face_detector_node:main',
+            'detect_faces = hermes_vision.detect_cli:main',
+        ],
+    },
+)

ros/hermes_vision/test/test_detect.py

@@ -0,0 +1,27 @@
+import numpy as np
+import pytest
+
+from hermes_vision.detect import Face, FaceDetector, merge, yunet_input_size
+
+
+@pytest.mark.parametrize('size,expected', [
+    ((1296, 972), (640, 480)),
+    ((640, 480), (640, 480)),
+    ((320, 240), (320, 256)),     # small frames are not upscaled; 240/32 = 7.5 rounds to 8
+    ((1920, 1080), (640, 352)),
+    ((10, 10), (32, 32)),
+])
+def test_yunet_input_size_is_a_multiple_of_32(size, expected):
+    got = yunet_input_size(*size)
+    assert got == expected and got[0] % 32 == 0 and got[1] % 32 == 0
+
+
+def test_merge_drops_overlapping_boxes_and_keeps_distinct_ones():
+    a, b, c = Face(100, 100, 80, 80), Face(105, 98, 78, 82), Face(400, 100, 80, 80)
+    assert merge([a, b, c]) == [a, c]
+
+
+def test_blank_image_has_no_faces():
+    det = FaceDetector(model_path='/nonexistent.onnx')      # forces the Haar fallback
+    assert det.backend == 'haar'
+    assert det.detect(np.zeros((480, 640, 3), np.uint8)) == []