diff --git a/CONTEXT.md b/CONTEXT.md
index b09c8b6..3899bde 100644
--- a/CONTEXT.md
+++ b/CONTEXT.md
@@ -211,8 +211,8 @@ hotter, so fit the fan before enclosing it. Playback and the servos are untested
The only way in or out of the ROS graph. Serves MCP (SDK 2.3, streamable HTTP) on
`http://10.55.0.2:8765/mcp`, the USB address only (retrying the bind if the link is not up yet), with
-a bearer token. **Tool names are the stable contract with Hermes**: `look()`, and `turn_head` /
-`head_position` only when a head is fitted. With no servos only `look` is registered, by design
+a bearer token. **Tool names are the stable contract with Hermes**: `look()`, `faces()`, `list_people()`, and `turn_head` /
+`head_position` only when a head is fitted. With no servos those two are not registered, by design
(`head: false`; the node refuses `head: true` until `hermes_head` exists). `look()` waits for the next
camera frame taken after the call (never a stale one) and passes the camera's JPEG through (1296x972,
~155 KB); it also publishes `looking` on `/hermes/state` so the face squints and flashes.
@@ -250,6 +250,30 @@ Hermes does it itself, so `look()` needs no change. Two consequences: each look(
room in talos's Hermes image cache (Hermes's retention, not the robot's), and a look-and-describe turn
takes ~70 s on the local models, mostly the vision call.
+### Hermes's feedback on the bridge (2026-10-05) and what was done
+
+Hermes (on talos, from `~/.hermes/cache/scratch/hermes-bot-v0-face-identity.md`) asked for recognitions
+instead of raw photos. **Built:** `faces()` returns the faces in view as structured JSON with no photo
+(`track_id`, pixel and normalised `bbox`, detection `confidence`, `seen_for_s`, and `identity`
+`{name, similarity}` or `null`); `look(send_image="always"|"never")` now returns that same JSON as a
+text block plus the photo (default kept as before: the image, so scene description still works);
+`list_people()` gives names and sample counts; a `robot://status` resource (hardware present or
+absent, what is detected, enrolled count, tools) and prompts `who_is_here` and `look_around` fill the
+previously empty `list_resources` / `list_prompts`; `/vision/faces` carries a persistent track id
+(`hermes_vision.tracking.FaceTracker`, matching by box overlap, identity re-checked once a second;
+`Detection2D.id` is the track id, `results[0]` the face, `results[1]` the identity if recognised).
+Verified with a real Hermes turn on the local `ornith` model: it called `faces()` (0.03 s, 593
+characters, no image, no vision call) and answered that Kyle was there at 0.78 similarity.
+**Declined, deliberately:** embeddings in responses (biometric templates, meaningless to an LLM,
+and the registry stays on the robot); age/gender/hat attributes (no model for them here, and guessing
+age and gender from faces is unreliable and sensitive); auto-updating a person's face data from new
+captures (it lets a stranger slowly become "Kyle" with nobody deciding that); a remote `enroll` tool
+(a prompt could make the robot call anyone "Kyle": enrolment stays the owner's step on the robot,
+`python3 -m hermes_vision.people enroll NAME`; a gated enrol tool, off by default, is possible if the
+owner wants it); detecting people, objects or motion (only faces exist, and `robot://status` says so).
+`look()`'s default was not flipped to JSON-only, because the image is what lets Hermes describe a
+scene; that default is the owner's call.
+
## Face recognition (opt-in)
`hermes_vision.recognize` / `people`: SFace embeddings on YuNet's landmarks. Nothing is stored until
diff --git a/README.md b/README.md
index bb91052..762d635 100644
--- a/README.md
+++ b/README.md
@@ -100,10 +100,17 @@ door.
```
ping 10.55.0.2
-hermes mcp test robot # connects, finds the look tool
-hermes chat -q "Use the look tool and tell me what you see."
+hermes mcp test robot # connects, lists the tools
+hermes chat -q "Who is at the robot's desk right now?" # calls faces(): instant, no photo
+hermes chat -q "Use the look tool and tell me what you see." # sends a photo
```
+What Hermes can use: `faces()` (who is in view: names, positions, tracking ids; instant, no photo),
+`look()` (the same faces summary plus a current photo), `list_people()`, and, once a head is fitted,
+`turn_head` and `head_position`. `robot://status` says what hardware this robot actually has. Only faces
+are detected. Face data never leaves the robot: Hermes gets names and similarity scores, and an
+unrecognised face is `null`, never a guess.
+
The robot's face wakes when Hermes connects and falls asleep about 40 seconds after it leaves. A
look-and-describe turn takes a minute or so on local models: `look()` itself is instant. Hermes does not
give MCP images to the main model: it saves the photo to its image cache and the model calls
diff --git a/pi/calibration/ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972.yaml b/pi/calibration/ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972.yaml
new file mode 100644
index 0000000..9b55333
--- /dev/null
+++ b/pi/calibration/ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972.yaml
@@ -0,0 +1,20 @@
+image_width: 1296
+image_height: 972
+camera_name: ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1152.96331, 0, 652.952458, 0, 1151.52195, 463.351689, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.0727701258, -0.232756953, 0.00468014288, -0.00488550855, 0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1152.96331, 0, 652.952458, 0, 0, 1151.52195, 463.351689, 0, 0, 0, 1, 0]
diff --git a/pi/install.sh b/pi/install.sh
index 71a7882..83dfc02 100755
--- a/pi/install.sh
+++ b/pi/install.sh
@@ -116,6 +116,13 @@ chown -R "$ROBOT_USER:$ROBOT_USER" "$HOME_DIR/ros_ws/src"
as_user bash -c "source /opt/ros/jazzy/setup.bash && source '$HOME_DIR/camera_ws/install/setup.bash' \
&& cd '$HOME_DIR/ros_ws' && colcon build --symlink-install"
+step "Camera calibration (this camera's, from pi/calibration; an existing file is never overwritten)"
+as_user mkdir -p "$HOME_DIR/.ros/camera_info"
+for f in "$PI_DIR"/calibration/*.yaml; do
+ [ -e "$f" ] || continue
+ [ -e "$HOME_DIR/.ros/camera_info/$(basename "$f")" ] || install -o "$ROBOT_USER" -g "$ROBOT_USER" -m 644 "$f" "$HOME_DIR/.ros/camera_info/"
+done
+
step "USB gadget (NCM Ethernet + ACM serial) and the audio codec overlay"
before="$(cfg_sum)"
bash "$PI_DIR/install-usb-gadget.sh"
diff --git a/ros/hermes_bringup/config/robot.yaml b/ros/hermes_bringup/config/robot.yaml
index d80e119..3a86970 100644
--- a/ros/hermes_bringup/config/robot.yaml
+++ b/ros/hermes_bringup/config/robot.yaml
@@ -36,5 +36,6 @@ hermes_mcp:
bind: 10.55.0.2 # the USB link; the firewall accepts 8765 only from 10.55.0.1 on usb0
port: 8765
head: false # no servos fitted yet: turn_head and head_position stay unregistered
+ speakers: false # no speakers fitted yet: reported honestly in robot://status
connected_grace_s: 30.0 # an authenticated client heard within this long keeps the heartbeat going
look_timeout_s: 3.0
diff --git a/ros/hermes_face/hermes_face/face_node.py b/ros/hermes_face/hermes_face/face_node.py
index d829cbb..5512087 100644
--- a/ros/hermes_face/hermes_face/face_node.py
+++ b/ros/hermes_face/hermes_face/face_node.py
@@ -3,7 +3,8 @@
Subscribes: /hermes/state (std_msgs/String): sleeping idle listening thinking looking speaking
/joint_states (sensor_msgs/JointState): pan_joint, tilt_joint in radians. The sign
convention is the MCP tool's: positive pan = the robot's left, positive tilt = up.
- /vision/faces (vision_msgs/Detection2DArray): the eyes glance toward the best face
+ /vision/faces (vision_msgs/Detection2DArray): the eyes glance toward the best face (a named one
+ first: the name is results[1].class_id)
/camera/camera_info (sensor_msgs/CameraInfo): the image size for that, if published
/audio/activity (std_msgs/String): speaking | listening | quiet, from hermes_audio
/audio/scope/{mic,speaker} (std_msgs/Float32MultiArray): the real waveform for the mouth
@@ -115,7 +116,8 @@ class FaceNode(Node):
def _on_faces(self, msg: Detection2DArray) -> None:
target = pick_target([
Target(d.bbox.center.position.x, d.bbox.center.position.y, d.bbox.size_x, d.bbox.size_y,
- d.results[0].hypothesis.score if d.results else 1.0, d.id)
+ d.results[0].hypothesis.score if d.results else 1.0,
+ d.results[1].hypothesis.class_id if len(d.results) > 1 else '') # the name, if recognised
for d in msg.detections])
if target:
gx, gy = gaze_from_pixel(target.cx, target.cy, *self.image_size)
diff --git a/ros/hermes_mcp/hermes_mcp/bridge.py b/ros/hermes_mcp/hermes_mcp/bridge.py
index d87fc72..18e5785 100644
--- a/ros/hermes_mcp/hermes_mcp/bridge.py
+++ b/ros/hermes_mcp/hermes_mcp/bridge.py
@@ -2,16 +2,25 @@
so it is testable with a stub camera.
MCP tool names are the stable contract with Hermes and must not change when the internals do:
- look() -> the next fresh JPEG from the camera
+ look(send_image) -> the faces in view as JSON text, and (by default) the next fresh JPEG
+ faces() -> the same faces summary with no photo: cheap, the right call for "who is here"
+ list_people() -> the names enrolled on the robot (never their face data)
turn_head(pan, tilt) -> only registered if a head backend is given (servos fitted)
head_position() -> likewise
+Also an `robot://status` resource and prompts, so a client can discover what this robot can do.
+
+Faces never come with embeddings: they are biometric templates, meaningless to a language model, and
+the registry stays on the robot. A face that is not recognised has identity null; a name is never
+made up. Enrolling people is deliberately not a tool: it is a step the owner takes on the robot.
"""
from __future__ import annotations
import hmac
+import json
+import os
import threading
import time
-from typing import Callable, Optional, Protocol
+from typing import Any, Callable, Literal, Optional, Protocol
from mcp.server.mcpserver import Image, MCPServer
from mcp.server.transport_security import TransportSecuritySettings
@@ -43,32 +52,131 @@ class LatestFrame:
return self._jpeg
+class FacesBuffer:
+ """The newest face detections, with how long each tracked person has been in view."""
+
+ def __init__(self) -> None:
+ self._cv = threading.Condition()
+ self._faces: Optional[list[dict]] = None
+ self._size = (1296, 972)
+ self._t = -1.0
+ self._first: dict[int, tuple[float, float]] = {} # track id -> (first seen, last seen)
+
+ def set_size(self, width: int, height: int) -> None:
+ with self._cv:
+ self._size = (int(width), int(height))
+
+ def put(self, faces: list[dict], t: Optional[float] = None) -> None:
+ """faces: dicts with track_id, bbox {x, y, w, h}, confidence, name ('' if unknown), similarity."""
+ now = time.monotonic() if t is None else t
+ with self._cv:
+ for f in faces:
+ first, _ = self._first.get(f['track_id'], (now, now))
+ self._first[f['track_id']] = (first, now)
+ self._first = {i: v for i, v in self._first.items() if now - v[1] < 5.0}
+ self._faces, self._t = faces, now
+ self._cv.notify_all()
+
+ def _snapshot(self, now: float) -> dict:
+ w, h = self._size
+ out = []
+ for f in self._faces or []:
+ b = f['bbox']
+ first = self._first.get(f['track_id'], (now, now))[0]
+ out.append({
+ 'track_id': f['track_id'],
+ 'bbox': {k: round(float(b[k])) for k in ('x', 'y', 'w', 'h')},
+ 'bbox_norm': {'cx': round((b['x'] + b['w'] / 2) / w, 3), 'cy': round((b['y'] + b['h'] / 2) / h, 3),
+ 'w': round(b['w'] / w, 3), 'h': round(b['h'] / h, 3)},
+ 'confidence': round(float(f['confidence']), 2),
+ 'identity': ({'name': f['name'], 'similarity': round(float(f['similarity']), 2)}
+ if f.get('name') else None),
+ 'seen_for_s': round(max(0.0, now - first), 1),
+ })
+ return {'available': True, 'count': len(out), 'image_size': {'w': w, 'h': h},
+ 'data_age_s': round(max(0.0, now - self._t), 2), 'faces': out}
+
+ def summary(self, max_age_s: float = 1.5, wait_s: float = 0.0) -> dict:
+ """The faces in view now. If the newest data is older than max_age_s, wait up to wait_s for fresh
+ data; if there is none, say so rather than reporting a stale room."""
+ deadline = time.monotonic() + wait_s
+ with self._cv:
+ while self._faces is None or time.monotonic() - self._t > max_age_s:
+ remaining = deadline - time.monotonic()
+ if remaining <= 0:
+ return {'available': False, 'reason': 'face detection is not producing results right now'}
+ self._cv.wait(remaining)
+ return self._snapshot(time.monotonic())
+
+
+def people_summary(path: str) -> dict:
+ """Who is enrolled on this robot: names and how many samples, read straight from the registry file
+ (JSON), never the face data itself."""
+ try:
+ with open(path) as f:
+ db = json.load(f)
+ except FileNotFoundError:
+ db = {}
+ return {'count': len(db), 'people': [{'name': n, 'samples': len(v)} for n, v in sorted(db.items())]}
+
+
class Head(Protocol):
def move(self, pan: float, tilt: float) -> tuple[float, float]: ...
def position(self) -> tuple[float, float]: ...
def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: None,
- head: Optional[Head] = None, look_timeout_s: float = 3.0) -> MCPServer:
+ head: Optional[Head] = None, look_timeout_s: float = 3.0,
+ faces_buf: Optional[FacesBuffer] = None, people_path: str = '',
+ hardware: Optional[dict] = None) -> MCPServer:
+ faces_buf = faces_buf or FacesBuffer()
+ hw = {'camera': True, 'head': head is not None, 'speakers': False, **(hardware or {})}
mcp = MCPServer(
"robot",
instructions=(
- "This server is a small physical robot on the user's desk. "
- "Call look() whenever you need to see what is in front of it; the image is current. "
- "If turn_head is available, aim first and then look."
+ "This server is a small physical robot on the user's desk, with a camera and a face display. "
+ "To know WHO or HOW MANY people are there, call faces(): it is instant and returns names, "
+ "positions and tracking ids with no photo; identity null means unknown, so say so rather "
+ "than guessing. Call look() when you need to see the scene itself: it returns the faces "
+ "summary and a current photo. list_people() says who the robot knows. "
+ + ("turn_head aims the camera: aim first, then look. " if head is not None else
+ "The camera is fixed: there is no head to turn. ")
+ + "Only faces are detected: not other people-shaped things, objects or motion. "
+ "Read robot://status for what hardware this robot has."
),
)
@mcp.tool()
- def look() -> Image:
- """Take a photograph with the robot's camera and return it. Use this to see the room,
- the desk, the person, or anything the user asks you to look at."""
+ def faces() -> dict[str, Any]:
+ """Report the faces the robot's camera sees right now, with no photo (instant and cheap).
+ Each face has a track_id (the same person keeps the same id while they stay in view),
+ a bounding box in camera pixels and normalised, a detection confidence, seen_for_s (how long
+ they have been in view), and identity: {name, similarity} if the robot recognises them, or
+ null if not. null means unknown: never invent a name for it. similarity is a cosine score, not a
+ probability. Use this to answer who or how many."""
+ return faces_buf.summary(max_age_s=1.5, wait_s=1.5)
+
+ @mcp.tool()
+ def look(send_image: Literal["always", "never"] = "always") -> list:
+ """Look through the robot's camera. Returns the faces summary (JSON text, same as faces()) and,
+ unless send_image is "never", a current photograph of the scene. Use it to see the room, the
+ desk, a document, or anything the user asks you to look at; use faces() alone for who/how many."""
t0 = time.monotonic()
announce_look() # the face squints and flashes
+ summary = faces_buf.summary(max_age_s=1.5, wait_s=0.3)
+ text = json.dumps({'faces': summary})
+ if send_image == "never":
+ return [text]
jpeg = frames.wait_newer(t0, look_timeout_s)
if jpeg is None:
raise RuntimeError("the robot's camera is not producing images")
- return Image(data=jpeg, format="jpeg")
+ return [text, Image(data=jpeg, format="jpeg")]
+
+ @mcp.tool()
+ def list_people() -> dict[str, Any]:
+ """Who the robot has been taught to recognise: names and how many samples each, never their
+ face data. People are enrolled by the owner on the robot, not through this server."""
+ return people_summary(people_path) if people_path else {'count': 0, 'people': []}
if head is not None:
@mcp.tool()
@@ -86,6 +194,33 @@ def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: N
p, t = head.position()
return f"pan {p:+.0f} degrees, tilt {t:+.0f} degrees"
+ @mcp.resource("robot://status")
+ def status() -> str:
+ """What this robot is and what it can do right now: hardware present, what is detected, who is known."""
+ live = faces_buf.summary(max_age_s=3.0)
+ return json.dumps({
+ 'robot': 'hermes-bot', 'hardware': hw,
+ 'camera': {'image_size': live.get('image_size') if live['available'] else None},
+ 'detects': ['faces'], 'not_detected': ['people', 'objects', 'motion'],
+ 'face_detection_running': live['available'],
+ 'recognition': {'enrolled': people_summary(people_path)['count'] if people_path else 0,
+ 'registry': 'on the robot; enrolled by the owner, not through MCP; embeddings never leave it'},
+ 'tools': sorted(['faces', 'look', 'list_people'] + (['turn_head', 'head_position'] if head else [])),
+ }, indent=2)
+
+ @mcp.prompt()
+ def who_is_here() -> str:
+ """Find out who is at the robot's desk."""
+ return ("Call faces() and tell me who is there. Name the people the robot recognises, say how many "
+ "faces it does not recognise (identity null: do not guess who they are), and mention anyone "
+ "who has only just appeared. If face detection is unavailable, say so.")
+
+ @mcp.prompt()
+ def look_around() -> str:
+ """See the scene and describe it."""
+ return ("Call look() and describe what you see in the photo, then say who the faces summary says is "
+ "there. Do not name anyone the summary lists as identity null.")
+
return mcp
diff --git a/ros/hermes_mcp/hermes_mcp/mcp_node.py b/ros/hermes_mcp/hermes_mcp/mcp_node.py
index 7efd85d..d398253 100644
--- a/ros/hermes_mcp/hermes_mcp/mcp_node.py
+++ b/ros/hermes_mcp/hermes_mcp/mcp_node.py
@@ -2,6 +2,7 @@
Serves MCP (streamable HTTP, bearer token) to Hermes on the USB link and talks to the graph locally.
Subscribes: image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
+ /vision/faces (vision_msgs/Detection2DArray) and /camera/camera_info, for faces() and look()
Publishes: /hermes/state (std_msgs/String):
`idle` once a second while an authenticated MCP client is connected: the agent heartbeat
the face uses to wake and, when it stops, to fall asleep ("no agent connected");
@@ -16,10 +17,11 @@ import time
import rclpy
import uvicorn
from rclpy.node import Node
-from sensor_msgs.msg import CompressedImage
+from sensor_msgs.msg import CameraInfo, CompressedImage
from std_msgs.msg import String
+from vision_msgs.msg import Detection2DArray
-from .bridge import LatestFrame, build_app, build_mcp
+from .bridge import FacesBuffer, LatestFrame, build_app, build_mcp
class McpNode(Node):
@@ -36,6 +38,11 @@ class McpNode(Node):
self.state_pub = self.create_publisher(String, '/hermes/state', 10)
self.frames = LatestFrame()
self.create_subscription(CompressedImage, topic, lambda m: self.frames.put(bytes(m.data)), 1)
+ self.faces = FacesBuffer()
+ self.create_subscription(Detection2DArray, '/vision/faces', self._on_faces, 5)
+ self.create_subscription(CameraInfo, '/camera/camera_info', self._on_camera_info, 1)
+ people_path = os.path.expanduser(str(param('faces_db', '~/.local/share/hermes/faces.json')))
+ hardware = {'speakers': bool(param('speakers', False))}
token = os.environ.get('ROBOT_MCP_TOKEN', '').strip()
self.activity = None
@@ -45,11 +52,31 @@ class McpNode(Node):
self.get_logger().error('ROBOT_MCP_TOKEN is missing or too short (openssl rand -hex 32): '
'not serving MCP, and no heartbeat')
return
- mcp = build_mcp(self.frames, self._announce_look, head=None, look_timeout_s=look_timeout)
+ mcp = build_mcp(self.frames, self._announce_look, head=None, look_timeout_s=look_timeout,
+ faces_buf=self.faces, people_path=people_path, hardware=hardware)
self.app, self.activity = build_app(mcp, token, self.host)
threading.Thread(target=self._serve, name='mcp-http', daemon=True).start()
self.create_timer(1.0, self._heartbeat)
+ def _on_camera_info(self, msg: CameraInfo) -> None:
+ if msg.width and msg.height:
+ self.faces.set_size(msg.width, msg.height)
+
+ def _on_faces(self, msg: Detection2DArray) -> None:
+ faces = []
+ for d in msg.detections:
+ if not d.results:
+ continue
+ c = d.bbox.center.position
+ faces.append({
+ 'track_id': int(d.id) if d.id.isdigit() else hash(d.id) & 0xFFFF,
+ 'bbox': {'x': c.x - d.bbox.size_x / 2, 'y': c.y - d.bbox.size_y / 2, 'w': d.bbox.size_x, 'h': d.bbox.size_y},
+ 'confidence': d.results[0].hypothesis.score,
+ 'name': d.results[1].hypothesis.class_id if len(d.results) > 1 else '',
+ 'similarity': d.results[1].hypothesis.score if len(d.results) > 1 else 0.0,
+ })
+ self.faces.put(faces)
+
def _announce_look(self) -> None:
self.state_pub.publish(String(data='looking'))
diff --git a/ros/hermes_mcp/package.xml b/ros/hermes_mcp/package.xml
index 4023f6c..74ec1a0 100644
--- a/ros/hermes_mcp/package.xml
+++ b/ros/hermes_mcp/package.xml
@@ -10,6 +10,7 @@
rclpy
sensor_msgs
std_msgs
+ vision_msgs
python3-pytest
diff --git a/ros/hermes_mcp/test/test_bridge.py b/ros/hermes_mcp/test/test_bridge.py
index d84da23..4486cfb 100644
--- a/ros/hermes_mcp/test/test_bridge.py
+++ b/ros/hermes_mcp/test/test_bridge.py
@@ -1,5 +1,6 @@
import asyncio
import base64
+import json
import threading
import time
@@ -8,7 +9,8 @@ import pytest
pytest.importorskip('mcp')
from starlette.testclient import TestClient # noqa: E402
-from hermes_mcp.bridge import Activity, BearerAuth, LatestFrame, build_app, build_mcp # noqa: E402
+from hermes_mcp.bridge import (Activity, BearerAuth, FacesBuffer, LatestFrame, build_app, build_mcp, # noqa: E402
+ people_summary)
TOKEN = 'x' * 32
@@ -25,9 +27,11 @@ def tool_names(mcp):
return {t.name for t in asyncio.run(mcp.list_tools())}
-def test_tools_are_look_only_without_a_head_and_keep_their_names_with_one():
- assert tool_names(build_mcp(LatestFrame())) == {'look'}
- assert tool_names(build_mcp(LatestFrame(), head=FakeHead())) == {'look', 'turn_head', 'head_position'}
+def test_tools_have_no_head_tools_without_a_head_and_keep_their_names_with_one():
+ base = {'look', 'faces', 'list_people'}
+ assert tool_names(build_mcp(LatestFrame())) == base
+ assert tool_names(build_mcp(LatestFrame(), head=FakeHead())) == base | {'turn_head', 'head_position'}
+ assert 'enroll' not in tool_names(build_mcp(LatestFrame(), head=FakeHead())) # enrolment is the owner's step
def test_look_returns_the_next_frame_not_a_stale_one():
@@ -37,8 +41,9 @@ def test_look_returns_the_next_frame_not_a_stale_one():
mcp = build_mcp(frames, announce_look=lambda: announced.append(1), look_timeout_s=2.0)
threading.Timer(0.3, lambda: frames.put(b'\xff\xd8fresh')).start()
result = asyncio.run(mcp.call_tool('look', {}))
- image = result.content[0]
+ text, image = result.content
assert announced == [1]
+ assert text.type == 'text' and json.loads(text.text)['faces']['available'] is False # no detector running here
assert image.type == 'image' and image.mime_type == 'image/jpeg'
assert base64.b64decode(image.data) == b'\xff\xd8fresh' # the frame that arrived after the call
@@ -97,3 +102,73 @@ def test_bearer_auth_passes_lifespan_through():
asyncio.run(BearerAuth(inner, TOKEN)({'type': 'lifespan'}, None, None))
assert seen == ['lifespan']
+
+
+def face(track_id, x, name='', sim=0.0, conf=0.9, w=300, h=400):
+ return {'track_id': track_id, 'bbox': {'x': x, 'y': 100, 'w': w, 'h': h}, 'confidence': conf, 'name': name, 'similarity': sim}
+
+
+def call(mcp, name, args=None):
+ return asyncio.run(mcp.call_tool(name, args or {}))
+
+
+def test_faces_reports_identity_null_for_a_stranger_and_never_makes_up_a_name():
+ buf = FacesBuffer()
+ buf.put([face(1, 100, 'Kyle', 0.82), face(2, 600)])
+ out = call(build_mcp(LatestFrame(), faces_buf=buf), 'faces').structured_content
+ assert out['available'] and out['count'] == 2 and out['image_size'] == {'w': 1296, 'h': 972}
+ kyle, stranger = out['faces']
+ assert kyle['identity'] == {'name': 'Kyle', 'similarity': 0.82}
+ assert stranger['identity'] is None # unknown, not guessed
+ assert set(kyle) == {'track_id', 'bbox', 'bbox_norm', 'confidence', 'identity', 'seen_for_s'}
+ assert 'embedding' not in json.dumps(out)
+
+
+def test_track_ids_persist_and_seen_for_grows():
+ buf = FacesBuffer()
+ buf.put([face(7, 100)], t=time.monotonic() - 3.0)
+ buf.put([face(7, 105)])
+ (f,) = buf.summary()['faces']
+ assert f['track_id'] == 7 and 2.5 < f['seen_for_s'] < 3.6
+
+
+def test_faces_says_when_detection_is_not_running_instead_of_reporting_an_empty_room():
+ out = call(build_mcp(LatestFrame(), faces_buf=FacesBuffer()), 'faces').structured_content
+ assert out['available'] is False and 'reason' in out
+ buf = FacesBuffer()
+ buf.put([face(1, 100)], t=time.monotonic() - 30) # stale data is not "now"
+ assert buf.summary(max_age_s=1.5)['available'] is False
+
+
+def test_an_empty_room_is_available_with_zero_faces():
+ buf = FacesBuffer()
+ buf.put([])
+ assert buf.summary()['available'] and buf.summary()['count'] == 0
+
+
+def test_look_can_skip_the_photo():
+ frames, buf = LatestFrame(), FacesBuffer()
+ buf.put([face(1, 100, 'Kyle', 0.8)])
+ frames.put(b'\xff\xd8x')
+ result = call(build_mcp(frames, faces_buf=buf), 'look', {'send_image': 'never'})
+ assert len(result.content) == 1 and result.content[0].type == 'text'
+ assert json.loads(result.content[0].text)['faces']['faces'][0]['identity']['name'] == 'Kyle'
+
+
+def test_list_people_gives_names_and_counts_never_face_data(tmp_path):
+ path = tmp_path / 'faces.json'
+ path.write_text(json.dumps({'Kyle': [[0.1] * 128] * 10, 'Alex': [[0.2] * 128] * 3}))
+ out = call(build_mcp(LatestFrame(), people_path=str(path)), 'list_people').structured_content
+ assert out == {'count': 2, 'people': [{'name': 'Alex', 'samples': 3}, {'name': 'Kyle', 'samples': 10}]}
+ assert people_summary(str(tmp_path / 'missing.json')) == {'count': 0, 'people': []}
+
+
+def test_the_server_can_be_discovered_resources_and_prompts_are_not_empty():
+ mcp = build_mcp(LatestFrame(), hardware={'speakers': False})
+ assert [str(r.uri) for r in asyncio.run(mcp.list_resources())] == ['robot://status']
+ assert {p.name for p in asyncio.run(mcp.list_prompts())} == {'who_is_here', 'look_around'}
+ status = json.loads(asyncio.run(mcp.read_resource('robot://status'))[0].content)
+ assert status['hardware'] == {'camera': True, 'head': False, 'speakers': False}
+ assert status['detects'] == ['faces'] and 'people' in status['not_detected']
+ assert 'embeddings never leave it' in status['recognition']['registry']
+ assert 'turn_head' not in status['tools']
diff --git a/ros/hermes_mcp/test/test_node.py b/ros/hermes_mcp/test/test_node.py
new file mode 100644
index 0000000..1b988ae
--- /dev/null
+++ b/ros/hermes_mcp/test/test_node.py
@@ -0,0 +1,16 @@
+import pytest
+
+rclpy = pytest.importorskip('rclpy')
+
+
+def test_the_node_constructs_without_a_token_and_does_not_serve(monkeypatch):
+ """Declaring a parameter twice raises at construction, and nothing else exercises the node."""
+ monkeypatch.delenv('ROBOT_MCP_TOKEN', raising=False)
+ from hermes_mcp.mcp_node import McpNode
+ rclpy.init()
+ try:
+ node = McpNode()
+ assert node.activity is None # no token: not serving, no heartbeat
+ node.destroy_node()
+ finally:
+ rclpy.shutdown()
diff --git a/ros/hermes_vision/hermes_vision/calibrate.py b/ros/hermes_vision/hermes_vision/calibrate.py
index 2d7420e..704bce9 100644
--- a/ros/hermes_vision/hermes_vision/calibrate.py
+++ b/ros/hermes_vision/hermes_vision/calibrate.py
@@ -44,12 +44,63 @@ def novel_enough(sig: np.ndarray, taken: list[np.ndarray], min_distance: float =
return all(np.linalg.norm(sig - t) >= min_distance for t in taken)
-def solve(obj_pts: list[np.ndarray], img_pts: list[np.ndarray], size: tuple[int, int]):
- """OpenCV calibration. Returns (rms reprojection error in px, camera matrix, distortion)."""
- rms, k, d, _, _ = cv2.calibrateCamera(obj_pts, img_pts, size, None, None)
+def solve(obj_pts: list[np.ndarray], img_pts: list[np.ndarray], size: tuple[int, int], fix_k3: bool = True):
+ """OpenCV calibration. Returns (rms reprojection error in px, camera matrix, distortion).
+
+ k3 is held at zero by default: for a lens this mild (about 57 degrees) it is the term that overfits
+ when the board never reached the corners of the image, giving huge cancelling k1, k2, k3 that look
+ fine where the board was and are wild outside it."""
+ flags = cv2.CALIB_FIX_K3 if fix_k3 else 0
+ rms, k, d, _, _ = cv2.calibrateCamera(obj_pts, img_pts, size, None, None, flags=flags)
return float(rms), k, d.reshape(-1)
+def view_errors(obj_pts, img_pts, k, d) -> np.ndarray:
+ """RMS reprojection error of each view (px) under camera k, d."""
+ errs = []
+ for o, i in zip(obj_pts, img_pts):
+ ok, rv, tv = cv2.solvePnP(o, i, k, d)
+ proj, _ = cv2.projectPoints(o, rv, tv, k, d)
+ errs.append(float(np.sqrt(((proj.reshape(-1, 2) - i.reshape(-1, 2)) ** 2).sum(axis=1).mean())))
+ return np.array(errs)
+
+
+def solve_robust(obj_pts, img_pts, size, fix_k3: bool = True, factor: float = 3.0, min_views: int = 12):
+ """Solve, then drop gross outlier views (a hand-held board blurs while it moves, and a blurred view
+ localises its corners about a pixel wrong) and solve again. A view is an outlier only if its error is
+ over `factor` times the median, so ordinary views are never discarded, and it never goes below
+ min_views. Returns (rms, k, d, indices of the views kept)."""
+ keep = list(range(len(img_pts)))
+ for _ in range(2):
+ rms, k, d = solve([obj_pts[i] for i in keep], [img_pts[i] for i in keep], size, fix_k3)
+ errs = view_errors([obj_pts[i] for i in keep], [img_pts[i] for i in keep], k, d)
+ limit = factor * float(np.median(errs))
+ good = [i for i, e in zip(keep, errs) if e <= limit]
+ if len(good) == len(keep) or len(good) < min_views:
+ break
+ keep = good
+ rms, k, d = solve([obj_pts[i] for i in keep], [img_pts[i] for i in keep], size, fix_k3)
+ return rms, k, d, keep
+
+
+_REGIONS = ('top-left', 'top', 'top-right', 'left', 'centre', 'right', 'bottom-left', 'bottom', 'bottom-right')
+
+
+def coverage(img_pts: list[np.ndarray], size: tuple[int, int]) -> tuple[float, list[str]]:
+ """How much of the image the board's corners have visited: the fraction of a 3 x 3 grid of regions
+ holding at least a few corners, and the names of the empty ones. Distortion is determined at the
+ edges and corners, so an uncovered edge means an unreliable model there."""
+ w, h = size
+ counts = np.zeros((3, 3), int)
+ for pts in img_pts:
+ c = pts.reshape(-1, 2)
+ ix = np.clip((c[:, 0] / w * 3).astype(int), 0, 2)
+ iy = np.clip((c[:, 1] / h * 3).astype(int), 0, 2)
+ np.add.at(counts, (iy, ix), 1)
+ empty = [_REGIONS[r * 3 + c] for r in range(3) for c in range(3) if counts[r, c] < 6]
+ return 1.0 - len(empty) / 9.0, empty
+
+
def camera_info_yaml(name: str, size: tuple[int, int], k: np.ndarray, d: np.ndarray) -> str:
"""The ROS camera_info YAML (the format camera_calibration_parsers reads)."""
w, h = size
@@ -80,8 +131,14 @@ def main() -> int:
p.add_argument('--topic', default='/camera/image_raw/compressed')
p.add_argument('--name', default=DEFAULT_NAME)
p.add_argument('--out-dir', default=os.path.expanduser('~/.ros/camera_info'))
+ p.add_argument('--full-model', action='store_true', help='also fit k3 (only with good coverage of the corners)')
+ p.add_argument('--from-views', metavar='NPZ', help='re-solve the views saved by an earlier run, no camera needed')
a = p.parse_args()
+ if a.from_views:
+ z = np.load(a.from_views)
+ return finish(a, list(z['obj']), list(z['img']), tuple(int(v) for v in z['size']), resave=False)
+
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
@@ -111,7 +168,9 @@ def main() -> int:
taken_sigs.append(sig)
obj_pts.append(obj)
img_pts.append(corners)
- print(f' view {len(img_pts)}/{a.views} taken', flush=True)
+ frac, empty = coverage(img_pts, state['size'])
+ hint = f'; not yet covered: {", ".join(empty)}' if empty else '; whole image covered'
+ print(f' view {len(img_pts)}/{a.views} taken{hint}', flush=True)
node.create_subscription(CompressedImage, a.topic, on_image, 1)
print(f'Hold the {a.cols}x{a.rows}-corner board ({a.square_mm:g} mm squares) in front of the camera; '
@@ -123,15 +182,28 @@ def main() -> int:
if len(img_pts) < 10:
print(f'only {len(img_pts)} usable views; nothing written (need at least 10, ideally {a.views})')
return 1
- size = state['size']
- rms, k, d = solve(obj_pts, img_pts, size)
+ return finish(a, obj_pts, img_pts, state['size'], resave=True)
+
+
+def finish(a, obj_pts, img_pts, size, resave: bool) -> int:
+ """Solve (dropping outlier views), report, and write the YAML."""
+ rms, k, d, keep = solve_robust(obj_pts, img_pts, size, fix_k3=not a.full_model)
+ dropped = sorted(set(range(len(img_pts))) - set(keep))
+ frac, empty = coverage([img_pts[i] for i in keep], size)
name = f'{a.name}_{size[0]}x{size[1]}'
os.makedirs(a.out_dir, exist_ok=True)
path = os.path.join(a.out_dir, name + '.yaml')
with open(path, 'w') as f:
f.write(camera_info_yaml(name, size, k, d))
- print(f'\n{len(img_pts)} views, reprojection error {rms:.3f} px ({"good" if rms < 0.5 else "mediocre: retake with more varied poses"})')
- print(f'focal length {k[0, 0]:.1f} x {k[1, 1]:.1f} px, centre ({k[0, 2]:.0f}, {k[1, 2]:.0f}) of {size[0]}x{size[1]}')
+ if resave:
+ np.savez(os.path.join(a.out_dir, name + '-views.npz'), obj=np.array(obj_pts), img=np.array(img_pts), size=size)
+ print(f'\n{len(keep)} of {len(img_pts)} views used'
+ + (f' (dropped as blurred outliers: {dropped})' if dropped else '')
+ + f', reprojection error {rms:.3f} px ({"good" if rms < 0.5 else "mediocre: retake with more varied, steadier poses"})')
+ print(f'image coverage {frac:.0%}' + (f'; never reached: {", ".join(empty)} (the model is unreliable there: '
+ 'retake with the board in those parts of the image)' if empty else ''))
+ print(f'focal length {k[0, 0]:.1f} x {k[1, 1]:.1f} px, centre ({k[0, 2]:.0f}, {k[1, 2]:.0f}) of {size[0]}x{size[1]}; '
+ f'distortion k1={d[0]:+.3f} k2={d[1]:+.3f} p1={d[2]:+.4f} p2={d[3]:+.4f} k3={d[4]:+.3f}')
print(f'wrote {path}\nrestart the robot to load it: sudo systemctl restart robot')
return 0
diff --git a/ros/hermes_vision/hermes_vision/face_detector_node.py b/ros/hermes_vision/hermes_vision/face_detector_node.py
index 4ff84e2..219e570 100644
--- a/ros/hermes_vision/hermes_vision/face_detector_node.py
+++ b/ros/hermes_vision/hermes_vision/face_detector_node.py
@@ -1,10 +1,13 @@
"""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.
- Detection2D.id is the recognised person's name, or empty if unknown or recognition is
- off. Recognition only runs for people enrolled with hermes_vision.people.
+Publishes: /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in full-size
+ camera pixels (center and size), header from the image. Detection2D.id is the track id
+ (a number as a string, stable while the person stays in view). results[0] is the face
+ itself (class_id 'face', score = detector confidence); results[1] exists only if the person
+ is recognised (class_id = their name, score = cosine similarity, which is not a
+ probability). An unknown face has no results[1]: a name is never made up. Recognition only
+ runs for people enrolled with hermes_vision.people.
/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.
"""
@@ -19,7 +22,8 @@ from sensor_msgs.msg import CompressedImage
from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
from .detect import Face, FaceDetector
-from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer, IdentityCache
+from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer
+from .tracking import FaceTracker
class FaceDetectorNode(Node):
@@ -36,7 +40,7 @@ class FaceDetectorNode(Node):
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)) // self.scale, int(param('detect_max_side', 480)))
- self.ids = IdentityCache(float(param('recognition_period_s', 1.0)))
+ self.tracker = FaceTracker(recheck_s=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))
@@ -63,20 +67,23 @@ class FaceDetectorNode(Node):
self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
return
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))
+ tracks = self.tracker.update(faces, now, (lambda f: self.recognizer.identify(img, f)) if self.recognizer else None)
k = self.scale # published boxes are in full-size camera pixels
out = Detection2DArray(header=msg.header)
- for f, name in zip(faces, names):
- f = Face(f.x * k, f.y * k, f.w * k, f.h * k, f.score)
+ for t in tracks:
+ f = Face(t.face.x * k, t.face.y * k, t.face.w * k, t.face.h * k, t.face.score)
d = Detection2D(header=msg.header)
- d.id = name
+ d.id = str(t.id)
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)
+ if t.name:
+ who = ObjectHypothesisWithPose()
+ who.hypothesis.class_id, who.hypothesis.score = t.name, float(t.similarity)
+ d.results.append(who)
out.detections.append(d)
self.pub.publish(out)
if self.debug_pub:
diff --git a/ros/hermes_vision/hermes_vision/tracking.py b/ros/hermes_vision/hermes_vision/tracking.py
new file mode 100644
index 0000000..edbafce
--- /dev/null
+++ b/ros/hermes_vision/hermes_vision/tracking.py
@@ -0,0 +1,65 @@
+"""Follow faces across frames, so a person keeps the same track id and their identity is not
+re-resolved every frame. No ROS imports.
+
+Faces are matched to existing tracks by box overlap (greedy, best overlap first). A track that is not
+seen for max_age_s is dropped; a face that matches nothing starts a new track. Recognition is the
+expensive step, so a track's identity is re-checked only every recheck_s.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Callable, Optional
+
+from .detect import Face
+from .recognize import iou
+
+
+@dataclass
+class Track:
+ id: int
+ face: Face
+ first: float # when the track began
+ last: float # when it was last matched
+ name: str = '' # '' = unknown (or not checked yet)
+ similarity: float = 0.0 # cosine similarity to the matched person (not a probability)
+ named_at: float = -1e9 # when identity was last resolved
+
+ def age(self, now: float) -> float:
+ return now - self.first
+
+
+class FaceTracker:
+ def __init__(self, iou_min: float = 0.3, max_age_s: float = 1.5, recheck_s: float = 1.0):
+ self.iou_min, self.max_age_s, self.recheck_s = iou_min, max_age_s, recheck_s
+ self._tracks: list[Track] = []
+ self._next_id = 1
+
+ def update(self, faces: list[Face], now: float,
+ identify: Optional[Callable[[Face], tuple[str, float]]] = None) -> list[Track]:
+ """Tracks for `faces` (same order). identify(face) -> (name, similarity) is called only for new
+ tracks and for tracks whose identity is older than recheck_s."""
+ pairs = sorted(((iou(f, t.face), fi, ti) for fi, f in enumerate(faces) for ti, t in enumerate(self._tracks)),
+ reverse=True)
+ face_to_track: dict[int, int] = {}
+ used: set[int] = set()
+ for score, fi, ti in pairs:
+ if score < self.iou_min:
+ break
+ if fi not in face_to_track and ti not in used:
+ face_to_track[fi] = ti
+ used.add(ti)
+ out: list[Track] = []
+ for fi, f in enumerate(faces):
+ if fi in face_to_track:
+ t = self._tracks[face_to_track[fi]]
+ t.face, t.last = f, now
+ else:
+ t = Track(self._next_id, f, now, now)
+ self._next_id += 1
+ self._tracks.append(t)
+ if identify is not None and now - t.named_at >= self.recheck_s:
+ t.name, t.similarity = identify(f)
+ t.named_at = now
+ out.append(t)
+ self._tracks = [t for t in self._tracks if now - t.last <= self.max_age_s]
+ return out
diff --git a/ros/hermes_vision/test/test_calibrate.py b/ros/hermes_vision/test/test_calibrate.py
index df0af9b..ef1042a 100644
--- a/ros/hermes_vision/test/test_calibrate.py
+++ b/ros/hermes_vision/test/test_calibrate.py
@@ -2,7 +2,8 @@ import cv2
import numpy as np
import yaml
-from hermes_vision.calibrate import board_points, camera_info_yaml, novel_enough, solve, view_signature
+from hermes_vision.calibrate import (board_points, camera_info_yaml, coverage, novel_enough, solve, solve_robust,
+ view_signature)
SIZE = (1296, 972)
K_TRUE = np.array([[1100.0, 0, 650.0], [0, 1090.0, 480.0], [0, 0, 1]])
@@ -72,3 +73,43 @@ def test_find_board_detects_a_real_rendered_checkerboard_and_not_a_blank_frame()
corners = find_board(bgr, 9, 6)
assert corners is not None and corners.reshape(-1, 2).shape == (54, 2)
assert find_board(np.full_like(bgr, 128), 9, 6) is None
+
+
+def test_k3_is_held_at_zero_by_default_but_can_be_fitted():
+ obj, views = synthetic_views()
+ _, _, d = solve([obj] * len(views), views, SIZE)
+ assert d[4] == 0.0
+ _, _, d_full = solve([obj] * len(views), views, SIZE, fix_k3=False)
+ assert d_full[4] != 0.0
+
+
+def test_coverage_names_the_regions_the_board_never_reached():
+ w, h = SIZE
+ centre_only = [np.random.default_rng(1).uniform([w * 0.4, h * 0.4], [w * 0.6, h * 0.6], (54, 2)).astype(np.float32)]
+ frac, empty = coverage(centre_only, SIZE)
+ assert frac < 0.2 and 'top-left' in empty and 'bottom-right' in empty and 'centre' not in empty
+ rng = np.random.default_rng(2)
+ everywhere = [rng.uniform([0, 0], [w, h], (200, 2)).astype(np.float32)]
+ assert coverage(everywhere, SIZE) == (1.0, [])
+
+
+def test_blurred_outlier_views_are_dropped_and_the_result_stays_accurate():
+ obj, views = synthetic_views(n=40, seed=9)
+ rng = np.random.default_rng(5)
+ noisy = [v + rng.normal(0, 0.15, v.shape).astype(np.float32) for v in views] # ordinary jitter
+ bad = [v + rng.normal(0, 2.0, v.shape).astype(np.float32) for v in views[:4]] # four blurred views
+ allv = noisy + bad
+ rms_all, k_all, _ = solve([obj] * len(allv), allv, SIZE)
+ rms, k, d, keep = solve_robust([obj] * len(allv), allv, SIZE)
+ assert set(range(len(noisy), len(allv))) <= set(range(len(allv))) - set(keep) # the four bad ones are gone
+ assert len(keep) >= len(noisy) - 2 # the ordinary ones are kept
+ assert rms < rms_all and rms < 0.3
+ assert abs(k[0, 0] - 1100) / 1100 < 0.02
+
+
+def test_it_never_drops_below_the_minimum_number_of_views():
+ obj, views = synthetic_views(n=14, seed=4)
+ rng = np.random.default_rng(1)
+ allv = [v + rng.normal(0, 1.0, v.shape).astype(np.float32) * (i % 2) for i, v in enumerate(views)]
+ _, _, _, keep = solve_robust([obj] * len(allv), allv, SIZE, min_views=len(allv) - 1)
+ assert len(keep) >= len(allv) - 1
diff --git a/ros/hermes_vision/test/test_tracking.py b/ros/hermes_vision/test/test_tracking.py
new file mode 100644
index 0000000..6251d15
--- /dev/null
+++ b/ros/hermes_vision/test/test_tracking.py
@@ -0,0 +1,55 @@
+from hermes_vision.detect import Face
+from hermes_vision.tracking import FaceTracker
+
+
+def f(x, y=100, w=80, h=80):
+ return Face(x, y, w, h, 0.9)
+
+
+def test_a_face_keeps_its_track_id_and_age_grows():
+ tr = FaceTracker()
+ ids = []
+ for i in range(8):
+ (t,) = tr.update([f(100 + 3 * i)], i * 0.25)
+ ids.append(t.id)
+ assert ids == [1] * 8 and abs(t.age(1.75) - 1.75) < 1e-9
+
+
+def test_two_people_get_two_ids_and_keep_them_when_they_swap_order():
+ tr = FaceTracker()
+ a, b = tr.update([f(100), f(500)], 0.0)
+ b2, a2 = tr.update([f(502), f(102)], 0.25) # reported in the other order
+ assert (a.id, b.id) == (a2.id, b2.id) and a.id != b.id
+
+
+def test_a_track_survives_a_short_gap_but_not_a_long_one():
+ tr = FaceTracker(max_age_s=1.5)
+ (t,) = tr.update([f(100)], 0.0)
+ tr.update([], 1.0) # nobody for a second
+ (t2,) = tr.update([f(100)], 1.2)
+ assert t2.id == t.id
+ tr.update([], 1.3)
+ tr.update([], 3.5) # gone for over max_age
+ (t3,) = tr.update([f(100)], 3.6)
+ assert t3.id != t.id
+
+
+def test_identity_is_resolved_for_new_tracks_and_rechecked_once_a_second():
+ calls = []
+ tr = FaceTracker(recheck_s=1.0)
+ ident = lambda face: (calls.append(face), ('Kyle', 0.8))[1]
+ for i in range(8): # 4 Hz for two seconds
+ (t,) = tr.update([f(100 + i)], i * 0.25, ident)
+ assert t.name == 'Kyle' and t.similarity == 0.8
+ assert len(calls) == 2 # at t=0 and again at t=1.0
+
+
+def test_a_stranger_is_unknown_not_named():
+ tr = FaceTracker()
+ (t,) = tr.update([f(100)], 0.0, lambda face: ('', 0.12))
+ assert t.name == '' and t.similarity == 0.12
+
+
+def test_without_an_identifier_nothing_is_named():
+ (t,) = FaceTracker().update([f(100)], 0.0)
+ assert t.name == '' and t.id == 1