From 0b23c74486a5405cbb6baaf40a03900f678f6ddd Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 15:57:52 +0200 Subject: [PATCH 01/11] dotbot/detection: draw the outline fit only inside the outline's box AI-assisted: Claude Opus 5.5 --- dotbot/camera/detection/pose.py | 39 ++++++++++++++++++++------- dotbot/tests/test_camera_detection.py | 17 ++++++++++++ 2 files changed, 46 insertions(+), 10 deletions(-) diff --git a/dotbot/camera/detection/pose.py b/dotbot/camera/detection/pose.py index 39d9f369..b8cb690e 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.""" diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index c5cdcba7..d37d9b2c 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -499,3 +499,20 @@ 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 + + +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)) From 52c1010587e43196a1859a87cf127334f959c5fa Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 16:00:52 +0200 Subject: [PATCH 02/11] dotbot/detection: fit every candidate on a frame, named from LH2 fixes AI-assisted: Claude Opus 5.5 --- dotbot/camera/detection/__init__.py | 21 +- dotbot/camera/detection/pose.py | 74 ++++-- dotbot/camera/detection/robot.py | 314 ++++++++++++++++++++++++-- dotbot/tests/test_camera_detection.py | 193 ++++++++++++++++ dotbot/tests/test_camera_service.py | 10 +- dotbot/tests/test_server.py | 22 +- 6 files changed, 570 insertions(+), 64 deletions(-) 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 b8cb690e..965764de 100644 --- a/dotbot/camera/detection/pose.py +++ b/dotbot/camera/detection/pose.py @@ -481,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 @@ -502,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: @@ -519,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..c2cce05e 100644 --- a/dotbot/camera/detection/robot.py +++ b/dotbot/camera/detection/robot.py @@ -1,10 +1,11 @@ # 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 +`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. @@ -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 = 5 + +# 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,59 @@ 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) + def wrap180(deg: float) -> float: """`deg` mapped into (-180, 180].""" @@ -90,6 +161,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 +202,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 +251,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 +273,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, used = {}, set() + for dist, ti, pi in pairs: + if dist > gate: + break + if ti in taken or pi in used: + continue + taken[ti] = pi + used.add(pi) + 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, None, seeds) + ) + 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: + 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 +459,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: diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index d37d9b2c..2e4bda1f 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -102,6 +102,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. @@ -501,6 +513,187 @@ def test_the_nose_signal_holds_when_the_board_is_displaced(): 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.""" + 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) + + 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) + 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 diff --git a/dotbot/tests/test_camera_service.py b/dotbot/tests/test_camera_service.py index bb751072..73b7b3fe 100644 --- a/dotbot/tests/test_camera_service.py +++ b/dotbot/tests/test_camera_service.py @@ -166,7 +166,7 @@ def detect(self, bgr): 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): @@ -388,12 +388,12 @@ def __init__(self): self.calls = 0 def detect(self, bgr): - from dotbot.camera.detection import Detection + 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( @@ -444,7 +444,7 @@ def detect(self, bgr): 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_server.py b/dotbot/tests/test_server.py index bb9f745e..65b88d9e 100644 --- a/dotbot/tests/test_server.py +++ b/dotbot/tests/test_server.py @@ -1402,22 +1402,18 @@ def __init__(self, status="found"): self.status = status def detect(self, bgr): - from dotbot.camera.detection import Detection, Pose + from dotbot.camera.detection import Detection, Pose, RobotFix if self.status == "none": - return Detection("none", 0, None, 1.0) - 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, - ), - 12.5, + 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, (RobotFix(self.status, pose),), 12.5) @contextlib.contextmanager From 75456cbbf8cc08e517c33fae600017b53a6f9fa2 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 16:00:52 +0200 Subject: [PATCH 03/11] dotbot/camera: add a detection rate that holds a share of one core AI-assisted: Claude Opus 5.5 --- dotbot/camera/rate.py | 86 +++++++++++++++++++++++++++++++ dotbot/tests/test_camera_rate.py | 87 ++++++++++++++++++++++++++++++++ 2 files changed, 173 insertions(+) create mode 100644 dotbot/camera/rate.py create mode 100644 dotbot/tests/test_camera_rate.py 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/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) From 3fdc8a34bb5670e4385099dc6e68d623efc8b0aa Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 16:02:44 +0200 Subject: [PATCH 04/11] dotbot: report every robot a camera sees, paced to a share of a core Breaking: a camera detection record carries `robots`, a list of {address, status, timestamp, pose}, in place of the single `pose`. The camera CSV writes one row per robot with cam_address, cam_status and cam_timestamp under schema_version 2, so an older log is not appended to. AI-assisted: Claude Opus 5.5 --- dotbot/camera/service.py | 65 +++++++++++++++++++-- dotbot/config.py | 2 + dotbot/controller.py | 76 +++++++++++++++++------- dotbot/controller_app.py | 39 +++++++++++++ dotbot/csv_data_logger.py | 40 +++++++++---- dotbot/models.py | 36 ++++++++---- dotbot/server.py | 16 ++++++ dotbot/tests/test_camera_service.py | 82 ++++++++++++++++++++++++-- dotbot/tests/test_controller.py | 86 ++++++++++++++++++++++++---- dotbot/tests/test_csv_data_logger.py | 37 +++++++----- dotbot/tests/test_server.py | 36 ++++++++++-- 11 files changed, 429 insertions(+), 86 deletions(-) diff --git a/dotbot/camera/service.py b/dotbot/camera/service.py index 6d587bd3..15c786df 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 @@ -104,6 +116,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 +221,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", @@ -359,13 +376,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 +419,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 +428,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", @@ -411,6 +451,19 @@ def _detected(self, warped, sequence: int, stamp: float) -> dict | None: 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.""" head = ( diff --git a/dotbot/config.py b/dotbot/config.py index df30204a..3b541d1d 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 = None + camera_detect_share: float | None = None background_map: str | None = None log_output: str | None = None csv_data_output: str | None = None diff --git a/dotbot/controller.py b/dotbot/controller.py index acd8e17f..9ab28137 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -51,6 +51,8 @@ ) 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.rate import DETECT_SHARE from dotbot.camera.raster import WARP_FPS_MAX from dotbot.camera.service import CameraService from dotbot.csv_data_logger import ( @@ -152,6 +154,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 +339,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 +380,25 @@ 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. + """ + margin = robot_geometry().envelope_mm + return [ + (dotbot.address, dotbot.lh2_position.x, dotbot.lh2_position.y) + for dotbot in list(self.dotbots.values()) + if dotbot.lh2_position is not None + and dotbot.status != DotBotStatus.LOST + and area.x - margin <= dotbot.lh2_position.x <= area.x_max + margin + and area.y - margin <= dotbot.lh2_position.y <= area.y_max + margin + ] + 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 +408,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,14 +418,15 @@ 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 @@ -413,21 +440,26 @@ def _lh2_in_area(self, record: dict) -> Optional[dict]: and area.x <= dotbot.lh2_position.x <= area.x_max and area.y <= dotbot.lh2_position.y <= area.y_max ] - if not standing: + robot = robot or {} + named = self.dotbots.get(robot.get("address") or "") + if named is not None and named.lh2_position is not None: + chosen = named + 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 = min( + standing, + key=lambda d: (d.lh2_position.x - target[0]) ** 2 + + (d.lh2_position.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": chosen.lh2_position.x, + "y": chosen.lh2_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..619025cb 100644 --- a/dotbot/controller_app.py +++ b/dotbot/controller_app.py @@ -28,6 +28,8 @@ ) from dotbot.cli._cfg import from_config from dotbot.cli._conn import ConnError, needs_swarm_id, parse_connection +from dotbot.camera.detection.robot import MAX_ROBOTS +from dotbot.camera.rate import DETECT_SHARE from dotbot.cli._site import site_from_context from dotbot.controller import Controller, ControllerSettings from dotbot.logger import setup_logging @@ -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_service.py b/dotbot/tests/test_camera_service.py index 73b7b3fe..f9f89367 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,7 +163,7 @@ 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) @@ -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,74 @@ 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]))], + ) + 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 +326,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,7 +459,7 @@ class BadPoseOnce: def __init__(self): self.calls = 0 - def detect(self, bgr): + def detect(self, bgr, priors=(), stamp=None): from dotbot.camera.detection import Detection, RobotFix self.calls += 1 @@ -439,7 +511,7 @@ 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() diff --git a/dotbot/tests/test_controller.py b/dotbot/tests/test_controller.py index f1b92a96..9d71246c 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,63 @@ 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.""" + from dotbot.models import DotBotStatus + + controller, _ = _camera_controller(tmp_path, monkeypatch, None) + _settled_camera_log(controller) + lost = _bot("0000000000000004", 1500, 500) + lost.status = DotBotStatus.LOST + controller.dotbots = { + "0000000000000001": _bot("0000000000000001", 1500, 500), + "0000000000000002": _bot("0000000000000002", 950, 500), + "0000000000000003": _bot("0000000000000003", 100, 100), + "0000000000000004": lost, + } + 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 65b88d9e..927c1bfa 100644 --- a/dotbot/tests/test_server.py +++ b/dotbot/tests/test_server.py @@ -1304,6 +1304,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,7 +1418,7 @@ class CannedDetector: def __init__(self, status="found"): self.status = status - def detect(self, bgr): + def detect(self, bgr, priors=(), stamp=None): from dotbot.camera.detection import Detection, Pose, RobotFix if self.status == "none": @@ -1413,7 +1430,12 @@ def detect(self, bgr): tmpl_margin=0.91, refined=True, ) - return Detection(self.status, 1, (RobotFix(self.status, pose),), 12.5) + return Detection( + self.status, + 1, + (RobotFix(self.status, pose, "0000000000000001", stamp or 0.0),), + 12.5, + ) @contextlib.contextmanager @@ -1464,8 +1486,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 @@ -1478,13 +1505,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 From 9a1f1ff23552ecee14410aafa96bc91f618cdab7 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Wed, 23 Sep 2026 16:02:53 +0200 Subject: [PATCH 05/11] dotbot/console-web: draw every camera robot and its heading in the inspector AI-assisted: Claude Opus 5.5 --- dotbot/console-web/src/Inspector.tsx | 65 ++++++++--- dotbot/console-web/src/MapView.tsx | 110 +++++++++--------- dotbot/console-web/src/RightPane.tsx | 9 +- dotbot/console-web/src/cameraLayer.test.tsx | 117 ++++++++++++++------ dotbot/console-web/src/cameraLayer.ts | 12 +- dotbot/console-web/src/inspector.test.ts | 39 ++++++- dotbot/console-web/src/localization.test.ts | 24 +++- dotbot/console-web/src/localization.ts | 14 ++- dotbot/console-web/src/types.ts | 27 +++-- dotbot/console-web/src/useFleet.test.ts | 2 + 10 files changed, 295 insertions(+), 124 deletions(-) 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", () => { From 6b14c28036c421acdba2acad6cf9b22eeb7667b2 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:28:13 +0200 Subject: [PATCH 06/11] dotbot/detection: match fixes to candidates as a whole, not greedily A greedy nearest-first pairing lets a lagging fix take the neighbour's candidate and leaves the robot's own fix to split that neighbour, so two robots swap names. A fix sharing a component too small for two robots now names nothing instead of copying its host's pose. AI-assisted: Claude Opus 5.5 --- dotbot/camera/detection/robot.py | 98 +++++++++++++++++++++++---- dotbot/tests/test_camera_detection.py | 46 ++++++++++--- 2 files changed, 118 insertions(+), 26 deletions(-) diff --git a/dotbot/camera/detection/robot.py b/dotbot/camera/detection/robot.py index c2cce05e..9dcdac3b 100644 --- a/dotbot/camera/detection/robot.py +++ b/dotbot/camera/detection/robot.py @@ -5,9 +5,9 @@ `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. +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. @@ -153,6 +153,9 @@ class _Target: # 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: @@ -336,14 +339,8 @@ def _targets(self, candidates, priors) -> list[_Target]: for ti, t in enumerate(targets) for pi, q in enumerate(points) ) - taken, used = {}, set() - for dist, ti, pi in pairs: - if dist > gate: - break - if ti in taken or pi in used: - continue - taken[ti] = pi - used.add(pi) + taken = match_within(pairs, gate) + used = set(taken.values()) for ti, pi in taken.items(): targets[ti].address = priors[pi].address shared = {} @@ -364,7 +361,13 @@ def _targets(self, candidates, priors) -> list[_Target]: host.split_seeds = seeds else: targets.append( - _Target(host.seed_px, host.z, priors[pi].address, None, seeds) + _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] @@ -419,6 +422,8 @@ def _region(self, target, targets, mask, labels): 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) @@ -446,9 +451,7 @@ def _fit(self, target, region, features_map, fit, stamp) -> RobotFix: fitted = ( None if region is None - else pose_one( - features_map, region, self.mm_per_px, self._template, fit - ) + else pose_one(features_map, region, self.mm_per_px, self._template, fit) ) if fitted is None: return RobotFix(NONE, None, target.address, stamp) @@ -503,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/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index 2e4bda1f..96750dbb 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) @@ -587,9 +583,7 @@ def test_a_robot_with_no_fix_is_still_found_unnamed(): 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) - ] + (fix,) = [r for r in unnamed if np.allclose(r.pose.centre_px, centre, atol=2.0)] assert_at(fix, centre, heading) @@ -605,9 +599,7 @@ def test_a_fix_on_empty_floor_names_nothing(): 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 - ) + 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"} @@ -709,3 +701,35 @@ def test_the_outline_is_drawn_in_its_own_box_exactly_as_on_the_whole_grid(): 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} From def710039f8d52d68504ea57e2ed17bf05f5d82c Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:30:56 +0200 Subject: [PATCH 07/11] dotbot/controller: name camera robots only from robots still advertising A robot is marked lost only after 60 s of silence, and until then its last fix could name whichever robot now stands where it was. Each fix is also read once, since the position is replaced whole by another thread. AI-assisted: Claude Opus 5.5 --- dotbot/controller.py | 50 +++++++++++++++++++-------------- dotbot/tests/test_controller.py | 11 ++++---- 2 files changed, 35 insertions(+), 26 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 9ab28137..a4f7d9e6 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -52,8 +52,8 @@ 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.rate import DETECT_SHARE 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, @@ -107,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 @@ -386,16 +388,21 @@ def _lh2_priors(self, area) -> List[tuple]: 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 - return [ - (dotbot.address, dotbot.lh2_position.x, dotbot.lh2_position.y) - for dotbot in list(self.dotbots.values()) - if dotbot.lh2_position is not None - and dotbot.status != DotBotStatus.LOST - and area.x - margin <= dotbot.lh2_position.x <= area.x_max + margin - and area.y - margin <= dotbot.lh2_position.y <= area.y_max + margin - ] + 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 each robot. @@ -433,31 +440,32 @@ def _lh2_in_area(self, record: dict, robot: Optional[dict]) -> Optional[dict]: ) 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 ] robot = robot or {} named = self.dotbots.get(robot.get("address") or "") - if named is not None and named.lh2_position is not None: - chosen = named + 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} else: pose = robot.get("pose") or {} target = pose.get("centre_mm") or area.centre - chosen = min( + chosen, position = min( standing, - key=lambda d: (d.lh2_position.x - target[0]) ** 2 - + (d.lh2_position.y - target[1]) ** 2, + key=lambda s: (s[1].x - target[0]) ** 2 + (s[1].y - target[1]) ** 2, ) return { "address": chosen.address, - "x": chosen.lh2_position.x, - "y": chosen.lh2_position.y, + "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/tests/test_controller.py b/dotbot/tests/test_controller.py index 9d71246c..7b541043 100644 --- a/dotbot/tests/test_controller.py +++ b/dotbot/tests/test_controller.py @@ -738,18 +738,19 @@ def test_a_named_robot_is_logged_against_its_own_fix( 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.""" - from dotbot.models import DotBotStatus + """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) - lost = _bot("0000000000000004", 1500, 500) - lost.status = DotBotStatus.LOST + 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": lost, + "0000000000000004": silent, } area = controller.cameras[0].area assert sorted(a for a, _, _ in controller._lh2_priors(area)) == [ From 5e8951878419586098dd5dc056618b4905858b9b Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:30:56 +0200 Subject: [PATCH 08/11] dotbot/config: bound camera_max_robots and camera_detect_share AI-assisted: Claude Opus 5.5 --- dotbot/config.py | 4 ++-- dotbot/tests/test_config.py | 17 +++++++++++++---- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/dotbot/config.py b/dotbot/config.py index 3b541d1d..179ff987 100644 --- a/dotbot/config.py +++ b/dotbot/config.py @@ -178,8 +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 = None - camera_detect_share: float | 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/tests/test_config.py b/dotbot/tests/test_config.py index 25dc123e..39a68553 100644 --- a/dotbot/tests/test_config.py +++ b/dotbot/tests/test_config.py @@ -87,8 +87,7 @@ def test_load_none_is_empty(): def test_load_valid(tmp_path): path = tmp_path / "dotbot.toml" - path.write_text( - """ + path.write_text(""" default_deployment = "inria" conn = "mqtts://broker.local:8883" swarm_id = "0001" @@ -104,8 +103,7 @@ def test_load_valid(tmp_path): [run.controller] http_port = 8000 -""" - ) +""") config = cfg.load_config(path) assert config.default_deployment == "inria" assert config.fw.board == "dotbot-v3" @@ -152,6 +150,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 ------------------------------------------------------ From 4f95bbbbc61960af4db07f3197a2fae48c8141e9 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:30:56 +0200 Subject: [PATCH 09/11] dotbot/detection: report two robots per camera frame by default AI-assisted: Claude Opus 5.5 --- dotbot/camera/detection/robot.py | 2 +- dotbot/tests/test_camera_detection.py | 13 +++++++++++-- dotbot/tests/test_camera_service.py | 13 +++++++++++-- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/dotbot/camera/detection/robot.py b/dotbot/camera/detection/robot.py index 9dcdac3b..cfff2ad5 100644 --- a/dotbot/camera/detection/robot.py +++ b/dotbot/camera/detection/robot.py @@ -68,7 +68,7 @@ NONE = "none" # Robots fitted per frame at most, unless the caller sets its own cap. -MAX_ROBOTS = 5 +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 diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index 96750dbb..dfa0f4d0 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -524,6 +524,7 @@ def test_the_nose_signal_holds_when_the_board_is_displaced(): 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) @@ -655,7 +656,7 @@ def test_a_frame_out_of_time_fits_the_rest_on_the_next(): 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) + 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] @@ -678,7 +679,7 @@ 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) + 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) @@ -733,3 +734,11 @@ def test_matching_takes_the_most_pairs_then_the_least_distance(): 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_service.py b/dotbot/tests/test_camera_service.py index f9f89367..99abca92 100644 --- a/dotbot/tests/test_camera_service.py +++ b/dotbot/tests/test_camera_service.py @@ -261,7 +261,11 @@ def test_robots_are_named_from_the_fixes_the_controller_holds(): """ 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)] + 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) @@ -274,6 +278,7 @@ def fix(centre, heading): DEV_CORNER, open_source=looping(frame), priors=lambda: [("aa", *fix(*robots[0])), ("bb", *fix(*robots[1]))], + max_robots=3, ) assert service.start() try: @@ -289,7 +294,11 @@ def fix(centre, heading): 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)] + 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), From 8230d6d055ff7a04ee3ad3fcb2c1f2fc1d383b8e Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:34:27 +0200 Subject: [PATCH 10/11] dotbot/camera: stamp each stream frame with its capture time A reader slower than the stream that keeps every part builds a backlog in the socket buffers, seconds deep, and stamping at receipt hides it. The X-Timestamp header lets a reader age each frame and drop stale ones. AI-assisted: Claude Opus 5.5 --- dotbot/camera/service.py | 18 ++++++++++++------ dotbot/tests/test_server.py | 4 ++++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/dotbot/camera/service.py b/dotbot/camera/service.py index 15c786df..256dd519 100644 --- a/dotbot/camera/service.py +++ b/dotbot/camera/service.py @@ -104,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 @@ -278,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) @@ -364,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() @@ -450,7 +453,6 @@ 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: @@ -464,11 +466,15 @@ def _raster_priors(self) -> list[Prior]: ] -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/tests/test_server.py b/dotbot/tests/test_server.py index 927c1bfa..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) From 20af47de1da83b9d09e7ec211cb62e41d7e52561 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 08:36:39 +0200 Subject: [PATCH 11/11] dotbot: apply black, isort and pyupgrade to the multi-robot camera files AI-assisted: Claude Opus 5.5 --- dotbot/controller_app.py | 4 ++-- dotbot/tests/test_camera_detection.py | 6 +++--- dotbot/tests/test_config.py | 6 ++++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/dotbot/controller_app.py b/dotbot/controller_app.py index 619025cb..e5dc4de5 100644 --- a/dotbot/controller_app.py +++ b/dotbot/controller_app.py @@ -26,10 +26,10 @@ SWARMIT_URL_DEFAULT, pydotbot_version, ) -from dotbot.cli._cfg import from_config -from dotbot.cli._conn import ConnError, needs_swarm_id, parse_connection 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 from dotbot.controller import Controller, ControllerSettings from dotbot.logger import setup_logging diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index dfa0f4d0..89386717 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -584,7 +584,7 @@ def test_a_robot_with_no_fix_is_still_found_unnamed(): 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)] + (fix,) = (r for r in unnamed if np.allclose(r.pose.centre_px, centre, atol=2.0)) assert_at(fix, centre, heading) @@ -626,11 +626,11 @@ def test_two_robots_a_few_centimetres_apart_are_two_robots(gap_mm, headings): detection = unhurried().detect(raster) assert len(detection.robots) == 2 for centre, heading in ((a, headings[0]), (b, headings[1])): - (fix,) = [ + (fix,) = ( r for r in detection.robots if np.allclose(r.pose.centre_px, centre, atol=2.0) - ] + ) assert_at(fix, centre, heading) diff --git a/dotbot/tests/test_config.py b/dotbot/tests/test_config.py index 39a68553..14f41195 100644 --- a/dotbot/tests/test_config.py +++ b/dotbot/tests/test_config.py @@ -87,7 +87,8 @@ def test_load_none_is_empty(): def test_load_valid(tmp_path): path = tmp_path / "dotbot.toml" - path.write_text(""" + path.write_text( + """ default_deployment = "inria" conn = "mqtts://broker.local:8883" swarm_id = "0001" @@ -103,7 +104,8 @@ def test_load_valid(tmp_path): [run.controller] http_port = 8000 -""") +""" + ) config = cfg.load_config(path) assert config.default_deployment == "inria" assert config.fw.board == "dotbot-v3"