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
2026-10-04 22:55:53 -07:00
parent 5e48b6fa03
commit 8d5033d0a2
14 changed files with 440 additions and 107 deletions
@@ -48,12 +48,35 @@ printed enclosure, speakers. Work until those arrive is limited to what the Pi,
  `cam` then captures 1296x972 RGB888 at ~16 fps (dim room). `pi/capture_still.py` takes a still
  with it. **`camera_ros` still has to be built against this fork** (the apt one uses ROS's broken
  libcamera). Frames are padded (stride 3936) and B,G,R in memory.
- Face detection: `pi/detect_faces.py` (OpenCV YuNet, Haar fallback). YuNet 2023mar needs OpenCV
-  4.8+, so `~/venv` has `opencv-python-headless==4.10.0.84` with `numpy<2` (the system NumPy stays
-  1.26.4 for ROS); YuNet also needs input sides in multiples of 32. Verified on a sample image.
-  First camera shot was nearly black (mean 5/255): the lens was facing something close and dim.
- Not done: audio (`wm8960-soundcard` overlay + mixer; `alsa-utils` is installed, no card yet), PWM
-  (waits for servos). Ask before rebooting the Pi.
+- **`camera_ros` 0.7.0 is built from source against the fork** in `~/camera_ws` (overlay on ROS; at
+  runtime the fork must come first in `LD_LIBRARY_PATH`, which `source ~/robot-env.sh` does; copy
+  from `pi/robot-env.sh`). Verified: `ros2 run camera_ros camera_node --ros-args -p width:=1296 -p
+  height:=972 -p format:=RGB888` publishes `/camera/image_raw`, `/camera/image_raw/compressed` (~26
+  Hz) and `/camera/camera_info`. It costs ~140% of a core with raw and compressed both on; lower the
+  frame rate or size before running everything together. No calibration file yet.
+- **`ros/hermes_vision`**: `hermes_vision.detect` (YuNet, Haar fallback; no ROS imports) and a node
+  `face_detector` that subscribes to the compressed image and publishes `/vision/faces`
+  (`vision_msgs/Detection2DArray`, bbox centre/size in pixels, `class_id: face`, score), at `rate_hz`
+  (default 4, ~47% of a core on a Pi 4); `publish_debug:=true` adds an annotated image. CLI:
+  `python -m hermes_vision.detect_cli IMAGE`. YuNet 2023mar needs OpenCV 4.8+, so `~/venv` has
+  `opencv-python-headless==4.10.0.84` with `numpy<2` (system NumPy stays 1.26.4 for ROS), and
+  `robot-env.sh` puts the venv first on `PYTHONPATH`; YuNet also needs input sides that are multiples
+  of 32. Tested on a sample image and on a live frame (one face found, upright, no flip needed).
+- **Audio: card works, microphones read digital silence (open).** The stock `wm8960-soundcard`
+  overlay fails every stream with `wm8960: No MCLK configured` on Ubuntu 24.04: it puts the 12.288 MHz
+  fixed clock on the sound card's codec link, but the codec driver reads `mclk` from its own node.
+  `pi/overlays/hermes-wm8960.dts` is the stock overlay plus `clocks`/`clock-names` on the codec node;
+  `pi/install-audio-overlay.sh` compiles it to `/boot/firmware/overlays/hermes-wm8960.dtbo`, sets
+  `dtparam=audio=off` and `dtoverlay=hermes-wm8960`. With it, capture opens at 8-48 kHz. Address
+  the card by name (`hw:CARD=wm8960soundcard,DEV=0`): its index changes (it was card 1, then 0).
+  But every recording is exactly zero (no noise floor), with every mixer path on and gains maxed.
+  Checked: DAPM shows ADCs, input mixers, boost mixers and LINPUT1/RINPUT1 on; registers
+  `R25=0xfc`, `R47=0x30`, `R32/33=0x108`; MCLK 12.288 MHz and BCLK 1.536 MHz running; GPIO18-21 in
+  `alt0` for the I2S; setting `ALRCGPIO` (R9 bit 6) over I2C made no difference. Not yet checked:
+  the HAT's hardware **Audio ON/OFF slide switch** position, MICBIAS, a DAC-to-ADC loopback.
+  `Left/Right Input Mixer Boost Switch` default to off and must be turned on for the mic path.
+  No speakers on hand, so playback is untested.
+- Not done: PWM (waits for servos). Ask before rebooting the Pi.

 ## Package 0b (USB link): Pi side and talos side working

