From 8d5033d0a2f4afc98bc929d865abe493173624f8 Mon Sep 17 00:00:00 2001 From: "K. Isom" Date: Sun, 4 Oct 2026 22:55:53 -0700 Subject: [PATCH] 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 Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs --- CONTEXT.md | 35 ++++-- pi/detect_faces.py | 101 ------------------ pi/install-audio-overlay.sh | 21 ++++ pi/overlays/hermes-wm8960.dts | 87 +++++++++++++++ pi/robot-env.sh | 13 +++ ros/hermes_vision/hermes_vision/__init__.py | 0 ros/hermes_vision/hermes_vision/detect.py | 95 ++++++++++++++++ ros/hermes_vision/hermes_vision/detect_cli.py | 39 +++++++ .../hermes_vision/face_detector_node.py | 80 ++++++++++++++ ros/hermes_vision/package.xml | 20 ++++ ros/hermes_vision/resource/hermes_vision | 0 ros/hermes_vision/setup.cfg | 4 + ros/hermes_vision/setup.py | 25 +++++ ros/hermes_vision/test/test_detect.py | 27 +++++ 14 files changed, 440 insertions(+), 107 deletions(-) delete mode 100644 pi/detect_faces.py create mode 100755 pi/install-audio-overlay.sh create mode 100644 pi/overlays/hermes-wm8960.dts create mode 100644 pi/robot-env.sh create mode 100644 ros/hermes_vision/hermes_vision/__init__.py create mode 100644 ros/hermes_vision/hermes_vision/detect.py create mode 100644 ros/hermes_vision/hermes_vision/detect_cli.py create mode 100644 ros/hermes_vision/hermes_vision/face_detector_node.py create mode 100644 ros/hermes_vision/package.xml create mode 100644 ros/hermes_vision/resource/hermes_vision create mode 100644 ros/hermes_vision/setup.cfg create mode 100644 ros/hermes_vision/setup.py create mode 100644 ros/hermes_vision/test/test_detect.py diff --git a/CONTEXT.md b/CONTEXT.md index 2ee3fa2..1e90e4d 100644 --- a/CONTEXT.md +++ b/CONTEXT.md @@ -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 diff --git a/pi/detect_faces.py b/pi/detect_faces.py deleted file mode 100644 index 2e3519d..0000000 --- a/pi/detect_faces.py +++ /dev/null @@ -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() diff --git a/pi/install-audio-overlay.sh b/pi/install-audio-overlay.sh new file mode 100755 index 0000000..84866a1 --- /dev/null +++ b/pi/install-audio-overlay.sh @@ -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" diff --git a/pi/overlays/hermes-wm8960.dts b/pi/overlays/hermes-wm8960.dts new file mode 100644 index 0000000..ab775ac --- /dev/null +++ b/pi/overlays/hermes-wm8960.dts @@ -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"; + }; + }; + }; +}; diff --git a/pi/robot-env.sh b/pi/robot-env.sh new file mode 100644 index 0000000..cbd508c --- /dev/null +++ b/pi/robot-env.sh @@ -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}" diff --git a/ros/hermes_vision/hermes_vision/__init__.py b/ros/hermes_vision/hermes_vision/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_vision/hermes_vision/detect.py b/ros/hermes_vision/hermes_vision/detect.py new file mode 100644 index 0000000..c8db18b --- /dev/null +++ b/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) diff --git a/ros/hermes_vision/hermes_vision/detect_cli.py b/ros/hermes_vision/hermes_vision/detect_cli.py new file mode 100644 index 0000000..4665ef0 --- /dev/null +++ b/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() diff --git a/ros/hermes_vision/hermes_vision/face_detector_node.py b/ros/hermes_vision/hermes_vision/face_detector_node.py new file mode 100644 index 0000000..4572127 --- /dev/null +++ b/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() diff --git a/ros/hermes_vision/package.xml b/ros/hermes_vision/package.xml new file mode 100644 index 0000000..834b937 --- /dev/null +++ b/ros/hermes_vision/package.xml @@ -0,0 +1,20 @@ + + + + hermes_vision + 0.1.0 + Face detection on the Hermes robot's camera image. + K. Isom + UNLICENSED + + rclpy + sensor_msgs + vision_msgs + python3-numpy + + python3-pytest + + + ament_python + + diff --git a/ros/hermes_vision/resource/hermes_vision b/ros/hermes_vision/resource/hermes_vision new file mode 100644 index 0000000..e69de29 diff --git a/ros/hermes_vision/setup.cfg b/ros/hermes_vision/setup.cfg new file mode 100644 index 0000000..77c669c --- /dev/null +++ b/ros/hermes_vision/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/hermes_vision +[install] +install_scripts=$base/lib/hermes_vision diff --git a/ros/hermes_vision/setup.py b/ros/hermes_vision/setup.py new file mode 100644 index 0000000..7ed7d65 --- /dev/null +++ b/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', + ], + }, +) diff --git a/ros/hermes_vision/test/test_detect.py b/ros/hermes_vision/test/test_detect.py new file mode 100644 index 0000000..58ca549 --- /dev/null +++ b/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)) == []