diff --git a/dotbot/camera/detection/__init__.py b/dotbot/camera/detection/__init__.py index c0cb2435..6117ea77 100644 --- a/dotbot/camera/detection/__init__.py +++ b/dotbot/camera/detection/__init__.py @@ -4,20 +4,31 @@ """Finding a DotBot on the floor raster a registered camera is warped into. Two stages: `propose` says where a robot-sized object might be, `pose` fits -the board outline to the evidence around one of those points, and `robot` -runs both and reports the result in frame millimetres. +the board outline to the evidence around each of those points, and `robot` +runs both and reports the results in frame millimetres. The detector is tooling for comparing what the camera sees against what the -lighthouse reports. It identifies nothing: one pose per frame, the strongest -candidate, with no association to any robot address. +lighthouse reports. The camera identifies nothing by itself: a pose carries +a robot address only when a lighthouse fix handed to it stands on that +candidate. """ from dotbot.camera.detection.robot import ( Detection, Pose, + Prior, RobotDetector, + RobotFix, frame_pose, wrap180, ) -__all__ = ["Detection", "Pose", "RobotDetector", "frame_pose", "wrap180"] +__all__ = [ + "Detection", + "Pose", + "Prior", + "RobotDetector", + "RobotFix", + "frame_pose", + "wrap180", +] diff --git a/dotbot/camera/detection/pose.py b/dotbot/camera/detection/pose.py index 39d9f369..965764de 100644 --- a/dotbot/camera/detection/pose.py +++ b/dotbot/camera/detection/pose.py @@ -158,15 +158,29 @@ def poly_px(pts_mm, cx, cy, heading_deg, mm_per_px): def render(polys, n, cx, cy, heading_deg, mm_per_px): - """Anti-aliased coverage of one or more polygons on an n x n grid.""" + """Anti-aliased coverage of polygons on an n x n grid, cut to their box. + + Returns `(coverage, x0, y0)`, the box and its top-left in the grid, or + None when nothing lands on the grid. Everything outside the box is zero, + so only the box is drawn. + """ import cv2 # lazy: opencv-python is only required to run the detector - buf = np.zeros((n * SS, n * SS), np.uint8) - for p in polys if isinstance(polys, (list, tuple)) else [polys]: - q = poly_px(p, cx, cy, heading_deg, mm_per_px) - cv2.fillPoly(buf, [np.round((q + 0.5) * SS).astype(np.int32)], 255) - resized = cv2.resize(buf, (n, n), interpolation=cv2.INTER_AREA) - return resized.astype(np.float32) / 255.0 + polys = polys if isinstance(polys, (list, tuple)) else [polys] + qs = [poly_px(p, cx, cy, heading_deg, mm_per_px) for p in polys] + points = np.vstack(qs) + x0 = max(int(np.floor(points[:, 0].min())) - 1, 0) + y0 = max(int(np.floor(points[:, 1].min())) - 1, 0) + x1 = min(int(np.ceil(points[:, 0].max())) + 2, n) + y1 = min(int(np.ceil(points[:, 1].max())) + 2, n) + if x1 <= x0 or y1 <= y0: + return None + buf = np.zeros(((y1 - y0) * SS, (x1 - x0) * SS), np.uint8) + for q in qs: + scaled = np.round((q + 0.5) * SS).astype(np.int32) - [x0 * SS, y0 * SS] + cv2.fillPoly(buf, [scaled.astype(np.int32)], 255) + resized = cv2.resize(buf, (x1 - x0, y1 - y0), interpolation=cv2.INTER_AREA) + return resized.astype(np.float32) / 255.0, x0, y0 def features(bgr, keep_mask=None): @@ -317,12 +331,17 @@ def score(self, c, heading, win=None): if win is None: return -1e9 board, x0, y0 = win - n = self.roi - template = render(OUTLINE_MM, n, c[0] - x0, c[1] - y0, heading, self.mmpp) + rendered = render( + OUTLINE_MM, self.roi, c[0] - x0, c[1] - y0, heading, self.mmpp + ) + if rendered is None: + return -1e9 + template, bx, by = rendered s = float(template.sum()) if s < 1: return -1e9 - return float((board * template).sum()) / np.sqrt(s) + h, w = template.shape + return float((board[by : by + h, bx : bx + w] * template).sum()) / np.sqrt(s) def refine(self, c0, heading0): """The best pose near `(c0, heading0)` as (centre, heading), or None.""" @@ -462,20 +481,14 @@ def pose_one(features_map, reg, mm_per_px, tmpl, fit): return out -def pose_at( - seed_px, - mm_per_px, - features_map, - mask, - tmpl, - fit, - win_mm=110.0, - snap_mm=45.0, -): - """Pose of the robot near `seed_px`. - - Tolerates a seed tens of millimetres off: the region is the mask - component whose centroid is nearest the seed, within `snap_mm`. +def component_at(seed_px, mm_per_px, mask, win_mm=110.0, snap_mm=45.0): + """The robot-sized mask component nearest `seed_px`, as a boolean region. + + The component the seed stands on, when it stands on one big enough; + otherwise, to tolerate a seed tens of millimetres off, the one whose + centroid is nearest the seed, within `snap_mm`. Either is cut to a + window `win_mm` either side of the seed. None when no component + qualifies. """ import cv2 # lazy: opencv-python is only required to run the detector @@ -483,15 +496,27 @@ def pose_at( r = int(round(win_mm / mm_per_px)) x0, y0 = max(0, int(seed_px[0]) - r), max(0, int(seed_px[1]) - r) x1, y1 = min(w, int(seed_px[0]) + r), min(h, int(seed_px[1]) + r) - sub = np.zeros_like(mask) - sub[y0:y1, x0:x1] = mask[y0:y1, x0:x1] + if x1 <= x0 or y1 <= y0: + return None + sub = np.ascontiguousarray(mask[y0:y1, x0:x1]) n, labels, stats, centroids = cv2.connectedComponentsWithStats(sub, 8) + under = labels[ + min(max(int(seed_px[1]) - y0, 0), y1 - y0 - 1), + min(max(int(seed_px[0]) - x0, 0), x1 - x0 - 1), + ] best, best_dist = None, None for k in range(1, n): if stats[k, cv2.CC_STAT_AREA] * mm_per_px**2 < MIN_COMPONENT_MM2: continue + if k == under: + best = k + break dd = ( - float(np.hypot(centroids[k][0] - seed_px[0], centroids[k][1] - seed_px[1])) + float( + np.hypot( + centroids[k][0] + x0 - seed_px[0], centroids[k][1] + y0 - seed_px[1] + ) + ) * mm_per_px ) if dd > snap_mm: @@ -500,4 +525,36 @@ def pose_at( best, best_dist = k, dd if best is None: return None - return pose_one(features_map, labels == best, mm_per_px, tmpl, fit) + region = np.zeros(mask.shape, bool) + region[y0:y1, x0:x1] = labels == best + return region + + +def split_region(region, seeds_px, iterations=10): + """`region` cut into one part per seed, by k-means on its pixel positions. + + For robots standing close enough that the mask joins them into one + component. Each part is the cluster started at its own seed, so the + order of the parts is the order of `seeds_px`. + """ + ys, xs = np.nonzero(region) + points = np.stack([xs, ys], 1).astype(float) + centres = np.asarray(seeds_px, float).copy() + for _ in range(iterations): + nearest = np.argmin( + ((points[:, None, :] - centres[None, :, :]) ** 2).sum(axis=2), axis=1 + ) + moved = centres.copy() + for k in range(len(centres)): + members = points[nearest == k] + if len(members): + moved[k] = members.mean(axis=0) + if np.allclose(moved, centres, atol=0.05): + break + centres = moved + parts = [] + for k in range(len(centres)): + part = np.zeros(region.shape, bool) + part[ys[nearest == k], xs[nearest == k]] = True + parts.append(part) + return parts diff --git a/dotbot/camera/detection/robot.py b/dotbot/camera/detection/robot.py index 7a7f5b3a..cfff2ad5 100644 --- a/dotbot/camera/detection/robot.py +++ b/dotbot/camera/detection/robot.py @@ -1,12 +1,13 @@ # SPDX-FileCopyrightText: 2026-present Inria # SPDX-License-Identifier: BSD-3-Clause -"""One robot found on one warped camera frame, reported in frame millimetres. +"""The robots found on one warped camera frame, reported in frame millimetres. -`RobotDetector.detect` runs the two stages - propose, then fit a pose - and -classifies the result. `frame_pose` is the only place raster pixels and the -detector's own heading become the frame millimetres and the robot -`direction` degrees every other surface speaks. +`RobotDetector.detect` runs the two stages - propose, then fit a pose to +each candidate - and classifies each result. Lighthouse fixes, when the +caller has them, name the candidates they stand on. `frame_pose` is the +only place raster pixels and the detector's own heading become the frame +millimetres and the robot `direction` degrees every other surface speaks. Two heading conventions meet here; `frame_pose` names both and converts. @@ -18,8 +19,9 @@ from __future__ import annotations +import itertools import time -from dataclasses import dataclass +from dataclasses import dataclass, field import numpy as np @@ -32,12 +34,15 @@ OutlineFit, Template, axes, + component_at, features, - pose_at, + pose_one, robot_mask, + split_region, ) from dotbot.camera.detection.propose import as_bgr from dotbot.camera.detection.propose import verify as propose_candidates +from dotbot.robots import robot_geometry # The two signals that stop the estimator reporting a pose it cannot stand # behind: the share of the wide green mass lying toward the nose, and how much @@ -62,6 +67,28 @@ REFUSED = "refused" NONE = "none" +# Robots fitted per frame at most, unless the caller sets its own cap. +MAX_ROBOTS = 2 + +# Wall time one frame may spend fitting poses before the rest wait for the +# next frame. At least one candidate is fitted on every frame, so a slow +# machine still makes progress. +FRAME_BUDGET_MS = 500.0 + +# How far a lighthouse fix may sit from the outline centre of the robot it +# belongs to: the photodiode's own offset ahead of the centre, the 50 mm the +# firmware holds a fix before replacing it, and the fix's own error. +PRIOR_GATE_MM = robot_geometry().diode_ahead_of_centre_mm + 50.0 + 30.0 + +# A mask component larger than this, shared by two lighthouse fixes, is two +# robots touching. One robot and its wheels cover about 11000 mm2 on the +# bench camera, and a robot trailing its programming cable about 16000. +SPLIT_MIN_MM2 = 1.5 * robot_geometry().envelope_mm ** 2 + +# A pose from an earlier frame is re-reported while its robot is still +# proposed, for at most this long. +CARRY_S = 3.0 + @dataclass(frozen=True) class Pose: @@ -74,15 +101,62 @@ class Pose: refined: bool +@dataclass(frozen=True) +class Prior: + """Where the lighthouse last put one robot, in raster pixels.""" + + address: str + point_px: tuple[float, float] + + +@dataclass(frozen=True) +class RobotFix: + """One robot on one frame: its pose, and whose it is when that is known. + + `stamp` is the time of the frame the pose was fitted on, which is an + earlier frame's when the budget left this robot for later. + """ + + status: str + pose: Pose | None + address: str | None = None + stamp: float = 0.0 + + @dataclass(frozen=True) class Detection: - """What one frame yielded: a pose, a refused pose, or nothing.""" + """What one frame yielded: one entry per robot fitted. + + Robots carrying an address come first, then the rest, each by the + strength of its candidate. + """ status: str candidates: int - pose: Pose | None + robots: tuple[RobotFix, ...] elapsed_ms: float + @property + def pose(self) -> Pose | None: + """The strongest robot's pose, for a caller that expects one robot.""" + return next((r.pose for r in self.robots if r.pose is not None), None) + + +@dataclass +class _Target: + """One candidate the frame will report, and what fitting it needs.""" + + seed_px: np.ndarray + z: float + address: str | None + key: object = None + # The seeds of every robot sharing this candidate's mask component, this + # one's first, when lighthouse fixes say more than one robot stands there. + split_seeds: list = field(default_factory=list) + # A robot fitted on another candidate's mask component, which it gives + # up when that component is too small to hold two robots. + guest: bool = False + def wrap180(deg: float) -> float: """`deg` mapped into (-180, 180].""" @@ -90,6 +164,16 @@ def wrap180(deg: float) -> float: return 180.0 if wrapped == -180.0 else wrapped +def frame_status(robots) -> str: + """`found` if any robot was, else `refused` if any was, else `none`.""" + statuses = {r.status for r in robots} + if FOUND in statuses: + return FOUND + if REFUSED in statuses: + return REFUSED + return NONE + + def classify(pose: Pose) -> str: """`found` when both confidence signals clear their floor, else `refused`.""" if pose.green_flare < GREEN_FLARE_MIN: @@ -121,19 +205,33 @@ def _off_sheets(candidates, quads): class RobotDetector: - """The detector one camera runs, holding what is the same every frame. + """The detector one camera runs, holding what outlives one frame. `keep_mask` marks the raster pixels that are floor this camera can see: the proposer ignores everything outside it and the floor statistics are - measured inside it. + measured inside it. `max_robots` caps the robots reported per frame and + `budget_ms` the wall time a frame spends fitting them; the ones a frame + has no time for are fitted first on the next. """ - def __init__(self, mm_per_px: float, keep_mask=None, exclude_sheets: bool = True): + def __init__( + self, + mm_per_px: float, + keep_mask=None, + exclude_sheets: bool = True, + max_robots: int = MAX_ROBOTS, + budget_ms: float = FRAME_BUDGET_MS, + ): self.mm_per_px = float(mm_per_px) self.keep_mask = keep_mask self.exclude_sheets = bool(exclude_sheets) + self.max_robots = max(1, int(max_robots)) + self.budget_ms = float(budget_ms) self._template = Template(self.mm_per_px) self._markers = None + # Last fit per robot key, as (RobotFix, centre_px). + self._fits: dict = {} + self._keys = itertools.count() def sheet_quads(self, bgr) -> list: """The registration pages in this frame, as raster-pixel polygons. @@ -156,9 +254,15 @@ def sheet_quads(self, bgr) -> list: quads.append(centre + (quad - centre) * SHEET_GROW) return quads - def detect(self, bgr) -> Detection: - """The strongest verified candidate on this frame, fitted.""" + def detect(self, bgr, priors=(), stamp: float | None = None) -> Detection: + """Every verified candidate on this frame, up to the cap, fitted. + + `priors` are the lighthouse fixes of the robots that may stand on + this raster; a candidate within `PRIOR_GATE_MM` of one carries its + address. `stamp` is the frame's time, which each fit reports. + """ started = time.perf_counter() + stamp = time.time() if stamp is None else float(stamp) bgr = as_bgr(bgr) # The evidence maps carry the frame's floor-relative chroma, which is # what the colour check reads, so both stages measure one field. @@ -172,18 +276,185 @@ def detect(self, bgr) -> Detection: ] candidates = _off_sheets(candidates, self.sheet_quads(bgr)) if not candidates: - return Detection(NONE, 0, None, _ms_since(started)) - best = max(candidates, key=lambda c: c["z"]) - fitted = pose_at( - best["centre"], - self.mm_per_px, - features_map, - robot_mask(features_map), - self._template, - OutlineFit(features_map, self.mm_per_px), + self._fits = {} + return Detection(NONE, 0, (), _ms_since(started)) + + import cv2 # lazy: opencv-python is only required to run the detector + + targets = self._targets(candidates, priors) + mask = robot_mask(features_map) + # Whole-frame components, so robots sharing one are seen sharing it + # even where a fitting window cuts it. + _, labels = cv2.connectedComponents(mask, connectivity=8) + fit = OutlineFit(features_map, self.mm_per_px) + + # Oldest fit first, so a budget that cannot cover every robot on one + # frame covers each of them in turn. + def age(target): + held = self._fits.get(target.key) + return (held[0].stamp if held else -np.inf, -target.z) + + fits = {} + for target in sorted(targets, key=age): + if fits and (time.perf_counter() - started) * 1000.0 > self.budget_ms: + break + region = self._region(target, targets, mask, labels) + fits[id(target)] = self._fit(target, region, features_map, fit, stamp) + + robots, kept = [], {} + for target in targets: + if id(target) in fits: + fix = fits[id(target)] + if fix.pose is None: + continue + else: + held = self._fits.get(target.key) + if held is None or stamp - held[0].stamp > CARRY_S: + continue + fix = held[0] + kept[target.key] = (fix, target.seed_px) + robots.append(fix) + self._fits = kept + return Detection( + frame_status(robots), len(candidates), tuple(robots), _ms_since(started) + ) + + def _targets(self, candidates, priors) -> list[_Target]: + """The candidates to report, named where a lighthouse fix allows. + + Pairs are taken nearest first, one fix per candidate. A fix left + over within reach of a taken candidate means two robots close enough + to be one blob, so that candidate is fitted once per robot. Named + candidates are kept before unnamed ones, each by response, up to the + cap. + """ + gate = PRIOR_GATE_MM / self.mm_per_px + targets = [ + _Target(np.asarray(c["centre"], float), float(c["z"]), None) + for c in candidates + ] + points = [np.asarray(p.point_px, float) for p in priors] + pairs = sorted( + (float(np.linalg.norm(t.seed_px - q)), ti, pi) + for ti, t in enumerate(targets) + for pi, q in enumerate(points) + ) + taken = match_within(pairs, gate) + used = set(taken.values()) + for ti, pi in taken.items(): + targets[ti].address = priors[pi].address + shared = {} + for dist, ti, pi in pairs: + if dist > gate: + break + if pi in used or ti not in taken: + continue + used.add(pi) + shared.setdefault(ti, [taken[ti]]).append(pi) + for ti, group in shared.items(): + host = targets[ti] + for pi in group: + # Each robot of a shared blob is fitted on its own part, the + # one grown from its own fix, which is listed first. + seeds = [points[pi]] + [points[o] for o in group if o != pi] + if pi == taken[ti]: + host.split_seeds = seeds + else: + targets.append( + _Target( + host.seed_px, + host.z, + priors[pi].address, + split_seeds=seeds, + guest=True, + ) + ) + targets.sort(key=lambda t: (t.address is None, -t.z)) + targets = targets[: self.max_robots] + self._assign_keys(targets) + return targets + + def _assign_keys(self, targets) -> None: + """Key each target to the robot it continues from the last frame. + + A named target is keyed by its address; an unnamed one by the + nearest unnamed fit of the last frame within one robot of it. + """ + reach = robot_geometry().envelope_mm / self.mm_per_px + free = { + key: centre + for key, (fix, centre) in self._fits.items() + if fix.address is None + } + for target in targets: + if target.address is not None: + target.key = ("address", target.address) + continue + nearest = min( + free.items(), + key=lambda kv: np.linalg.norm(kv[1] - target.seed_px), + default=None, + ) + if ( + nearest is not None + and np.linalg.norm(nearest[1] - target.seed_px) < reach + ): + target.key = nearest[0] + del free[nearest[0]] + else: + target.key = ("unnamed", next(self._keys)) + + def _region(self, target, targets, mask, labels): + """The mask pixels `target` is fitted on, or None if there are none. + + Its own component, cut into one part per robot when other robots + share it: another candidate's seed standing on it, or, for robots + only the lighthouse tells apart, a component too big for one robot. + """ + if target.split_seeds: + region = component_at( + target.split_seeds[0], + self.mm_per_px, + mask, + win_mm=PRIOR_GATE_MM + robot_geometry().envelope_mm, + snap_mm=PRIOR_GATE_MM, + ) + if region is None: + return None + if region.sum() * self.mm_per_px**2 < SPLIT_MIN_MM2: + if target.guest and _label_at(labels, target.seed_px) in labels[region]: + return None + return region + return split_region(region, target.split_seeds)[0] + region = component_at(target.seed_px, self.mm_per_px, mask) + if region is None: + return None + h, w = labels.shape + own = np.bincount(labels[region]).argmax() + others = [ + other.seed_px + for other in targets + if other is not target + and not other.split_seeds + and 0 <= int(other.seed_px[1]) < h + and 0 <= int(other.seed_px[0]) < w + and labels[int(other.seed_px[1]), int(other.seed_px[0])] == own + ] + if not others: + return region + # Split the whole component, since the window may have cut the part + # another robot's seed stands on. + return split_region(labels == own, [target.seed_px] + others)[0] + + def _fit(self, target, region, features_map, fit, stamp) -> RobotFix: + """One target's pose, fitted on `region`.""" + fitted = ( + None + if region is None + else pose_one(features_map, region, self.mm_per_px, self._template, fit) ) if fitted is None: - return Detection(NONE, len(candidates), None, _ms_since(started)) + return RobotFix(NONE, None, target.address, stamp) pose = Pose( centre_px=(float(fitted["centre"][0]), float(fitted["centre"][1])), heading_atan2_deg=float(fitted["heading"]), @@ -191,7 +462,7 @@ def detect(self, bgr) -> Detection: tmpl_margin=float(fitted["tmpl_margin"]), refined=bool(fitted["refined"]), ) - return Detection(classify(pose), len(candidates), pose, _ms_since(started)) + return RobotFix(classify(pose), pose, target.address, stamp) def frame_pose(pose: Pose, area: Area, mm_per_px: float) -> dict: @@ -235,5 +506,70 @@ def _mm(point) -> list[float]: return [round(float(point[0]), 1), round(float(point[1]), 1)] +def match_within(pairs, gate: float, exact_max: int = 6) -> dict: + """Candidate to fix, as the most pairs within `gate`, then the least distance. + + `pairs` are `(distance, candidate, fix)` sorted by distance. Candidates + and fixes linked by pairs within the gate are solved group by group, + exactly while a group holds at most `exact_max` candidates and nearest + first above that. + """ + near = [p for p in pairs if p[0] <= gate] + parent: dict = {} + + def root(node): + while parent.setdefault(node, node) != node: + node = parent[node] + return node + + for _, ti, pi in near: + parent[root(("t", ti))] = root(("p", pi)) + groups: dict = {} + for pair in near: + groups.setdefault(root(("t", pair[1])), []).append(pair) + + taken: dict = {} + for group in groups.values(): + candidates = sorted({ti for _, ti, _ in group}) + if len(candidates) > exact_max: + for _, ti, pi in group: + if ti not in taken and pi not in taken.values(): + taken[ti] = pi + continue + options = {ti: [(d, pi) for d, t, pi in group if t == ti] for ti in candidates} + best = (0, 0.0, {}) + + def search(k, used, cost, chosen): + nonlocal best + if k == len(candidates): + if (len(chosen), -cost) > (best[0], -best[1]): + best = (len(chosen), cost, dict(chosen)) + return + if len(chosen) + len(candidates) - k < best[0]: + return + ti = candidates[k] + for d, pi in options[ti]: + if pi not in used: + chosen[ti] = pi + search(k + 1, used | {pi}, cost + d, chosen) + del chosen[ti] + search(k + 1, used, cost, chosen) + + search(0, frozenset(), 0.0, {}) + taken.update(best[2]) + return taken + + +def _label_at(labels, point_px) -> int: + """The component label under `point_px`, clamped onto the raster.""" + h, w = labels.shape + return int( + labels[ + min(max(int(point_px[1]), 0), h - 1), + min(max(int(point_px[0]), 0), w - 1), + ] + ) + + def _ms_since(started: float) -> float: return round((time.perf_counter() - started) * 1000.0, 1) diff --git a/dotbot/camera/rate.py b/dotbot/camera/rate.py new file mode 100644 index 00000000..29404a73 --- /dev/null +++ b/dotbot/camera/rate.py @@ -0,0 +1,86 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# SPDX-License-Identifier: BSD-3-Clause + +"""How often a camera's detector runs: often enough, and never more than its share. + +The detector's cost grows with the robots in view and with whatever else +the machine is doing, so the interval between detections is chosen from +what they have been taking: a running average of the wall time per +detection, divided by the share of one core the detector may hold. More +robots or a busier machine lower the rate, and it recovers when either +goes away. It never rises above the warp rate, since there would be no new +frame to detect on, and never falls below `DETECT_HZ_MIN`. +""" + +from __future__ import annotations + +# The share of one core detection may hold, on average. +DETECT_SHARE = 0.4 + +# Slowest rate the controller will go to, whatever a detection costs. +DETECT_HZ_MIN = 0.5 + +# Weight of the newest detection in the running average of the cost. +SMOOTHING = 0.3 + +# A rate is reported as changed once it moves this far from the last one +# reported, as a fraction of it, so the log carries steps and not jitter. +REPORT_STEP = 0.25 + + +class DetectRate: + """The interval between detection starts that holds them to `share`. + + Times are seconds on whatever clock the caller reads; nothing here + reads one. + """ + + def __init__( + self, + share: float = DETECT_SHARE, + max_hz: float = 10.0, + min_hz: float = DETECT_HZ_MIN, + smoothing: float = SMOOTHING, + ): + if not 0.0 < share <= 1.0: + raise ValueError(f"detection share must be in (0, 1], got {share}") + self.share = float(share) + self.max_hz = float(max_hz) + self.min_hz = min(float(min_hz), self.max_hz) + self.smoothing = float(smoothing) + self.cost_s: float | None = None + self._next = 0.0 + self._reported_hz: float | None = None + + @property + def interval_s(self) -> float: + """Seconds from one detection's start to the next one's.""" + if self.cost_s is None: + return 1.0 / self.max_hz + interval = self.cost_s / self.share + return min(max(interval, 1.0 / self.max_hz), 1.0 / self.min_hz) + + @property + def hz(self) -> float: + return 1.0 / self.interval_s + + def wait_s(self, now: float) -> float: + """How long until the next detection is due, zero if it is.""" + return max(0.0, self._next - now) + + def ran(self, started: float, seconds: float) -> bool: + """Record one detection; True when the rate moved by `REPORT_STEP`.""" + seconds = max(0.0, float(seconds)) + if self.cost_s is None: + self.cost_s = seconds + else: + self.cost_s += self.smoothing * (seconds - self.cost_s) + self._next = started + self.interval_s + hz = self.hz + if ( + self._reported_hz is None + or abs(hz - self._reported_hz) > REPORT_STEP * self._reported_hz + ): + self._reported_hz = hz + return True + return False diff --git a/dotbot/camera/service.py b/dotbot/camera/service.py index 6d587bd3..256dd519 100644 --- a/dotbot/camera/service.py +++ b/dotbot/camera/service.py @@ -17,7 +17,8 @@ A detector runs on a second thread, fed the warped array and never the JPEG. The slot between the two threads holds one frame, so a detector slower than the warp processes every Nth frame and neither the warp nor -the stream ever waits on it. +the stream ever waits on it. How often it runs is `DetectRate`'s choice, +which holds it to a share of one core however many robots are in view. """ from __future__ import annotations @@ -40,7 +41,8 @@ release_capture, settle, ) -from dotbot.camera.detection import RobotDetector, frame_pose +from dotbot.camera.detection import Prior, RobotDetector, frame_pose +from dotbot.camera.detection.robot import MAX_ROBOTS from dotbot.camera.raster import ( MM_PER_PX, WARP_FPS_MAX, @@ -50,6 +52,7 @@ raster_size, raster_transform, ) +from dotbot.camera.rate import DETECT_SHARE, DetectRate from dotbot.camera.registration import CameraCalibration from dotbot.logger import LOGGER @@ -70,6 +73,12 @@ class CameraService: exercisable without a device. `detector` is the same seam for the robot detector, and `on_detection` is called with every record it produces, on the detector's own thread. + + `priors` returns the lighthouse fixes a detection may name robots + from, as `(address, x_mm, y_mm)` in frame millimetres; it is called on + the detector's thread once per detection. `max_robots` caps the robots + one detection reports and `detect_share` the share of one core the + detector holds on average. """ def __init__( @@ -81,6 +90,9 @@ def __init__( detector: RobotDetector | None = None, on_detection: Callable[[dict], None] | None = None, detect: bool = True, + priors: Callable[[], list] | None = None, + max_robots: int = MAX_ROBOTS, + detect_share: float = DETECT_SHARE, ): self.calibration = calibration self.area = area @@ -92,6 +104,7 @@ def __init__( self._lock = threading.Lock() self._jpeg: bytes | None = None self._sequence = 0 + self._stamp = 0.0 self.transform = raster_transform(calibration.matrix, area) self.coverage_mm = coverage_mm( calibration.matrix, calibration.width, calibration.height @@ -104,6 +117,9 @@ def __init__( self._pending: tuple[np.ndarray, int, float] | None = None self._pending_event = threading.Event() self._detection: dict | None = None + self._priors = priors + self.max_robots = max_robots + self.rate = DetectRate(detect_share, max_hz=WARP_FPS_MAX) @property def raster(self) -> tuple[int, int]: @@ -206,7 +222,9 @@ def start(self) -> bool: if self.detect: # Built here rather than in __init__, because it holds the mask. if self._detector is None: - self._detector = RobotDetector(MM_PER_PX, self.keep_mask) + self._detector = RobotDetector( + MM_PER_PX, self.keep_mask, max_robots=self.max_robots + ) self._detect_thread = threading.Thread( target=self._detect_loop, name=f"Camera {self.area.name} detect", @@ -261,12 +279,13 @@ async def parts(self): interval = 1.0 / WARP_FPS_MAX sent = -1 while True: - jpeg, sequence = self.held() + with self._lock: + jpeg, sequence, stamp = self._jpeg, self._sequence, self._stamp if jpeg is None: return if sequence != sent: sent = sequence - yield _part(jpeg) + yield _part(jpeg, stamp) if not self._reading: return await asyncio.sleep(interval) @@ -347,6 +366,7 @@ def _warp(self, frame, stamp: float) -> None: with self._lock: self._jpeg = buffer.tobytes() self._sequence += 1 + self._stamp = stamp self._pending = (warped, self._sequence, stamp) self._pending_event.set() @@ -359,13 +379,28 @@ def _detect_loop(self) -> None: """ try: while self._reading: + # Sleep out the rate's interval first, then take whatever + # warp is newest, so the frame detected on is the latest one. + wait = self.rate.wait_s(time.monotonic()) + if wait > 0: + time.sleep(min(wait, 0.1)) + continue self._pending_event.wait(timeout=0.5) self._pending_event.clear() with self._lock: pending, self._pending = self._pending, None if pending is None: continue + started = time.monotonic() record = self._detected(*pending) + if self.rate.ran(started, time.monotonic() - started): + self.logger.info( + "Camera detection rate changed", + area=self.area.name, + rate_hz=round(self.rate.hz, 2), + cost_ms=round(self.rate.cost_s * 1000.0, 1), + robots=len(record["robots"]) if record else None, + ) if record is None or self._on_detection is None: continue try: @@ -387,7 +422,7 @@ def _detect_loop(self) -> None: def _detected(self, warped, sequence: int, stamp: float) -> dict | None: """One warp's record, held for a late console, or None if it failed.""" try: - detection = self._detector.detect(warped) + detection = self._detector.detect(warped, self._raster_priors(), stamp) record = { "area": self.area.name, "camera_id": self.calibration.id, @@ -396,9 +431,17 @@ def _detected(self, warped, sequence: int, stamp: float) -> dict | None: "status": detection.status, "candidates": detection.candidates, "elapsed_ms": detection.elapsed_ms, + "rate_hz": round(self.rate.hz, 2), + "robots": [ + { + "address": fix.address, + "status": fix.status, + "timestamp": fix.stamp, + "pose": frame_pose(fix.pose, self.area, MM_PER_PX), + } + for fix in detection.robots + ], } - if detection.pose is not None: - record["pose"] = frame_pose(detection.pose, self.area, MM_PER_PX) except Exception as exc: # pylint:disable=broad-except self.logger.warning( "Camera robot detection failed on a frame", @@ -410,12 +453,28 @@ def _detected(self, warped, sequence: int, stamp: float) -> dict | None: self._detection = record return record + def _raster_priors(self) -> list[Prior]: + """The lighthouse fixes the controller holds, in raster pixels.""" + if self._priors is None: + return [] + return [ + Prior( + address, + ((x - self.area.x) / MM_PER_PX, (y - self.area.y) / MM_PER_PX), + ) + for address, x, y in self._priors() + ] + -def _part(jpeg: bytes) -> bytes: - """One JPEG as a part of the multipart stream.""" +def _part(jpeg: bytes, stamp: float) -> bytes: + """One JPEG as a part of the multipart stream. + + `X-Timestamp` is `time.time()` when the frame was read off the device. + """ head = ( f"--{STREAM_BOUNDARY}\r\n" "Content-Type: image/jpeg\r\n" - f"Content-Length: {len(jpeg)}\r\n\r\n" + f"Content-Length: {len(jpeg)}\r\n" + f"X-Timestamp: {stamp:.3f}\r\n\r\n" ) return head.encode("ascii") + jpeg + b"\r\n" diff --git a/dotbot/config.py b/dotbot/config.py index df30204a..179ff987 100644 --- a/dotbot/config.py +++ b/dotbot/config.py @@ -178,6 +178,8 @@ class ControllerSection(_Strict): lh2_calibration: str | None = None camera_calibration: str | None = None camera_detect: bool | None = None + camera_max_robots: int | None = Field(None, ge=1) + camera_detect_share: float | None = Field(None, gt=0.0, le=1.0) background_map: str | None = None log_output: str | None = None csv_data_output: str | None = None diff --git a/dotbot/console-web/src/Inspector.tsx b/dotbot/console-web/src/Inspector.tsx index 39e1d7bb..87c1ea60 100644 --- a/dotbot/console-web/src/Inspector.tsx +++ b/dotbot/console-web/src/Inspector.tsx @@ -2,7 +2,7 @@ import React, { useState } from "react"; import { stateLabel } from "./viewChrome"; -import { LINK_LABEL, UnifiedBot } from "./types"; +import { CameraDetection, CameraRobot, LINK_LABEL, UnifiedBot } from "./types"; // Right-side inspector: the low-level layer next to the map's high-level one. // Renders what `dotbot swarm info` prints, from the same /status payload, and @@ -40,6 +40,25 @@ export function formatHeading(bot: UnifiedBot): string { return `${Math.round(pose.heading_deg)} deg (${pose.heading_source})`; } +/** The robot a camera last fitted under this bot's address, if any did. */ +export function cameraRobotFor( + bot: UnifiedBot, + detections: Record, +): CameraRobot | null { + for (const detection of Object.values(detections)) { + const robot = detection.robots?.find((r) => r.address === bot.id); + if (robot) return robot; + } + return null; +} + +/** The body heading an overhead camera measures, in the direction convention. */ +export function formatCameraHeading(robot: CameraRobot | null): string { + if (!robot) return "not seen"; + const confidence = robot.status === "found" ? "" : ", low confidence"; + return `${Math.round(robot.pose.heading_deg)} deg${confidence}`; +} + const hex32 = (v: number) => `0x${(v >>> 0).toString(16).padStart(8, "0")}`; // FaultType values that actually populate the fault status registers. A @@ -112,7 +131,11 @@ const Row: React.FC<{ k: string; v: string; indent?: boolean; accent?: boolean } ); -const Card: React.FC<{ bot: UnifiedBot }> = ({ bot }) => { +const Card: React.FC<{ bot: UnifiedBot; camera: CameraRobot | null; cameras: boolean }> = ({ + bot, + camera, + cameras, +}) => { const [showRaw, setShowRaw] = useState(false); const [copied, setCopied] = useState(false); const sw = bot.swarmit; @@ -169,6 +192,7 @@ const Card: React.FC<{ bot: UnifiedBot }> = ({ bot }) => { v={bot.position ? `${Math.round(bot.position.x)}, ${Math.round(bot.position.y)}` : "no fix"} /> + {cameras && } {info && ( <> @@ -246,14 +270,29 @@ const Card: React.FC<{ bot: UnifiedBot }> = ({ bot }) => { ); }; -export const InspectorBody: React.FC<{ bots: UnifiedBot[] }> = ({ bots }) => ( -
- {bots.length === 0 ? ( -
- Select a bot to inspect it. -
- ) : ( - bots.map((b) => ) - )} -
-); +// `cameraDetections` is each camera's latest detection, keyed by area; with +// none, the Camera row is left out rather than reading "not seen". +export const InspectorBody: React.FC<{ + bots: UnifiedBot[]; + cameraDetections?: Record; +}> = ({ bots, cameraDetections = {} }) => { + const cameras = Object.keys(cameraDetections).length > 0; + return ( +
+ {bots.length === 0 ? ( +
+ Select a bot to inspect it. +
+ ) : ( + bots.map((b) => ( + + )) + )} +
+ ); +}; diff --git a/dotbot/console-web/src/MapView.tsx b/dotbot/console-web/src/MapView.tsx index daebca46..55d00977 100644 --- a/dotbot/console-web/src/MapView.tsx +++ b/dotbot/console-web/src/MapView.tsx @@ -637,8 +637,10 @@ export const MapView: React.FC = (props) => { area, ); const detection = (props.cameraDetections ?? {})[camera.area]; - const found = detectionStroke(detection); - const pose = detection?.pose; + const drawn = (detection?.robots ?? []).flatMap((robot) => { + const stroke = detectionStroke(robot); + return stroke ? [{ robot, stroke }] : []; + }); return (
= (props) => { the falloff, which dims the image as the extrapolation past the span grows, and by the markers in the photograph underneath. */} - {(hasSpan(camera.coverage_mm) || pose) && ( + {(hasSpan(camera.coverage_mm) || drawn.length > 0) && ( = (props) => { vectorEffect="non-scaling-stroke" /> )} - {/* What the camera makes of the robot standing on this - floor: its board outline and its two tyres, the same - parts the map glyph draws, a line from the centre to - the nose so the heading is readable, and a dot on the - photodiode, which is the point the lighthouse + {/* What the camera makes of each robot standing on + this floor: its board outline and its two tyres, the + same parts the map glyph draws, a line from the centre + to the nose so the heading is readable, and a dot on + the photodiode, which is the point the lighthouse reports and so the one the two can be compared at. */} - {found && pose && ( - - {(pose.wheels_mm ?? []).map((wheel, i) => ( + {drawn.map(({ robot, stroke }, k) => { + const pose = robot.pose; + const id = `${camera.area}-${k}`; + return ( + + {(pose.wheels_mm ?? []).map((wheel, i) => ( + + ))} - ))} - - - - - )} + + + + ); + })} )}
diff --git a/dotbot/console-web/src/RightPane.tsx b/dotbot/console-web/src/RightPane.tsx index f4a356ac..ebccf368 100644 --- a/dotbot/console-web/src/RightPane.tsx +++ b/dotbot/console-web/src/RightPane.tsx @@ -12,7 +12,7 @@ import { robotOpacityFor, } from "./cameraLayer"; import { InspectorBody } from "./Inspector"; -import { DETECTION_TEXT } from "./localization"; +import { detectionText } from "./localization"; import { PanelToggle } from "./PanelToggle"; import type { DrawMode, RobotDrawing } from "./robotDrawing"; import { SetupCard } from "./SetupCard"; @@ -293,9 +293,10 @@ const CameraRow: React.FC<{ detection && (
- {DETECTION_TEXT[detection.status]} + {detectionText(detection)}
) )} @@ -523,7 +524,9 @@ export const RightPane: React.FC = (props) => { /> ))} - {props.tab === "robot" && } + {props.tab === "robot" && ( + + )} {props.tab === "layers" && (
diff --git a/dotbot/console-web/src/cameraLayer.test.tsx b/dotbot/console-web/src/cameraLayer.test.tsx index 9b26c89c..d5098f68 100644 --- a/dotbot/console-web/src/cameraLayer.test.tsx +++ b/dotbot/console-web/src/cameraLayer.test.tsx @@ -36,6 +36,8 @@ import { RightPane, RightTab } from "./RightPane"; import type { Area, CameraDetection, + CameraPose, + CameraRobot, LH2Position, RegisteredCamera, Site, @@ -796,49 +798,69 @@ const WHEELS: number[][][] = [ ], ]; +const POSE: CameraPose = { + centre_mm: [1523.4, 488.1], + photodiode_mm: [1540.2, 511.7], + nose_mm: [1551.0, 526.2], + outline_mm: OUTLINE, + wheels_mm: WHEELS, + heading_deg: -37.5, + heading_atan2_deg: 52.5, + green_flare: 0.83, + tmpl_margin: 0.91, + refined: true, +}; + +const robot = ( + status: CameraRobot["status"] = "found", + address: string | null = "217B829760EBA3E0", + pose: CameraPose = POSE, +): CameraRobot => ({ address, status, timestamp: 1758100000.123, pose }); + const detection = ( status: CameraDetection["status"], - withPose = status !== "none", + robots: CameraRobot[] = status === "none" ? [] : [robot(status)], ): CameraDetection => ({ area: "dev-corner", camera_id: "7b21c0d9f3a1", sequence: 412, timestamp: 1758100000.123, status, - candidates: status === "none" ? 0 : 1, + candidates: robots.length, elapsed_ms: 48.2, - pose: withPose - ? { - centre_mm: [1523.4, 488.1], - photodiode_mm: [1540.2, 511.7], - nose_mm: [1551.0, 526.2], - outline_mm: OUTLINE, - wheels_mm: WHEELS, - heading_deg: -37.5, - heading_atan2_deg: 52.5, - green_flare: 0.83, - tmpl_margin: 0.91, - refined: true, - } - : undefined, + rate_hz: 3.4, + robots, }); +// A second robot 200 mm to the right of the first. +const shifted = (dx: number): CameraPose => { + const move = ([x, y]: number[]) => [x + dx, y]; + return { + ...POSE, + centre_mm: move(POSE.centre_mm) as [number, number], + photodiode_mm: move(POSE.photodiode_mm) as [number, number], + nose_mm: move(POSE.nose_mm) as [number, number], + outline_mm: POSE.outline_mm.map(move), + wheels_mm: POSE.wheels_mm!.map((w) => w.map(move)), + }; +}; + describe("detectionStroke", () => { it("draws a pose it stands behind solid, and one it does not dashed", () => { - expect(detectionStroke(detection("found"))).toEqual({ + expect(detectionStroke(robot("found"))).toEqual({ stroke: "var(--accent)", }); - expect(detectionStroke(detection("refused"))).toEqual({ + expect(detectionStroke(robot("refused"))).toEqual({ stroke: "var(--muted)", dasharray: "6 4", }); }); it("draws nothing when there is no pose to draw", () => { - expect(detectionStroke(detection("none"))).toBeNull(); expect(detectionStroke(undefined)).toBeNull(); - // A status that claims a pose it does not carry draws nothing either. - expect(detectionStroke(detection("found", false))).toBeNull(); + // A robot that claims a pose it does not carry draws nothing either. + const bare = { ...robot("found"), pose: undefined } as unknown as CameraRobot; + expect(detectionStroke(bare)).toBeNull(); }); }); @@ -848,9 +870,9 @@ describe("the detection on the map", () => { , ); const map = screen.getByTestId("map"); - const group = within(map).getByTestId("camera-detection-dev-corner"); + const group = within(map).getByTestId("camera-detection-dev-corner-0"); - const outline = within(map).getByTestId("camera-detection-outline-dev-corner"); + const outline = within(map).getByTestId("camera-detection-outline-dev-corner-0"); expect(outline.getAttribute("points")).toBe( polygonPoints(OUTLINE, DEV_CORNER), ); @@ -861,7 +883,7 @@ describe("the detection on the map", () => { // glyph the lighthouse draws beside it. WHEELS.forEach((wheel, i) => { const tyre = within(map).getByTestId( - `camera-detection-wheel-dev-corner-${i}`, + `camera-detection-wheel-dev-corner-0-${i}`, ); expect(tyre.getAttribute("points")).toBe( polygonPoints(wheel, DEV_CORNER), @@ -872,13 +894,13 @@ describe("the detection on the map", () => { ); }); - const nose = within(map).getByTestId("camera-detection-nose-dev-corner"); + const nose = within(map).getByTestId("camera-detection-nose-dev-corner-0"); expect(nose.getAttribute("x1")).toBe(String(1523.4 - DEV_CORNER.x)); expect(nose.getAttribute("y1")).toBe(String(488.1 - DEV_CORNER.y)); expect(nose.getAttribute("x2")).toBe(String(1551.0 - DEV_CORNER.x)); expect(nose.getAttribute("y2")).toBe(String(526.2 - DEV_CORNER.y)); - const diode = within(map).getByTestId("camera-detection-diode-dev-corner"); + const diode = within(map).getByTestId("camera-detection-diode-dev-corner-0"); expect(diode.getAttribute("cx")).toBe(String(1540.2 - DEV_CORNER.x)); expect(diode.getAttribute("cy")).toBe(String(511.7 - DEV_CORNER.y)); @@ -888,15 +910,16 @@ describe("the detection on the map", () => { }); it("draws the board alone for a pose that carries no tyres", () => { - const found = detection("found"); - const older = { ...found, pose: { ...found.pose!, wheels_mm: undefined } }; + const older = detection("found", [ + robot("found", null, { ...POSE, wheels_mm: undefined }), + ]); render(); const map = screen.getByTestId("map"); expect( - within(map).queryByTestId("camera-detection-wheel-dev-corner-0"), + within(map).queryByTestId("camera-detection-wheel-dev-corner-0-0"), ).toBeNull(); expect( - within(map).getByTestId("camera-detection-outline-dev-corner"), + within(map).getByTestId("camera-detection-outline-dev-corner-0"), ).toBeTruthy(); }); @@ -905,7 +928,7 @@ describe("the detection on the map", () => { , ); const map = screen.getByTestId("map"); - const outline = within(map).getByTestId("camera-detection-outline-dev-corner"); + const outline = within(map).getByTestId("camera-detection-outline-dev-corner-0"); expect(outline.getAttribute("stroke")).toBe("var(--muted)"); expect(outline.getAttribute("stroke-dasharray")).toBe("6 4"); }); @@ -914,7 +937,7 @@ describe("the detection on the map", () => { render(); const map = screen.getByTestId("map"); expect( - within(map).queryByTestId("camera-detection-dev-corner"), + within(map).queryByTestId("camera-detection-dev-corner-0"), ).toBeNull(); }); @@ -929,7 +952,7 @@ describe("the detection on the map", () => { ); const map = screen.getByTestId("map"); expect( - within(map).getByTestId("camera-detection-dev-corner"), + within(map).getByTestId("camera-detection-dev-corner-0"), ).toBeInTheDocument(); expect(within(map).queryByTestId("camera-coverage-dev-corner")).toBeNull(); }); @@ -942,6 +965,34 @@ describe("the detection on the map", () => { ).toBe("robot seen"); }); + it("draws every robot the camera found, each tagged with its address", () => { + render( + , + ); + const map = screen.getByTestId("map"); + const first = within(map).getByTestId("camera-detection-dev-corner-0"); + const second = within(map).getByTestId("camera-detection-dev-corner-1"); + expect(first.getAttribute("data-address")).toBe("217B829760EBA3E0"); + expect(second.getAttribute("data-address")).toBeNull(); + expect( + within(map).getByTestId("camera-detection-outline-dev-corner-1").getAttribute("points"), + ).toBe(polygonPoints(shifted(200).outline_mm, DEV_CORNER)); + expect( + within(map).getByTestId("camera-detection-outline-dev-corner-1").getAttribute("stroke-dasharray"), + ).toBe("6 4"); + expect( + within(screen.getByTestId("pane")).getByTestId("camera-detection-status-dev-corner") + .textContent, + ).toBe("2 robots seen"); + }); + it("says nothing before the first message arrives", () => { render(); const pane = screen.getByTestId("pane"); diff --git a/dotbot/console-web/src/cameraLayer.ts b/dotbot/console-web/src/cameraLayer.ts index 0c9016b1..17809a0d 100644 --- a/dotbot/console-web/src/cameraLayer.ts +++ b/dotbot/console-web/src/cameraLayer.ts @@ -24,7 +24,7 @@ // comparison the layer exists to make. import { loadRecord, store } from "./persisted"; -import type { Area, CameraDetection, LH2Position } from "./types"; +import type { Area, CameraRobot, LH2Position } from "./types"; const OPACITY_KEY = "dotbot.console.cameraOpacity"; const OFFSET_KEY = "dotbot.console.cameraOffset"; @@ -241,7 +241,7 @@ export function polygonPoints(polygon: number[][], area: Area): string { return polygon.map(([x, y]) => `${x - area.x},${y - area.y}`).join(" "); } -/** How one detection's outline is stroked, or null when there is nothing to draw. */ +/** How one robot's outline is stroked, or null when there is nothing to draw. */ export interface DetectionStroke { stroke: string; dasharray?: string; @@ -255,11 +255,11 @@ export interface DetectionStroke { * estimator hesitating instead of seeing an empty floor. */ export function detectionStroke( - detection: CameraDetection | undefined, + robot: CameraRobot | undefined, ): DetectionStroke | null { - if (!detection || !detection.pose) return null; - if (detection.status === "found") return { stroke: "var(--accent)" }; - if (detection.status === "refused") + if (!robot || !robot.pose) return null; + if (robot.status === "found") return { stroke: "var(--accent)" }; + if (robot.status === "refused") return { stroke: "var(--muted)", dasharray: "6 4" }; return null; } diff --git a/dotbot/console-web/src/inspector.test.ts b/dotbot/console-web/src/inspector.test.ts index 22af131e..d5e241af 100644 --- a/dotbot/console-web/src/inspector.test.ts +++ b/dotbot/console-web/src/inspector.test.ts @@ -1,7 +1,13 @@ import { describe, expect, it } from "vitest"; -import { formatLh2, formatUptime, infoText } from "./Inspector"; -import { SwarmitNode, UnifiedBot } from "./types"; +import { + cameraRobotFor, + formatCameraHeading, + formatLh2, + formatUptime, + infoText, +} from "./Inspector"; +import { CameraDetection, SwarmitNode, UnifiedBot } from "./types"; const bot = (over: Partial = {}): UnifiedBot => ({ id: "217B829760EBA3E0", @@ -111,3 +117,32 @@ describe("infoText", () => { expect(hung).toContain("fault WatchdogTimeout"); }); }); + +describe("the camera's heading for the selected robot", () => { + const detection = (address: string | null, status: "found" | "refused") => + ({ + area: "dev-corner", + status, + robots: [{ address, status, timestamp: 1, pose: { heading_deg: -37.4 } }], + }) as unknown as CameraDetection; + + it("reads the robot the camera named with this bot's address", () => { + const robot = cameraRobotFor(bot(), { + "dev-corner": detection("217B829760EBA3E0", "found"), + }); + expect(formatCameraHeading(robot)).toBe("-37 deg"); + }); + + it("says when the camera saw it without standing behind the pose", () => { + const robot = cameraRobotFor(bot(), { + "dev-corner": detection("217B829760EBA3E0", "refused"), + }); + expect(formatCameraHeading(robot)).toBe("-37 deg, low confidence"); + }); + + it("never takes an unnamed robot for this one", () => { + const robot = cameraRobotFor(bot(), { "dev-corner": detection(null, "found") }); + expect(robot).toBeNull(); + expect(formatCameraHeading(robot)).toBe("not seen"); + }); +}); diff --git a/dotbot/console-web/src/localization.test.ts b/dotbot/console-web/src/localization.test.ts index 8fc43a9f..64a4cd08 100644 --- a/dotbot/console-web/src/localization.test.ts +++ b/dotbot/console-web/src/localization.test.ts @@ -183,8 +183,16 @@ describe("the station rows", () => { }); const camera = (area: string) => ({ area }) as RegisteredCamera; -const detection = (area: string, status: CameraDetection["status"]) => - ({ area, status }) as CameraDetection; +const detection = ( + area: string, + status: CameraDetection["status"], + robots = status === "none" ? 0 : 1, +) => + ({ + area, + status, + robots: Array.from({ length: robots }, () => ({ status })), + }) as CameraDetection; describe("the camera rows", () => { it("names each camera's area and what it last saw", () => { @@ -210,6 +218,18 @@ describe("the camera rows", () => { ]); }); + it("counts the robots when there is more than one", () => { + expect( + cameraStatusRows([camera("a"), camera("b")], { + a: detection("a", "found", 3), + b: detection("b", "refused", 2), + }), + ).toEqual([ + { area: "a", label: "3 robots seen" }, + { area: "b", label: "2 robots, low confidence" }, + ]); + }); + it("has nothing to say with no camera registered", () => { expect(cameraStatusRows([], {})).toEqual([]); expect(camerasSummary([])).toBe("none registered"); diff --git a/dotbot/console-web/src/localization.ts b/dotbot/console-web/src/localization.ts index d1ccb86c..89e2105e 100644 --- a/dotbot/console-web/src/localization.ts +++ b/dotbot/console-web/src/localization.ts @@ -90,11 +90,13 @@ export function stationsSummary(session: CalibrationSession | null): string { // --- what the overhead cameras see ----------------------------------------- /** What a camera's own detector last made of the floor it looks at. */ -export const DETECTION_TEXT: Record = { - found: "robot seen", - refused: "robot, low confidence", - none: "no robot", -}; +export function detectionText(detection: CameraDetection): string { + const n = detection.robots.length; + if (detection.status === "none" || n === 0) return "no robot"; + const robots = n === 1 ? "robot" : `${n} robots`; + if (detection.status === "found") return `${robots} seen`; + return `${robots}, low confidence`; +} export interface CameraStatusRow { area: string; @@ -112,7 +114,7 @@ export function cameraStatusRows( const detection = detections[camera.area]; let label: string; if (camera.detect === false) label = "detection off"; - else if (detection) label = DETECTION_TEXT[detection.status]; + else if (detection) label = detectionText(detection); else label = "no frame yet"; return { area: camera.area, label }; }); diff --git a/dotbot/console-web/src/types.ts b/dotbot/console-web/src/types.ts index 77600517..59cd4ce3 100644 --- a/dotbot/console-web/src/types.ts +++ b/dotbot/console-web/src/types.ts @@ -103,9 +103,9 @@ export interface WsNotification { // --- what a camera sees on its own area ------------------------------------ // -// One pose per frame, the strongest candidate, with no association to any -// robot address: the layer exists to put the camera's idea of a robot over -// the lighthouse's, and the reader makes the association by looking. +// One pose per robot the camera found, each named with the address of the +// robot whose lighthouse fix stands on it, or unnamed when none does: the +// layer exists to put the camera's idea of a robot over the lighthouse's. // Every *_mm is frame millimetres, x right and y down, the same frame as an // LH2 position. `centre_mm` is the board outline's centre, which is what the @@ -132,10 +132,20 @@ export interface CameraPose { refined: boolean; } -// `status` is "found" when the estimator stands behind the pose, "refused" -// when it fitted one but a confidence signal did not clear its floor, and -// "none" when there was nothing to fit - in which case there is no pose, so -// key on the status and never on the field's presence. +// One robot of a frame. `status` is "found" when the estimator stands behind +// the pose and "refused" when a confidence signal did not clear its floor. +// `timestamp` is the frame the pose was fitted on, an earlier one than the +// detection's own when that frame ran out of time for this robot. +export interface CameraRobot { + address: string | null; + status: "found" | "refused"; + timestamp: number; + pose: CameraPose; +} + +// The frame's `status` is "found" when any robot's is, "refused" when poses +// were fitted and none was, and "none" when there was nothing to fit, in +// which case `robots` is empty. `rate_hz` is how often the detector runs. export interface CameraDetection { area: string; camera_id: string; @@ -144,7 +154,8 @@ export interface CameraDetection { status: "found" | "refused" | "none"; candidates: number; elapsed_ms: number; - pose?: CameraPose; + rate_hz: number; + robots: CameraRobot[]; } // --- the calibration session the controller owns --------------------------- diff --git a/dotbot/console-web/src/useFleet.test.ts b/dotbot/console-web/src/useFleet.test.ts index eb08da87..aac64794 100644 --- a/dotbot/console-web/src/useFleet.test.ts +++ b/dotbot/console-web/src/useFleet.test.ts @@ -214,6 +214,8 @@ describe("withDetection (one camera's latest view of its own area)", () => { status: "none", candidates: 0, elapsed_ms: 4.2, + rate_hz: 3.4, + robots: [], }); it("keys by area", () => { diff --git a/dotbot/controller.py b/dotbot/controller.py index acd8e17f..a4f7d9e6 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -51,7 +51,9 @@ ) from dotbot.calibration.driver import SessionDriver from dotbot.calibration.lighthouse2 import homography_as_float32 +from dotbot.camera.detection.robot import MAX_ROBOTS from dotbot.camera.raster import WARP_FPS_MAX +from dotbot.camera.rate import DETECT_SHARE from dotbot.camera.service import CameraService from dotbot.csv_data_logger import ( CameraCSVLogger, @@ -105,6 +107,8 @@ INACTIVE_DELAY = 5 # seconds LOST_DELAY = 60 # seconds +# A robot silent this long no longer names what a camera sees. +CAMERA_PRIOR_MAX_AGE_S = 2.0 LH2_POSITION_DISTANCE_THRESHOLD = 20 # mm GPS_POSITION_DISTANCE_THRESHOLD = 5 # meters @@ -152,6 +156,8 @@ class ControllerSettings: lh2_calibration: Optional[str] = None camera_calibration: Optional[str] = None camera_detect: bool = True + camera_max_robots: int = MAX_ROBOTS + camera_detect_share: float = DETECT_SHARE background_map: str = "" headless: bool = False verbose: bool = False @@ -335,6 +341,9 @@ def _start_camera(self, spec: str) -> None: area, detect=self.settings.camera_detect, on_detection=self._on_camera_detection, + priors=lambda: self._lh2_priors(area), + max_robots=self.settings.camera_max_robots, + detect_share=self.settings.camera_detect_share, ) # `start()` hands the detector its first frame before it returns, so # the bookkeeping a detection row needs is in place first and rolled @@ -373,8 +382,30 @@ def _open_camera_log(self, area, camera_id: str) -> None: area=area.name, ) + def _lh2_priors(self, area) -> List[tuple]: + """The lighthouse fixes a camera over `area` may name robots from. + + Runs on the camera's detector thread, reading the robot table the + way `_on_camera_detection` does. The area is grown by one robot, so + a robot whose photodiode sits just outside it still names its body. + A robot silent for `CAMERA_PRIOR_MAX_AGE_S` names nothing. + """ + margin = robot_geometry().envelope_mm + fresh_since = time.time() - CAMERA_PRIOR_MAX_AGE_S + priors = [] + for dotbot in list(self.dotbots.values()): + position = dotbot.lh2_position + if ( + position is not None + and dotbot.last_seen >= fresh_since + and area.x - margin <= position.x <= area.x_max + margin + and area.y - margin <= position.y <= area.y_max + margin + ): + priors.append((dotbot.address, position.x, position.y)) + return priors + def _on_camera_detection(self, record: dict) -> None: - """One detection, logged with the lighthouse's answer for the same floor. + """One detection, logged with the lighthouse's answer for each robot. Runs on the camera's detector thread. `list(dict.values())` is atomic under the GIL and the model fields are reassigned whole, so the robot @@ -384,7 +415,9 @@ def _on_camera_detection(self, record: dict) -> None: if logger is None: return try: - logger.log(record, self._lh2_in_area(record)) + robots = record.get("robots") or [None] + for robot in robots: + logger.log(record, self._lh2_in_area(record, robot), robot) except Exception as exc: # pylint:disable=broad-except self.logger.warning( "Camera detection row not written", @@ -392,42 +425,49 @@ def _on_camera_detection(self, record: dict) -> None: error=str(exc), ) - def _lh2_in_area(self, record: dict) -> Optional[dict]: - """The lighthouse pose of the robot the camera is looking at. + def _lh2_in_area(self, record: dict, robot: Optional[dict]) -> Optional[dict]: + """The lighthouse pose to compare one camera robot against. - Rectangle membership, not tracking: with more than one robot in the - area the nearest to the detected pose is taken and `in_area` says how - many there were, so a row that cannot mean a one-to-one comparison - can be filtered out. `packet_age_s` ages the last packet of any kind - from that robot, not the fix it carries. + The robot the detector named, when it named one; otherwise the one + standing nearest the detected pose, by rectangle membership rather + than tracking. `in_area` says how many robots stood in the area, so + a row that cannot mean a one-to-one comparison can be filtered out. + `packet_age_s` ages the last packet of any kind from that robot, not + the fix it carries. """ area = next( (c.area for c in self.cameras if c.area.name == record.get("area")), None ) if area is None: return None + positions = [(d, d.lh2_position) for d in list(self.dotbots.values())] standing = [ - dotbot - for dotbot in list(self.dotbots.values()) - if dotbot.lh2_position is not None - and area.x <= dotbot.lh2_position.x <= area.x_max - and area.y <= dotbot.lh2_position.y <= area.y_max + (dotbot, position) + for dotbot, position in positions + if position is not None + and area.x <= position.x <= area.x_max + and area.y <= position.y <= area.y_max ] - if not standing: + robot = robot or {} + named = self.dotbots.get(robot.get("address") or "") + named_position = None if named is None else named.lh2_position + if named_position is not None: + chosen, position = named, named_position + elif not standing: return {"in_area": 0} - pose = record.get("pose") or {} - target = pose.get("centre_mm") or area.centre - nearest = min( - standing, - key=lambda d: (d.lh2_position.x - target[0]) ** 2 - + (d.lh2_position.y - target[1]) ** 2, - ) + else: + pose = robot.get("pose") or {} + target = pose.get("centre_mm") or area.centre + chosen, position = min( + standing, + key=lambda s: (s[1].x - target[0]) ** 2 + (s[1].y - target[1]) ** 2, + ) return { - "address": nearest.address, - "x": nearest.lh2_position.x, - "y": nearest.lh2_position.y, - "direction": nearest.direction, - "packet_age_s": round(time.time() - nearest.last_seen, 3), + "address": chosen.address, + "x": position.x, + "y": position.y, + "direction": chosen.direction, + "packet_age_s": round(time.time() - chosen.last_seen, 3), "in_area": len(standing), } diff --git a/dotbot/controller_app.py b/dotbot/controller_app.py index 5ed6afc3..e5dc4de5 100644 --- a/dotbot/controller_app.py +++ b/dotbot/controller_app.py @@ -26,6 +26,8 @@ SWARMIT_URL_DEFAULT, pydotbot_version, ) +from dotbot.camera.detection.robot import MAX_ROBOTS +from dotbot.camera.rate import DETECT_SHARE from dotbot.cli._cfg import from_config from dotbot.cli._conn import ConnError, needs_swarm_id, parse_connection from dotbot.cli._site import site_from_context @@ -266,6 +268,25 @@ def _maybe_scaffold_sim_state(explicit_init_state): "is detected, drawn, pushed to the console or logged." ), ) +@click.option( + "--camera-max-robots", + type=click.IntRange(min=1), + default=None, + help=( + f"The most robots one camera frame reports, {MAX_ROBOTS} by default. " + "Robots whose lighthouse fix stands on a candidate are kept first." + ), +) +@click.option( + "--camera-detect-share", + type=click.FloatRange(min=0.0, min_open=True, max=1.0), + default=None, + help=( + "The share of one CPU core the camera detector may hold on average, " + f"{DETECT_SHARE} by default. The detection rate falls as robots are " + "added or the machine gets busy, and recovers when either goes away." + ), +) @click.option( "-M", "--background-map", @@ -322,6 +343,8 @@ def main( lh2_calibration, camera_calibration, camera_detect, + camera_max_robots, + camera_detect_share, background_map, simulator_init_state, swarmit_url, @@ -375,10 +398,24 @@ def main( camera_detect, detect_source = _resolve_controller_key( "camera_detect", camera_detect, unified, True ) + camera_max_robots, _ = _resolve_controller_key( + "camera_max_robots", camera_max_robots, unified, MAX_ROBOTS + ) + camera_detect_share, _ = _resolve_controller_key( + "camera_detect_share", camera_detect_share, unified, DETECT_SHARE + ) + camera_max_robots = int(camera_max_robots) + camera_detect_share = float(camera_detect_share) if camera_calibration: print( f"Camera detection: {'on' if camera_detect else 'off'} " f"(from {detect_source})" + + ( + f", up to {camera_max_robots} robots, at most " + f"{camera_detect_share:.0%} of a core" + if camera_detect + else "" + ) ) conn = conn if conn is not None else file_data.get("conn") @@ -417,6 +454,8 @@ def main( "lh2_calibration": lh2_calibration, "camera_calibration": camera_calibration, "camera_detect": camera_detect, + "camera_max_robots": camera_max_robots, + "camera_detect_share": camera_detect_share, "background_map": background_map, "simulator_init_state": simulator_init_state, "swarmit_url": swarmit_url, diff --git a/dotbot/csv_data_logger.py b/dotbot/csv_data_logger.py index 4de4fbee..ca500c8d 100644 --- a/dotbot/csv_data_logger.py +++ b/dotbot/csv_data_logger.py @@ -124,14 +124,15 @@ def camera_log_path(csv_data_output: Union[str, Path]) -> Path: class CameraCSVLogger: - """One row per camera detection, with the lighthouse's own answer beside it. + """One row per robot per camera detection, with the lighthouse's answer beside it. A second file rather than more columns on the robot log: that one is - keyed to an address and written when a packet arrives, a detection has - no address and arrives from another thread. Each row copies the latest - lighthouse pose of the one robot standing in the camera's area, which is - what makes a single row enough to draw both poses superimposed. - `timestamp` joins the two files. + written when a packet arrives, a detection arrives from another thread + and may carry no address. Each row copies the latest lighthouse pose of + the robot the detector named, or of the one nearest it when it named + none, which is what makes a single row enough to draw both poses + superimposed. A detection with no robot is one row with no pose. + `timestamp` and `sequence` join the rows of one frame. The sidecar beside the CSV is what says what each column means; it is written from `_sidecar_text` and read back by anyone analysing the log. @@ -145,6 +146,9 @@ class CameraCSVLogger: "status", "candidates", "elapsed_ms", + "cam_address", + "cam_status", + "cam_timestamp", "cam_centre_x_mm", "cam_centre_y_mm", "cam_photodiode_x_mm", @@ -230,13 +234,16 @@ def _check_appendable(self) -> None: "Pass a new --csv-data-output." ) - def log(self, record: dict, lh2: Optional[dict] = None) -> None: - """One detection, and the lighthouse pose it is to be compared with. + def log( + self, record: dict, lh2: Optional[dict] = None, robot: Optional[dict] = None + ) -> None: + """One robot of one detection, and the lighthouse pose to compare it with. - Every detection is a row, `none` included, so a gap in the file is a - gap in the detection and not an absence of robots. + Every detection writes at least one row, `none` included, so a gap in + the file is a gap in the detection and not an absence of robots. """ - pose = record.get("pose") or {} + robot = robot or {} + pose = robot.get("pose") or {} centre = pose.get("centre_mm", (None, None)) photodiode = pose.get("photodiode_mm", (None, None)) lh2 = lh2 or {} @@ -248,6 +255,9 @@ def log(self, record: dict, lh2: Optional[dict] = None) -> None: "status": record.get("status"), "candidates": record.get("candidates"), "elapsed_ms": record.get("elapsed_ms"), + "cam_address": robot.get("address"), + "cam_status": robot.get("status"), + "cam_timestamp": robot.get("timestamp"), "cam_centre_x_mm": centre[0], "cam_centre_y_mm": centre[1], "cam_photodiode_x_mm": photodiode[0], @@ -283,7 +293,7 @@ def _sidecar_text(self) -> str: outline = ", ".join(f"[{float(x)}, {float(y)}]" for x, y in OUTLINE_MM) lines = [ - "schema_version = 1", + "schema_version = 2", 'kind = "camera-detection-log"', f'area = "{self.area}"', f'camera_id = "{self.camera_id}"', @@ -315,6 +325,12 @@ def _sidecar_text(self) -> str: "last accepted fix in every advertisement, so a small age does " 'not mean a fresh fix"', 'timestamp = "time.time() when the frame was read from the device"', + 'cam_address = "the robot whose lighthouse fix stood on this ' + "camera candidate, empty when none did; lh2_* then describe the " + 'robot standing nearest it"', + 'cam_timestamp = "time.time() of the frame this pose was fitted ' + "on, earlier than timestamp when the frame ran out of time and " + 'the pose was carried from an earlier one"', "", "[robot]", f"outline_mm = [{outline}]", diff --git a/dotbot/models.py b/dotbot/models.py index 5dcf3d29..e39f5b9f 100644 --- a/dotbot/models.py +++ b/dotbot/models.py @@ -158,18 +158,31 @@ class DotBotCameraPoseModel(BaseModel): refined: bool = False -class DotBotCameraDetectionModel(BaseModel): - """One camera frame's verdict on whether a robot stands on its area. +class DotBotCameraRobotModel(BaseModel): + """One robot a camera frame found, and whose it is when that is known. + + `address` is the robot whose lighthouse fix stands on this candidate, + or null when none does. `status` is `found` when both confidence + signals clear their floor and `refused` when one does not. `timestamp` + is when the frame the pose was fitted on was read, which is an earlier + frame's than the detection's own when the frame ran out of time. + """ + + address: Optional[str] = None + status: str = "found" + timestamp: float = 0.0 + pose: DotBotCameraPoseModel - `status` is `found` when both confidence signals clear their floor, - `refused` when a pose was fitted but one of them did not, and `none` - when there was nothing to fit, in which case there is no `pose`. - `sequence` is the warp counter, so a detection can be matched to the - frame the stream showed, and `timestamp` is when that frame was read - off the device. - The detector identifies nothing: one pose per frame, the strongest - candidate, with no association to any robot address. +class DotBotCameraDetectionModel(BaseModel): + """One camera frame's verdict on the robots standing on its area. + + `status` is `found` when any robot's pose clears both confidence + signals, `refused` when poses were fitted but none did, and `none` when + there was nothing to fit, in which case `robots` is empty. `sequence` + is the warp counter, so a detection can be matched to the frame the + stream showed, and `timestamp` is when that frame was read off the + device. `rate_hz` is the rate the detector was running at. """ area: str @@ -179,7 +192,8 @@ class DotBotCameraDetectionModel(BaseModel): status: str = "none" candidates: int = 0 elapsed_ms: float = 0.0 - pose: Optional[DotBotCameraPoseModel] = None + rate_hz: float = 0.0 + robots: List[DotBotCameraRobotModel] = [] class DotBotCalibrationReadsModel(BaseModel): diff --git a/dotbot/server.py b/dotbot/server.py index 03731fce..7e4843dc 100644 --- a/dotbot/server.py +++ b/dotbot/server.py @@ -42,6 +42,7 @@ DotBotCalibrationSaveModel, DotBotCalibrationSessionModel, DotBotCalibrationStartModel, + DotBotCameraDetectionModel, DotBotCameraModel, DotBotConnectionModel, DotBotModel, @@ -343,6 +344,21 @@ async def cameras(): ] +@api.get( + path="/controller/cameras/{area}/detection", + response_model=Optional[DotBotCameraDetectionModel], + summary="Return the latest detection of the camera covering one area", + tags=["controller"], +) +async def camera_detection(area: str): + """Camera detection HTTP GET handler; null before the first detection.""" + for camera in api.controller.cameras: + if camera.live and camera.area.name == area: + record = camera.held_detection() + return None if record is None else DotBotCameraDetectionModel(**record) + raise HTTPException(status_code=404, detail=f"No camera covers area {area!r}") + + @api.get( path="/controller/cameras/{area}/stream", response_class=StreamingResponse, diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index c5cdcba7..89386717 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -36,11 +36,7 @@ from dotbot.camera.detection.robot import GREEN_FLARE_MIN, TMPL_MARGIN_MIN, classify from dotbot.camera.sheets import MARKER_DICTIONARY from dotbot.tests.camera_fixtures import DETECTION_AREA as AREA -from dotbot.tests.camera_fixtures import ( - MM_PER_PX, - carpet, - draw_robot, -) +from dotbot.tests.camera_fixtures import MM_PER_PX, carpet, draw_robot # The middle of the default raster, in raster pixels. CENTRE_PX = (125.0, 125.0) @@ -102,6 +98,18 @@ def test_finds_the_robot_in_a_bench_raster(): assert abs(wrap180(detection.pose.heading_atan2_deg - 3.0)) < 3.0 +def test_finds_both_robots_in_the_bench_raster(): + """The cabled robot answers the filter harder, and the other is still found.""" + raster = cv2.imread(str(BENCH_RASTER)) + detection = RobotDetector(MM_PER_PX, budget_ms=float("inf")).detect(raster) + assert detection.candidates == 2 + cabled, free = detection.robots + assert cabled.pose.centre_px == pytest.approx((418.7, 374.6), abs=3.0) + assert free.status == "found" + assert free.pose.centre_px == pytest.approx((117.7, 168.7), abs=3.0) + assert abs(wrap180(free.pose.heading_atan2_deg + 74.6)) < 3.0 + + def test_direction_convention(): """The heading the console draws is the detector's, turned by 90 degrees. @@ -499,3 +507,238 @@ def test_the_nose_signal_holds_when_the_board_is_displaced(): # The lever this replaced swings 25 mm under the same displacement and # dips under its own 8 mm floor; the flare keeps several times its margin. assert min(flares) > 3 * GREEN_FLARE_MIN + + +# --- several robots --------------------------------------------------------- + +# Five robots on a 500 x 500 raster, a metre square at 2 mm/px, each at its +# own heading, in raster pixels and the detector's heading convention. +FLEET = [ + ((90.0, 90.0), 0.0), + ((400.0, 100.0), 90.0), + ((250.0, 250.0), -135.0), + ((100.0, 400.0), 37.0), + ((390.0, 390.0), 180.0), +] + + +def unhurried(**kwargs): + """A detector with no frame budget, so what it finds is not machine speed.""" + kwargs.setdefault("max_robots", len(FLEET)) + return RobotDetector(MM_PER_PX, budget_ms=float("inf"), **kwargs) + + +def fleet_raster(robots): + raster = carpet(500, 500) + for centre, heading in robots: + raster = draw_robot(raster, centre, heading) + return raster + + +def prior_for(address, centre, heading, lag_mm=(12.0, -9.0)): + """The fix a robot advertises: its photodiode, trailing by `lag_mm`.""" + from dotbot.camera.detection import Prior + + _, forward = axes(heading) + point = ( + np.asarray(centre) + + forward * PHOTODIODE_AHEAD_MM / MM_PER_PX + + np.asarray(lag_mm) / MM_PER_PX + ) + return Prior(address, (float(point[0]), float(point[1]))) + + +def by_address(detection): + return {r.address: r for r in detection.robots} + + +def assert_at(fix, centre, heading, atol_px=2.0): + assert fix.status == "found" + assert np.allclose(fix.pose.centre_px, centre, atol=atol_px) + assert abs(wrap180(fix.pose.heading_atan2_deg - heading)) < 3.0 + + +@pytest.mark.parametrize("count", [2, 3, 5]) +def test_several_robots_are_found_each_with_its_own_address(count): + robots = FLEET[:count] + priors = [prior_for(f"bot{i}", c, h) for i, (c, h) in enumerate(robots)] + detection = unhurried().detect(fleet_raster(robots), priors) + + assert detection.status == "found" + assert detection.candidates == count + named = by_address(detection) + assert set(named) == {f"bot{i}" for i in range(count)} + for i, (centre, heading) in enumerate(robots): + assert_at(named[f"bot{i}"], centre, heading) + + +def test_a_robot_with_no_fix_is_still_found_unnamed(): + """The fixes name what they stand on; the proposer finds the rest.""" + robots = FLEET[:3] + priors = [prior_for("bot0", *robots[0])] + detection = unhurried().detect(fleet_raster(robots), priors) + + assert len(detection.robots) == 3 + assert detection.robots[0].address == "bot0" + assert_at(detection.robots[0], *robots[0]) + unnamed = [r for r in detection.robots if r.address is None] + assert len(unnamed) == 2 + for centre, heading in robots[1:]: + (fix,) = (r for r in unnamed if np.allclose(r.pose.centre_px, centre, atol=2.0)) + assert_at(fix, centre, heading) + + +def test_a_fix_on_empty_floor_names_nothing(): + """A fix further than the gate from every candidate names no candidate.""" + robots = FLEET[:1] + far = prior_for("elsewhere", (400.0, 400.0), 0.0) + detection = unhurried().detect(fleet_raster(robots), [far]) + (fix,) = detection.robots + assert fix.address is None + assert_at(fix, *robots[0]) + + +def test_the_cap_keeps_named_robots_first(): + priors = [prior_for("bot4", *FLEET[4]), prior_for("bot2", *FLEET[2])] + detection = unhurried(max_robots=3).detect(fleet_raster(FLEET), priors) + assert detection.candidates == 5 + assert len(detection.robots) == 3 + assert {r.address for r in detection.robots[:2]} == {"bot4", "bot2"} + assert detection.robots[2].address is None + + +def test_one_robot_with_no_fix_is_found_as_before(): + """A single robot and no lighthouse is the whole-frame path on its own.""" + detection = unhurried().detect(fleet_raster(FLEET[3:4])) + (fix,) = detection.robots + assert fix.address is None + assert_at(fix, *FLEET[3]) + assert detection.pose == fix.pose + + +@pytest.mark.parametrize("gap_mm", [30.0, 40.0]) +@pytest.mark.parametrize("headings", [(90.0, 90.0), (0.0, 180.0), (30.0, -60.0)]) +def test_two_robots_a_few_centimetres_apart_are_two_robots(gap_mm, headings): + """The proposer separates them on its own, with no fix to help it.""" + a = (200.0, 250.0) + b = (a[0] + (94.0 + gap_mm) / MM_PER_PX, 250.0) + raster = fleet_raster([(a, headings[0]), (b, headings[1])]) + detection = unhurried().detect(raster) + assert len(detection.robots) == 2 + for centre, heading in ((a, headings[0]), (b, headings[1])): + (fix,) = ( + r + for r in detection.robots + if np.allclose(r.pose.centre_px, centre, atol=2.0) + ) + assert_at(fix, centre, heading) + + +@pytest.mark.parametrize("gap_mm", [0.0, 10.0, 20.0]) +@pytest.mark.parametrize("headings", [(90.0, 90.0), (0.0, 180.0), (30.0, -60.0)]) +def test_two_touching_robots_are_told_apart_by_their_fixes(gap_mm, headings): + """Too close for the proposer, which sees one blob; two fixes split it.""" + a = (200.0, 250.0) + b = (a[0] + (94.0 + gap_mm) / MM_PER_PX, 250.0) + raster = fleet_raster([(a, headings[0]), (b, headings[1])]) + priors = [ + prior_for("a", a, headings[0], lag_mm=(10.0, -16.0)), + prior_for("b", b, headings[1], lag_mm=(-12.0, 8.0)), + ] + named = by_address(unhurried().detect(raster, priors)) + assert set(named) == {"a", "b"} + assert_at(named["a"], a, headings[0]) + assert_at(named["b"], b, headings[1]) + + +def test_a_frame_out_of_time_fits_the_rest_on_the_next(): + """With no budget, one robot per frame, oldest first, the rest carried.""" + robots = FLEET[:3] + raster = fleet_raster(robots) + priors = [prior_for(f"bot{i}", c, h) for i, (c, h) in enumerate(robots)] + detector = RobotDetector(MM_PER_PX, budget_ms=0.0, max_robots=len(FLEET)) + + first = detector.detect(raster, priors, stamp=1.0) + assert [r.stamp for r in first.robots] == [1.0] + + second = detector.detect(raster, priors, stamp=2.0) + assert sorted(r.stamp for r in second.robots) == [1.0, 2.0] + + third = detector.detect(raster, priors, stamp=3.0) + assert sorted(r.stamp for r in third.robots) == [1.0, 2.0, 3.0] + named = by_address(third) + for i, (centre, heading) in enumerate(robots): + assert_at(named[f"bot{i}"], centre, heading) + + # Every robot has been fitted once, so the oldest goes next. + fourth = detector.detect(raster, priors, stamp=4.0) + assert sorted(r.stamp for r in fourth.robots) == [2.0, 3.0, 4.0] + + +def test_a_carried_pose_goes_with_its_robot(): + """A robot no longer proposed is not reported from an older frame.""" + robots = FLEET[:2] + priors = [prior_for(f"bot{i}", c, h) for i, (c, h) in enumerate(robots)] + detector = RobotDetector(MM_PER_PX, budget_ms=0.0, max_robots=len(FLEET)) + detector.detect(fleet_raster(robots), priors, stamp=1.0) + detector.detect(fleet_raster(robots), priors, stamp=2.0) + + alone = detector.detect(fleet_raster(robots[:1]), priors[:1], stamp=3.0) + assert [r.address for r in alone.robots] == ["bot0"] + + +def test_the_outline_is_drawn_in_its_own_box_exactly_as_on_the_whole_grid(): + """Drawing only the box is a saving, never a change in the fit's score.""" + from dotbot.camera.detection.pose import OUTLINE_MM, SS, poly_px, render + + n = 85 + for cx, cy, heading in [(42.0, 42.0, 0.0), (40.3, 47.8, 37.0), (2.0, 80.0, -123.0)]: + whole = np.zeros((n * SS, n * SS), np.uint8) + corners = poly_px(OUTLINE_MM, cx, cy, heading, MM_PER_PX) + cv2.fillPoly(whole, [np.round((corners + 0.5) * SS).astype(np.int32)], 255) + expected = cv2.resize(whole, (n, n), interpolation=cv2.INTER_AREA) / 255.0 + + box, x0, y0 = render(OUTLINE_MM, n, cx, cy, heading, MM_PER_PX) + drawn = np.zeros((n, n)) + drawn[y0 : y0 + box.shape[0], x0 : x0 + box.shape[1]] = box + assert np.array_equal(drawn.astype(np.float32), expected.astype(np.float32)) + + +def test_two_robots_are_named_as_a_whole_not_nearest_pair_first(): + """A's stale fix nearer B than A must not hand B A's name.""" + from dotbot.camera.detection import Prior + + a, b = (200.0, 250.0), (267.0, 250.0) + raster = fleet_raster([(a, 0.0), (b, 0.0)]) + priors = [Prior("a", (240.0, 250.0)), Prior("b", (300.0, 250.0))] + named = by_address(unhurried().detect(raster, priors)) + assert set(named) == {"a", "b"} + assert_at(named["a"], a, 0.0) + assert_at(named["b"], b, 0.0) + + +def test_a_second_fix_on_one_robot_is_not_a_second_robot(): + """A stale fix within reach of a lone robot adds no copy of its pose.""" + from dotbot.camera.detection import Prior + + a = (200.0, 250.0) + priors = [Prior("a", (210.0, 250.0)), Prior("stale", (160.0, 250.0))] + detection = unhurried().detect(fleet_raster([(a, 0.0)]), priors) + assert [r.address for r in detection.robots] == ["a"] + assert_at(detection.robots[0], a, 0.0) + + +def test_matching_takes_the_most_pairs_then_the_least_distance(): + from dotbot.camera.detection.robot import match_within + + pairs = sorted([(27.0, 1, 0), (40.0, 0, 0), (50.0, 1, 1), (117.0, 0, 1)]) + assert match_within(pairs, gate=54.5) == {0: 0, 1: 1} + assert match_within(pairs, gate=54.5, exact_max=0) == {1: 0} + + +def test_the_default_cap_is_two_robots(): + detection = RobotDetector(MM_PER_PX, budget_ms=float("inf")).detect( + fleet_raster(FLEET) + ) + assert detection.candidates == len(FLEET) + assert len(detection.robots) == 2 diff --git a/dotbot/tests/test_camera_rate.py b/dotbot/tests/test_camera_rate.py new file mode 100644 index 00000000..5b01d7a3 --- /dev/null +++ b/dotbot/tests/test_camera_rate.py @@ -0,0 +1,87 @@ +"""Tests for the camera detector's rate, run on a clock the test advances.""" + +import pytest + +from dotbot.camera.rate import DetectRate + + +class Clock: + def __init__(self): + self.now = 0.0 + + +def run(rate, clock, cost_s, frames): + """`frames` detections of `cost_s` each, started as soon as each is due. + + Returns the share of the elapsed time spent detecting. + """ + started_at = clock.now + busy = 0.0 + for _ in range(frames): + clock.now += rate.wait_s(clock.now) + rate.ran(clock.now, cost_s) + clock.now += cost_s + busy += cost_s + clock.now += rate.wait_s(clock.now) + return busy / (clock.now - started_at) + + +def test_a_cheap_detector_runs_at_the_warp_rate(): + rate, clock = DetectRate(0.4, max_hz=10.0), Clock() + run(rate, clock, 0.01, 20) + assert rate.hz == pytest.approx(10.0) + + +@pytest.mark.parametrize("cost_s", [0.12, 0.25, 0.6]) +def test_an_expensive_detector_holds_its_share_of_a_core(cost_s): + rate, clock = DetectRate(0.4, max_hz=10.0), Clock() + run(rate, clock, cost_s, 10) + assert run(rate, clock, cost_s, 20) == pytest.approx(0.4, abs=0.01) + assert rate.hz == pytest.approx(0.4 / cost_s, rel=0.01) + + +def test_the_rate_falls_as_robots_are_added_and_recovers_when_they_leave(): + rate, clock = DetectRate(0.4, max_hz=10.0), Clock() + run(rate, clock, 0.15, 20) + one = rate.hz + run(rate, clock, 0.6, 20) + five = rate.hz + run(rate, clock, 0.15, 20) + assert five < one / 3 + assert rate.hz == pytest.approx(one, rel=0.01) + + +def test_the_rate_never_falls_below_its_floor(): + rate, clock = DetectRate(0.4, max_hz=10.0, min_hz=0.5), Clock() + run(rate, clock, 5.0, 5) + assert rate.hz == pytest.approx(0.5) + assert rate.wait_s(clock.now) == 0.0 + + +def test_one_slow_frame_moves_the_rate_by_a_fraction_of_its_cost(): + rate, clock = DetectRate(0.4, max_hz=10.0, smoothing=0.3), Clock() + run(rate, clock, 0.2, 20) + rate.ran(clock.now, 2.0) + assert rate.cost_s == pytest.approx(0.2 + 0.3 * 1.8) + + +def test_a_change_is_reported_once_per_step_not_per_frame(): + rate, clock = DetectRate(0.4, max_hz=10.0), Clock() + reports = [] + for cost_s in [0.2] * 10 + [0.6] * 10: + clock.now += rate.wait_s(clock.now) + reports.append(rate.ran(clock.now, cost_s)) + clock.now += cost_s + assert reports[0] + assert not any(reports[1:10]) + # The average climbs over a few frames, so the step is reported in a + # few moves rather than one, and then goes quiet. + assert 1 <= sum(reports[10:]) <= 4 + assert not any(reports[16:]) + + +def test_a_share_outside_one_core_is_refused(): + with pytest.raises(ValueError): + DetectRate(0.0) + with pytest.raises(ValueError): + DetectRate(1.5) diff --git a/dotbot/tests/test_camera_service.py b/dotbot/tests/test_camera_service.py index bb751072..99abca92 100644 --- a/dotbot/tests/test_camera_service.py +++ b/dotbot/tests/test_camera_service.py @@ -9,6 +9,7 @@ """ import dataclasses +import time import cv2 import numpy as np @@ -162,11 +163,11 @@ class RecordingDetector: def __init__(self): self.frames = [] - def detect(self, bgr): + def detect(self, bgr, priors=(), stamp=None): from dotbot.camera.detection import Detection self.frames.append(bgr) - return Detection("none", 0, None, 0.0) + return Detection("none", 0, (), 0.0) def test_the_detector_sees_the_uncompressed_warp(synthetic_camera): @@ -228,7 +229,10 @@ def test_a_colour_frame_with_a_robot_is_detected_in_frame_millimetres(): assert record["camera_id"] == calibration.id # The four sheets are in plain view and none of them is a robot. assert record["candidates"] == 1 - pose = record["pose"] + (robot,) = record["robots"] + assert robot["address"] is None + assert robot["timestamp"] == record["timestamp"] + pose = robot["pose"] assert np.allclose(pose["centre_mm"], truth_mm, atol=4.0) assert abs(((pose["heading_atan2_deg"] - heading) + 180) % 360 - 180) < 3.0 assert abs(((pose["heading_deg"] - (heading - 90)) + 180) % 360 - 180) < 3.0 @@ -237,6 +241,83 @@ def test_a_colour_frame_with_a_robot_is_detected_in_frame_millimetres(): assert np.allclose(pose["photodiode_mm"], centre + forward * 29.0, atol=0.1) +def wait_for_robots(service, count, timeout=20.0): + """The first record carrying `count` robots, which a frame budget can + spread over more than one detection.""" + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + record = service.held_detection() + if record is not None and len(record["robots"]) >= count: + return record + time.sleep(0.05) + raise AssertionError(f"no detection carried {count} robots") + + +def test_robots_are_named_from_the_fixes_the_controller_holds(): + """Fixes go in as frame millimetres and come back as addresses. + + Three robots, two with a fix: the pair are named and the third is still + found, unnamed, through the same warp and back to millimetres. + """ + from dotbot.camera.detection.pose import PHOTODIODE_AHEAD_MM, axes + + robots = [ + ((1250.0, 250.0), 0.0), + ((1700.0, 400.0), 120.0), + ((1400.0, 700.0), -60.0), + ] + frame = synthetic_colour_frame(DEV_CORNER, robots) + calibration = registration_for(frame) + + def fix(centre, heading): + _, forward = axes(heading) + return tuple(np.asarray(centre) + forward * PHOTODIODE_AHEAD_MM + (15.0, -10.0)) + + service = CameraService( + calibration, + DEV_CORNER, + open_source=looping(frame), + priors=lambda: [("aa", *fix(*robots[0])), ("bb", *fix(*robots[1]))], + max_robots=3, + ) + assert service.start() + try: + record = wait_for_robots(service, 3) + finally: + service.stop() + + assert record["status"] == "found" + assert record["rate_hz"] > 0 + assert [r["address"] for r in record["robots"]] == ["aa", "bb", None] + for robot, (centre, _) in zip(record["robots"], robots): + assert np.allclose(robot["pose"]["centre_mm"], centre, atol=4.0) + + +def test_the_robot_cap_reaches_the_detector(): + robots = [ + ((1250.0, 250.0), 0.0), + ((1700.0, 400.0), 120.0), + ((1400.0, 700.0), -60.0), + ] + frame = synthetic_colour_frame(DEV_CORNER, robots) + service = CameraService( + registration_for(frame), + DEV_CORNER, + open_source=looping(frame), + max_robots=2, + ) + assert service.start() + try: + wait_for_robots(service, 2) + # Long enough for a third robot to have been fitted, were it allowed. + time.sleep(1.0) + record = service.held_detection() + finally: + service.stop() + assert record["candidates"] == 3 + assert len(record["robots"]) == 2 + + def test_no_robot_reports_none_not_nothing(synthetic_camera): """An empty floor produces records saying so, not an absence of records.""" @@ -254,7 +335,7 @@ def test_no_robot_reports_none_not_nothing(synthetic_camera): assert record is not None assert record["status"] == "none" - assert "pose" not in record + assert record["robots"] == [] assert record["candidates"] == 0 @@ -387,13 +468,13 @@ class BadPoseOnce: def __init__(self): self.calls = 0 - def detect(self, bgr): - from dotbot.camera.detection import Detection + def detect(self, bgr, priors=(), stamp=None): + from dotbot.camera.detection import Detection, RobotFix self.calls += 1 if self.calls == 1: - return Detection("found", 1, object(), 0.0) - return Detection("none", 0, None, 0.0) + return Detection("found", 1, (RobotFix("found", object()),), 0.0) + return Detection("none", 0, (), 0.0) frame = cv2.imread(str(synthetic_camera.source)) service = CameraService( @@ -439,12 +520,12 @@ def __init__(self): self.release = threading.Event() self.entered = threading.Event() - def detect(self, bgr): + def detect(self, bgr, priors=(), stamp=None): from dotbot.camera.detection import Detection self.entered.set() self.release.wait(timeout=5.0) - return Detection("none", 0, None, 0.0) + return Detection("none", 0, (), 0.0) def test_a_stuck_detector_does_not_stall_the_warp(synthetic_camera): diff --git a/dotbot/tests/test_config.py b/dotbot/tests/test_config.py index 25dc123e..14f41195 100644 --- a/dotbot/tests/test_config.py +++ b/dotbot/tests/test_config.py @@ -152,6 +152,17 @@ def test_load_bad_type_rejected(tmp_path): cfg.load_config(path) +@pytest.mark.parametrize( + "line", + ["camera_max_robots = 0", "camera_detect_share = 0.0", "camera_detect_share = 1.5"], +) +def test_load_camera_limit_out_of_range_rejected(tmp_path, line): + path = tmp_path / "dotbot.toml" + path.write_text(f"[run.controller]\n{line}\n") + with pytest.raises(cfg.ConfigError): + cfg.load_config(path) + + # --- deployment selection ------------------------------------------------------ diff --git a/dotbot/tests/test_controller.py b/dotbot/tests/test_controller.py index f1b92a96..7b541043 100644 --- a/dotbot/tests/test_controller.py +++ b/dotbot/tests/test_controller.py @@ -559,17 +559,24 @@ def test_a_camera_calibration_that_resolves_to_nothing_serves_no_layer( "status": "found", "candidates": 1, "elapsed_ms": 48.2, - "pose": { - "centre_mm": [1523.4, 488.1], - "photodiode_mm": [1540.2, 511.7], - "nose_mm": [1551.0, 526.2], - "outline_mm": [[1481.2, 500.3]], - "heading_deg": -37.5, - "heading_atan2_deg": 52.5, - "green_flare": 0.82, - "tmpl_margin": 0.91, - "refined": True, - }, + "robots": [ + { + "address": None, + "status": "found", + "timestamp": 1758100000.123, + "pose": { + "centre_mm": [1523.4, 488.1], + "photodiode_mm": [1540.2, 511.7], + "nose_mm": [1551.0, 526.2], + "outline_mm": [[1481.2, 500.3]], + "heading_deg": -37.5, + "heading_atan2_deg": 52.5, + "green_flare": 0.82, + "tmpl_margin": 0.91, + "refined": True, + }, + } + ], } @@ -694,6 +701,64 @@ def test_two_robots_in_the_area_log_the_nearer_one_and_say_so( assert row["lh2_in_area"] == "2" +def test_a_named_robot_is_logged_against_its_own_fix( + tmp_path, monkeypatch, serial_mock +): + """The detector's address wins over the nearest robot in the area.""" + from dotbot.csv_data_logger import camera_log_path + + csv_output = tmp_path / "run.csv" + controller, _ = _camera_controller(tmp_path, monkeypatch, csv_output) + _settled_camera_log(controller) + controller.dotbots = { + "0000000000000001": _bot("0000000000000001", 1900, 900), + "0000000000000002": _bot("0000000000000002", 1530, 495), + } + named = dict(CAMERA_RECORD["robots"][0], address="0000000000000001") + second = dict(CAMERA_RECORD["robots"][0], address="0000000000000002") + controller._on_camera_detection(dict(CAMERA_RECORD, robots=[named, second])) + for logger in controller.camera_csv_loggers.values(): + logger.close() + + with open(camera_log_path(csv_output), newline="") as handle: + import csv + + rows = list(csv.DictReader(handle))[-2:] + assert [r["cam_address"] for r in rows] == [ + "0000000000000001", + "0000000000000002", + ] + assert [r["lh2_address"] for r in rows] == [ + "0000000000000001", + "0000000000000002", + ] + assert rows[0]["sequence"] == rows[1]["sequence"] + + +def test_a_camera_hands_its_detector_the_fixes_in_and_near_its_area( + tmp_path, monkeypatch, serial_mock +): + """A fix one robot outside the area still names a body standing inside, + and a robot gone silent names nothing.""" + from dotbot.controller import CAMERA_PRIOR_MAX_AGE_S + + controller, _ = _camera_controller(tmp_path, monkeypatch, None) + _settled_camera_log(controller) + silent = _bot("0000000000000004", 1500, 500) + silent.last_seen = time.time() - CAMERA_PRIOR_MAX_AGE_S - 1.0 + controller.dotbots = { + "0000000000000001": _bot("0000000000000001", 1500, 500), + "0000000000000002": _bot("0000000000000002", 950, 500), + "0000000000000003": _bot("0000000000000003", 100, 100), + "0000000000000004": silent, + } + area = controller.cameras[0].area + assert sorted(a for a, _, _ in controller._lh2_priors(area)) == [ + "0000000000000001", + "0000000000000002", + ] + + def test_no_csv_output_means_no_camera_log(tmp_path, monkeypatch, serial_mock): controller, _ = _camera_controller(tmp_path, monkeypatch, None) try: diff --git a/dotbot/tests/test_csv_data_logger.py b/dotbot/tests/test_csv_data_logger.py index a7a4ab27..d26716e3 100644 --- a/dotbot/tests/test_csv_data_logger.py +++ b/dotbot/tests/test_csv_data_logger.py @@ -15,17 +15,24 @@ "status": "found", "candidates": 1, "elapsed_ms": 48.2, - "pose": { - "centre_mm": [1523.4, 488.1], - "photodiode_mm": [1540.2, 511.7], - "nose_mm": [1551.0, 526.2], - "outline_mm": [[1481.2, 500.3]], - "heading_deg": -37.5, - "heading_atan2_deg": 52.5, - "green_flare": 0.82, - "tmpl_margin": 0.91, - "refined": True, - }, + "robots": [ + { + "address": "0000000000000001", + "status": "found", + "timestamp": 1758100000.123, + "pose": { + "centre_mm": [1523.4, 488.1], + "photodiode_mm": [1540.2, 511.7], + "nose_mm": [1551.0, 526.2], + "outline_mm": [[1481.2, 500.3]], + "heading_deg": -37.5, + "heading_atan2_deg": 52.5, + "green_flare": 0.82, + "tmpl_margin": 0.91, + "refined": True, + }, + } + ], } NOTHING = { @@ -62,7 +69,7 @@ def test_the_camera_log_sits_beside_the_robot_log(): def test_a_found_detection_writes_every_column(tmp_path): path = camera_log_path(tmp_path / "run.csv") logger = CameraCSVLogger(path, area="dev-corner", camera_id="22248be43bde6d93") - logger.log(FOUND, LH2) + logger.log(FOUND, LH2, FOUND["robots"][0]) logger.close() with open(path, newline="") as handle: @@ -81,6 +88,8 @@ def test_a_found_detection_writes_every_column(tmp_path): assert row["lh2_travel_direction_deg"] == "315" assert row["lh2_packet_age_s"] == "0.42" assert row["lh2_in_area"] == "1" + assert row["cam_address"] == "0000000000000001" + assert row["cam_status"] == "found" assert all(row[name] != "" for name in CameraCSVLogger.FIELDNAMES) @@ -102,7 +111,7 @@ def test_a_detection_of_nothing_is_a_row_too(tmp_path): def test_rows_append_to_an_existing_file(tmp_path): path = camera_log_path(tmp_path / "run.csv") first = CameraCSVLogger(path, area="dev-corner") - first.log(FOUND, LH2) + first.log(FOUND, LH2, FOUND["robots"][0]) first.close() second = CameraCSVLogger(path, area="dev-corner") second.log(NOTHING, None) @@ -192,7 +201,7 @@ def test_a_re_registered_camera_still_appends(tmp_path): """`camera_id` is on every row, so a new registration is recoverable.""" path = camera_log_path(tmp_path / "run.csv") first = CameraCSVLogger(path, area="dev-corner", camera_id="1111111111111111") - first.log(FOUND, LH2) + first.log(FOUND, LH2, FOUND["robots"][0]) first.close() second = CameraCSVLogger(path, area="dev-corner", camera_id="2222222222222222") second.log(NOTHING, None) diff --git a/dotbot/tests/test_server.py b/dotbot/tests/test_server.py index bb9f745e..a7a249d1 100644 --- a/dotbot/tests/test_server.py +++ b/dotbot/tests/test_server.py @@ -1,5 +1,6 @@ import asyncio import contextlib +import time from unittest.mock import AsyncMock, MagicMock import httpx @@ -1209,6 +1210,9 @@ async def test_the_camera_stream_carries_the_area_warped_into_its_raster( ) parts = stream_parts(response.content) assert parts + # Stamped when the frame was read off the device, so a reader can age it. + stamp = float(parts[0][0].split(b"X-Timestamp: ")[1].split(b"\r\n")[0]) + assert time.time() - 60.0 < stamp <= time.time() raster = cv2.imdecode(np.frombuffer(parts[0][1], np.uint8), cv2.IMREAD_COLOR) assert raster.shape == (500, 500, 3) @@ -1304,6 +1308,23 @@ async def test_the_camera_stream_404s_for_an_area_no_camera_covers(synthetic_cam assert "annex" in response.json()["detail"] +@pytest.mark.asyncio +async def test_the_latest_detection_is_served_over_rest(synthetic_camera): + """The same record the WebSocket carries, for a script that polls.""" + with registered(synthetic_camera) as (service, started): + assert started + assert wait_for_detection(service) is not None + response = await client.get("/controller/cameras/dev-corner/detection") + missing = await client.get("/controller/cameras/annex/detection") + + assert response.status_code == 200 + body = response.json() + assert body["area"] == "dev-corner" + assert body["status"] == "none" + assert body["robots"] == [] + assert missing.status_code == 404 + + @pytest.mark.asyncio async def test_a_camera_registered_on_the_bench_describes_itself(real_camera): """The bench's own file: an integer source, and a real residual.""" @@ -1401,21 +1422,22 @@ class CannedDetector: def __init__(self, status="found"): self.status = status - def detect(self, bgr): - from dotbot.camera.detection import Detection, Pose + def detect(self, bgr, priors=(), stamp=None): + from dotbot.camera.detection import Detection, Pose, RobotFix if self.status == "none": - return Detection("none", 0, None, 1.0) + return Detection("none", 0, (), 1.0) + pose = Pose( + centre_px=(250.0, 250.0), + heading_atan2_deg=52.5, + green_flare=0.82, + tmpl_margin=0.91, + refined=True, + ) return Detection( self.status, 1, - Pose( - centre_px=(250.0, 250.0), - heading_atan2_deg=52.5, - green_flare=0.82, - tmpl_margin=0.91, - refined=True, - ), + (RobotFix(self.status, pose, "0000000000000001", stamp or 0.0),), 12.5, ) @@ -1468,8 +1490,13 @@ async def test_a_detection_reaches_the_console_once_per_warp(synthetic_camera): assert message["elapsed_ms"] == 12.5 assert message["sequence"] >= 1 assert message["timestamp"] > 0 + assert message["rate_hz"] > 0 - pose = message["pose"] + (robot,) = message["robots"] + assert robot["address"] == "0000000000000001" + assert robot["status"] == "found" + assert robot["timestamp"] == message["timestamp"] + pose = robot["pose"] assert pose["heading_atan2_deg"] == 52.5 assert pose["heading_deg"] == -37.5 assert len(pose["outline_mm"]) == 14 @@ -1482,13 +1509,12 @@ async def test_a_detection_reaches_the_console_once_per_warp(synthetic_camera): @pytest.mark.asyncio async def test_a_detection_of_nothing_carries_no_pose(synthetic_camera): - """`model_dump(exclude_none=True)` drops a null pose, so status is the key.""" with detecting(synthetic_camera, status="none") as controller: await controller._push_camera_detections() notification = controller.notify_clients.await_args[0][0] message = notification.model_dump(exclude_none=True)["camera_detection"] assert message["status"] == "none" - assert "pose" not in message + assert message["robots"] == [] @pytest.mark.asyncio