@@ -1,101 +0,0 @@
-#!/usr/bin/env python3
-"""Detect faces in images and write annotated copies.
-
-  detect_faces.py IMAGE [IMAGE ...] [--out DIR] [--min-size PX]
-
-Uses OpenCV's YuNet detector when its model loads (~/models/face_detection_yunet_2023mar.onnx),
-otherwise the Haar cascades shipped with opencv-data (frontal, plus profile in both directions).
-Prints one line per image: the number of faces, then x,y,w,h boxes in pixels.
-"""
-import argparse
-import os
-
-import cv2
-import numpy as np
-
-HAAR = '/usr/share/opencv4/haarcascades'
-YUNET = os.path.expanduser('~/models/face_detection_yunet_2023mar.onnx')
-
-
-def yunet_detector(w, h):
-    if not os.path.exists(YUNET):
-        return None
-    try:
-        cv2.FaceDetectorYN.create(YUNET, '', (640, 480), 0.6, 0.3, 5000)
-    except cv2.error:
-        return None
-    by_size = {}
-
-    def run(img):
-        # This OpenCV's YuNet needs input sides that are multiples of 32: work at about 640 wide.
-        h0, w0 = img.shape[:2]
-        scale = 640 / max(w0, h0) if max(w0, h0) > 640 else 1.0
-        tw, th = max(32, round(w0 * scale / 32) * 32), max(32, round(h0 * scale / 32) * 32)
-        small = cv2.resize(img, (tw, th), interpolation=cv2.INTER_AREA)
-        if (tw, th) not in by_size:   # OpenCV 4.6 breaks if the input size changes after create()
-            by_size[(tw, th)] = cv2.FaceDetectorYN.create(YUNET, '', (tw, th), 0.6, 0.3, 5000)
-        _, faces = by_size[(tw, th)].detect(small)
-        if faces is None:
-            return []
-        fx, fy = w0 / tw, h0 / th
-        return [(int(f[0] * fx), int(f[1] * fy), int(f[2] * fx), int(f[3] * fy)) for f in faces]
-    return run
-
-
-def haar_detector(min_size):
-    frontal = cv2.CascadeClassifier(os.path.join(HAAR, 'haarcascade_frontalface_alt2.xml'))
-    profile = cv2.CascadeClassifier(os.path.join(HAAR, 'haarcascade_profileface.xml'))
-
-    def run(img):
-        gray = cv2.equalizeHist(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY))
-        ms = (min_size, min_size)
-        boxes = [tuple(b) for b in 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 profile.detectMultiScale(g, 1.1, 5, minSize=ms):
-                boxes.append((gray.shape[1] - x - w if flip else x, y, w, h))
-        return merge(boxes)
-    return run
-
-
-def merge(boxes, overlap=0.4):
-    """Drop boxes that mostly overlap an earlier one (frontal and profile can both fire)."""
-    kept = []
-    for x, y, w, h in boxes:
-        for kx, ky, kw, kh in kept:
-            iw = min(x + w, kx + kw) - max(x, kx)
-            ih = min(y + h, ky + kh) - max(y, ky)
-            if iw > 0 and ih > 0 and iw * ih > overlap * min(w * h, kw * kh):
-                break
-        else:
-            kept.append((int(x), int(y), int(w), int(h)))
-    return kept
-
-
-def main():
-    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
-    p.add_argument('images', nargs='+')
-    p.add_argument('--out', default='.')
-    p.add_argument('--min-size', type=int, default=40)
-    args = p.parse_args()
-    os.makedirs(args.out, exist_ok=True)
-    detector, name = None, None
-    for path in args.images:
-        img = cv2.imread(path)
-        if img is None:
-            print(f'{path}: unreadable')
-            continue
-        if detector is None:
-            detector = yunet_detector(img.shape[1], img.shape[0])
-            name = 'yunet' if detector else 'haar'
-            detector = detector or haar_detector(args.min_size)
-        boxes = detector(img)
-        for x, y, w, h in boxes:
-            cv2.rectangle(img, (x, y), (x + w, y + h), (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(boxes)} face(s) [{name}] {boxes} -> {dest}')
-
-
-if __name__ == '__main__':
-    main()
@@ -0,0 +1,21 @@
+#!/bin/bash
+# BrainCraft HAT audio (HANDOFF package 1): our WM8960 overlay (pi/overlays/hermes-wm8960.dts: the
+# stock wm8960-soundcard plus the clock on the codec node, which Ubuntu's driver needs), with the
+# Pi's own headphone audio off (it shares PWM hardware with the servos). Run as root. Re-runnable.
+# Needs a reboot afterwards.
+set -euo pipefail
+cd "$(dirname "$0")"
+
+command -v dtc >/dev/null || apt-get -y install device-tree-compiler
+dtc -@ -I dts -O dtb -o /boot/firmware/overlays/hermes-wm8960.dtbo overlays/hermes-wm8960.dts
+
+cfg=/boot/firmware/config.txt
+[ -e "$cfg.bak-audio" ] || cp "$cfg" "$cfg.bak-audio"
+sed -i 's/^dtparam=audio=on$/dtparam=audio=off/' "$cfg"
+sed -i '/^dtoverlay=wm8960-soundcard$/d' "$cfg"
+if ! grep -qx 'dtoverlay=hermes-wm8960' "$cfg"; then
+  # dtparam lines must come before the first dtoverlay, so put this right after vc4-kms-v3d
+  sed -i '0,/^dtoverlay=vc4-kms-v3d$/s//dtoverlay=vc4-kms-v3d\ndtoverlay=hermes-wm8960/' "$cfg"
+fi
+grep -nE '^dtparam=audio|^dtoverlay=(hermes-)?wm8960' "$cfg"
+echo "done; reboot to apply"
@@ -0,0 +1,87 @@
+/dts-v1/;
+/plugin/;
+
+/*
+ * BrainCraft HAT audio: the WM8960 codec at I2C 0x1a with its 12.288 MHz master clock.
+ *
+ * Same as the kernel's wm8960-soundcard overlay, except that the clock is also attached to the
+ * codec's own node. Ubuntu 24.04's wm8960 driver reads "mclk" from there; the stock overlay only
+ * puts it on the sound card's codec link, so every capture/playback fails with
+ * "wm8960: No MCLK configured". Compiled by pi/install-audio-overlay.sh.
+ */
+/ {
+	compatible = "brcm,bcm2835";
+
+	fragment@0 {
+		target = <&i2s_clk_producer>;
+		__overlay__ {
+			status = "okay";
+		};
+	};
+
+	fragment@1 {
+		target-path = "/";
+		__overlay__ {
+			wm8960_mclk: wm8960_mclk {
+				compatible = "fixed-clock";
+				#clock-cells = <0>;
+				clock-frequency = <12288000>;
+			};
+		};
+	};
+
+	fragment@2 {
+		target = <&i2c1>;
+		__overlay__ {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "okay";
+
+			wm8960: wm8960 {
+				compatible = "wlf,wm8960";
+				reg = <0x1a>;
+				#sound-dai-cells = <0>;
+				clocks = <&wm8960_mclk>;
+				clock-names = "mclk";
+				AVDD-supply = <&vdd_5v0_reg>;
+				DVDD-supply = <&vdd_3v3_reg>;
+			};
+		};
+	};
+
+	fragment@3 {
+		target = <&sound>;
+		slave_overlay: __overlay__ {
+			compatible = "simple-audio-card";
+			simple-audio-card,format = "i2s";
+			simple-audio-card,name = "wm8960-soundcard";
+			status = "okay";
+
+			simple-audio-card,widgets =
+				"Microphone", "Mic Jack",
+				"Line", "Line In",
+				"Line", "Line Out",
+				"Speaker", "Speaker",
+				"Headphone", "Headphone Jack";
+			simple-audio-card,routing =
+				"Headphone Jack", "HP_L",
+				"Headphone Jack", "HP_R",
+				"Speaker", "SPK_LP",
+				"Speaker", "SPK_LN",
+				"LINPUT1", "Mic Jack",
+				"LINPUT3", "Mic Jack",
+				"RINPUT1", "Mic Jack",
+				"RINPUT2", "Mic Jack";
+
+			simple-audio-card,cpu {
+				sound-dai = <&i2s_clk_producer>;
+			};
+
+			dailink0_slave: simple-audio-card,codec {
+				sound-dai = <&wm8960>;
+				clocks = <&wm8960_mclk>;
+				clock-names = "mclk";
+			};
+		};
+	};
+};
@@ -0,0 +1,13 @@
+# Source this before running robot nodes on the Pi:  source ~/robot-env.sh
+#  - ROS Jazzy, then the overlay workspaces (camera_ros built against the fork; hermes_* packages)
+#  - the Raspberry Pi libcamera fork ahead of ROS's own copy (same soname, but ROS's crashes in the
+#    Pi image plugin: see CONTEXT.md)
+#  - ~/venv's site-packages first: Blinka/Adafruit for the display, OpenCV 4.10 for YuNet
+#  - a fixed ROS domain and subnet-only discovery, matching the host-side podman shell
+source /opt/ros/jazzy/setup.bash
+[ -f "$HOME/camera_ws/install/setup.bash" ] && source "$HOME/camera_ws/install/setup.bash"
+[ -f "$HOME/ros_ws/install/setup.bash" ] && source "$HOME/ros_ws/install/setup.bash"
+export LD_LIBRARY_PATH="/opt/libcamera-rpi/lib/aarch64-linux-gnu:${LD_LIBRARY_PATH:-}"
+export PYTHONPATH="$HOME/venv/lib/python3.12/site-packages:${PYTHONPATH:-}"
+export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}"
+export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-SUBNET}"
@@ -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)
@@ -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()
@@ -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()
@@ -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>
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/hermes_vision
+[install]
+install_scripts=$base/lib/hermes_vision
@@ -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',
+        ],
+    },
+)
@@ -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)) == []