dotbot/cli: add calibrate-lh2 show --next-round

dotbot/calibration/conics.py

@@ -728,23 +728,28 @@ def solve_calibration(
     )
     solved_from = "conics-joint" if joint.joint else "conics-free"
     anchor = FREE_FRAME_ANCHOR
+    rectangles, valid_mm = dict(joint.rectangles), joint.valid_mm
     if joint.anchored:
         solved_from, anchor = "conics-anchored", site.anchor
+        # In the site's frame, every station's rectangle is the site extent
+        if site.valid_mm is not None:
+            valid_mm = site.valid_mm
+            rectangles = {station: valid_mm for station in rectangles}
     stations = [
         StationSolution(
             index=station,
             homography=[[float(v) for v in row] for row in H],
             points=sum(t.points for t in joint.solutions[station].tracks),
             residual_mm=joint.station_rms_mm[station],
             solved_from=solved_from,
-            valid_mm=joint.rectangles[station],
+            valid_mm=rectangles[station],
         )
         for station, H in sorted(joint.homographies.items())
     ]
     now = datetime.datetime.now(datetime.timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ")
     calibration = Calibration(
         site=Site(name=site.name, anchor=anchor),
-        valid_mm=joint.valid_mm,
+        valid_mm=valid_mm,
         stations=stations,
         tracks=list(samples),
         links=sorted(joint.graph.links, key=lambda k: (k.a, k.b)),
@@ -792,8 +797,9 @@ def self_defined_site(
     The site is the starter site (`starter_layout`) around the robots'
     field (`spin_field`): a margin of floor round it, or a `size_mm` site
     with it centred, and a staging strip below it. The calibration is
-    shifted by the field's offset, each station's rectangle with it, and a
-    tag gets `-<name>` appended, so the two files answer to different tags.
+    shifted by the field's offset, each station's rectangle becomes the
+    site extent, and a tag gets `-<name>` appended, so the two files answer
+    to different tags.
     """
     if (
         not calibration.stations
@@ -828,12 +834,7 @@ def self_defined_site(
                 points=st.points,
                 residual_mm=st.residual_mm,
                 solved_from=st.solved_from,
-                valid_mm=(
-                    st.valid_mm[0] + dx,
-                    st.valid_mm[1] + dy,
-                    st.valid_mm[2] + dx,
-                    st.valid_mm[3] + dy,
-                ),
+                valid_mm=(0, 0, int(extent[0]), int(extent[1])),
             )
         )
     site = Site(

dotbot/calibration/driver.py

@@ -198,7 +198,11 @@ async def push(
                     client.send_lh2_calibration, payload, check.send_to
                 )
                 stale = await asyncio.to_thread(
-                    push_worklist, client, session.saved, check.addresses
+                    push_worklist,
+                    client,
+                    session.saved,
+                    check.addresses,
+                    check.info_gens,
                 )
             finally:
                 await asyncio.to_thread(self._listen_for_buttons, "")

dotbot/calibration/multi_station.py

@@ -56,6 +56,7 @@
     "StationGraph",
     "error_map_from_calibration",
     "multi_station_report",
+    "next_round",
     "solve_joint",
     "station_graph",
 ]
@@ -1188,6 +1189,117 @@ def appendable_rounds(calibration: Calibration) -> int:
     return max(t.round for t in calibration.tracks) + 1
 
 
+# --- Active collection ------------------------------------------------------
+
+# Least distance between two suggested spins, mm: closer ones tell the
+# solve little more than one.
+NEXT_ROUND_SPACING_MM = 400.0
+# How far inside an overlap its suggested ends sit, mm.
+NEXT_ROUND_INSET_MM = 150.0
+
+
+@dataclass(frozen=True)
+class SpinTarget:
+    """Where one robot should spin in the next round, and why."""
+
+    point_mm: tuple[float, float]
+    stations: tuple[int, ...]
+    why: str
+
+
+def _covering(
+    rectangles: Mapping[int, Sequence[int]], p, inset: float = 0.0
+) -> tuple[int, ...]:
+    """The stations whose rectangle, shrunk by `inset`, holds `p`."""
+    return tuple(
+        s
+        for s, (x0, y0, x1, y1) in sorted(rectangles.items())
+        if x0 + inset <= p[0] <= x1 - inset and y0 + inset <= p[1] <= y1 - inset
+    )
+
+
+def next_round(
+    error_map: ErrorMap,
+    rectangles: Mapping[int, Sequence[int]],
+    links: Sequence[LinkRecord],
+    robots: int = 4,
+    spacing_mm: float = NEXT_ROUND_SPACING_MM,
+) -> list[SpinTarget]:
+    """Where `robots` robots should spin next to strengthen the calibration.
+
+    First the two far ends of every overlap whose link is under the advised
+    strength (or missing), since a spread pair is what ties two stations;
+    then the cells the error map predicts worst, no two closer than
+    `spacing_mm` and none on a rectangle's edge, where a spinning robot
+    would leave it. Points are in the calibration's frame.
+    """
+    targets: list[SpinTarget] = []
+
+    def free(p) -> bool:
+        return all(math.dist(p, t.point_mm) >= spacing_mm for t in targets)
+
+    strength = {(k.a, k.b): k for k in links}
+    stations = sorted(rectangles)
+    for i, a in enumerate(stations):
+        for b in stations[i + 1 :]:
+            ra, rb = rectangles[a], rectangles[b]
+            ox0, oy0 = max(ra[0], rb[0]), max(ra[1], rb[1])
+            ox1, oy1 = min(ra[2], rb[2]), min(ra[3], rb[3])
+            if (
+                ox1 - ox0 <= 2 * NEXT_ROUND_INSET_MM
+                or oy1 - oy0 <= 2 * NEXT_ROUND_INSET_MM
+            ):
+                continue
+            link = strength.get((a, b))
+            if link is not None and link_advised(link):
+                continue
+            inset = NEXT_ROUND_INSET_MM
+            if ox1 - ox0 >= oy1 - oy0:
+                mid = (oy0 + oy1) / 2
+                ends = [(ox0 + inset, mid), (ox1 - inset, mid)]
+            else:
+                mid = (ox0 + ox1) / 2
+                ends = [(mid, oy0 + inset), (mid, oy1 - inset)]
+            why = f"ties {station_label(a)} and {station_label(b)}, " + (
+                f"now {link.shared} circle(s) over {link.spread_mm:.0f} mm"
+                if link is not None
+                else "not tied yet"
+            )
+            for p in ends:
+                if len(targets) < robots and free(p):
+                    targets.append(SpinTarget(p, _covering(rectangles, p), why))
+    sigma = error_map.sigma_mm
+    order = np.argsort(np.where(np.isnan(sigma), -np.inf, sigma), axis=None)[::-1]
+    for flat in order:
+        if len(targets) >= robots:
+            break
+        row, col = np.unravel_index(flat, sigma.shape)
+        value = sigma[row, col]
+        if np.isnan(value):
+            break
+        p = error_map.centre(int(row), int(col))
+        covering = _covering(rectangles, p, NEXT_ROUND_INSET_MM)
+        if covering and free(p):
+            targets.append(
+                SpinTarget(p, covering, f"predicted error {value:.1f} mm here")
+            )
+    return targets
+
+
+def next_round_lines(targets: Sequence[SpinTarget]) -> list[str]:
+    """The suggestions as the operator reads them."""
+    if not targets:
+        return ["next round: nothing to suggest"]
+    lines = [f"next round, {len(targets)} spin(s), in this calibration's frame:"]
+    for t in targets:
+        seen = ", ".join(station_label(s) for s in t.stations)
+        lines.append(
+            f"  ({t.point_mm[0]:6.0f}, {t.point_mm[1]:6.0f}) mm, seen by {seen}: "
+            f"{t.why}"
+        )
+    return lines
+
+
 # --- Report -----------------------------------------------------------------
 
 

dotbot/calibration/push.py

@@ -10,11 +10,13 @@
 named robot is in its app, since the net core takes a calibration only in the
 bootloader; a push to the whole fleet leaves such robots out instead, unless
 that leaves none. After the push, the robots whose reported calibration id is
-not the file's are the worklist.
+not the file's, with its stations, from a boot after the push, are the
+worklist.
 
 Everything here takes the `status()` mapping of a swarmit client, duck-typed:
 address to an object with `status`, `info_gen` and `info`, `info` carrying
-`info_version`, `lh2_site_name` and `lh2_calibration_id`, or None.
+`info_version`, `info_gen`, `lh2_site_name`, `lh2_calibration_id` and
+`lh2_station_mask`, or None.
 """
 
 from __future__ import annotations
@@ -23,7 +25,7 @@
 from dataclasses import dataclass, field
 from typing import Any, Mapping
 
-from dotbot.calibration.lighthouse2 import Calibration, pushed_id
+from dotbot.calibration.lighthouse2 import Calibration, pushed_id, station_mask
 
 DEVICE_INFO_VERSION_MIN = 4
 # Seconds a pushed robot gets to reset, rejoin and report the pushed id.
@@ -74,6 +76,7 @@ class PushCheck:
     other_site: dict[str, str] = field(default_factory=dict)
     running: list[str] = field(default_factory=list)
     stale: list[str] = field(default_factory=list)
+    info_gens: dict[str, int] = field(default_factory=dict)
     fleet: bool = False
 
     @property
@@ -140,6 +143,7 @@ def check_push(status: Mapping[str, Any], calibration: Calibration) -> PushCheck
             else:
                 check.unanswered.append(addr)
             continue
+        check.info_gens[addr] = getattr(info, "info_gen", None)
         if int(getattr(info, "info_version", 0)) < DEVICE_INFO_VERSION_MIN:
             check.old_firmware.append(addr)
             continue
@@ -188,23 +192,40 @@ def gate_push(
 
 
 def push_worklist(
-    client: Any, calibration: Calibration, addresses: list[str]
+    client: Any,
+    calibration: Calibration,
+    addresses: list[str],
+    info_gens: Mapping[str, int] | None = None,
 ) -> list[str]:
-    """The pushed robots that do not report the file's id after the push.
-
-    A push resets every robot that takes it, so each gets PUSH_REJOIN_TIMEOUT
-    to rejoin and report; one not heard by then is listed too. Nothing is
-    requested here: a rejoin moves the robot's device-info generation, and
-    swarmit then refreshes its device info by one broadcast.
+    """The pushed robots that do not report the file's id and stations after
+    the push.
+
+    A robot that takes a push commits it and resets, so its device info must
+    come from a later generation than `info_gens`, the one read before the
+    push; one already on the file's id would otherwise pass without having
+    stored anything. Each robot gets PUSH_REJOIN_TIMEOUT to rejoin and
+    report; one not heard by then is listed too. Nothing is requested here: a
+    rejoin moves the robot's device-info generation, and swarmit then
+    refreshes its device info by one broadcast.
     """
     wanted_id = pushed_id(calibration)
+    wanted_mask = station_mask(calibration)
+    before = {addr.upper(): gen for addr, gen in (info_gens or {}).items()}
     pending = {addr.upper() for addr in addresses}
     deadline = time.monotonic() + PUSH_REJOIN_TIMEOUT
     while pending:
         status = client.status()
         for addr in list(pending):
             info = getattr(status.get(addr), "info", None)
-            if info is not None and _reported_id(info) == wanted_id:
+            if (
+                info is not None
+                and _reported_id(info) == wanted_id
+                and getattr(info, "lh2_station_mask", 0) == wanted_mask
+                and (
+                    addr not in before
+                    or getattr(info, "info_gen", None) != before[addr]
+                )
+            ):
                 pending.discard(addr)
         if not pending or time.monotonic() >= deadline:
             break

dotbot/cli/swarm_lh2.py

@@ -301,10 +301,10 @@ def _await_point(session, stream, arrivals: queue.Queue):
     metavar="MM",
     help=(
         "With --spin: radius of the photodiode's circle in a spin, in mm, "
-        "which sets the scale of every distance. Default: the robot model's "
-        "measured value (51.4 mm on a DotBot v3, under the 53.5 mm from "
-        "photodiode to axle because the caster drags the turning point "
-        "forward). Re-measure it on a different floor."
+        "which sets the scale of every distance. Default: the site's "
+        "spin_radius_mm, else the robot model's measured value (51.4 mm on "
+        "a DotBot v3, under the 53.5 mm from photodiode to axle because the "
+        "caster drags the turning point forward). It changes with the floor."
     ),
 )
 @click.option(
@@ -652,7 +652,10 @@ def _collect_spin(
             f"{round_} earlier round(s))."
         )
     radius_from = "--spin-radius"
-    if spin_radius is None:
+    if spin_radius is None and site.spin_radius_mm is not None:
+        spin_radius = site.spin_radius_mm
+        radius_from = f"site {site.name}"
+    elif spin_radius is None:
         spin_radius = robot_geometry(ROBOT_DEFAULT).spin_radius_mm
         radius_from = f"{ROBOT_DEFAULT}'s measured value"
     devices = _devices(ctx)
@@ -920,7 +923,7 @@ def _gated_push(client, calibration, site_changed=False, devices=None, stop=Fals
         )
     client.send_lh2_calibration(payload, check.send_to)
     click.echo("Sent. Waiting for the robots to report the new id...")
-    stale = push_worklist(client, calibration, check.targets)
+    stale = push_worklist(client, calibration, check.targets, check.info_gens)
     if stale:
         click.echo(
             f"Still not on {calibration.id8} ({len(stale)}), push again: "
@@ -946,15 +949,33 @@ def _gated_push(client, calibration, site_changed=False, devices=None, stop=Fals
     default=None,
     help="The site to look the id up under. Defaults to `site` in the dotbot config.",
 )
+@click.option(
+    "--next-round",
+    "next_robots",
+    type=click.IntRange(min=1, max=64),
+    default=None,
+    metavar="N",
+    help=(
+        "Also say where N robots should spin in another round (collect "
+        "--spin --append) to strengthen the weakest ties and the worst "
+        "predicted error; points in this calibration's frame."
+    ),
+)
 @click.pass_context
-def _show(ctx, calibration, site_name):
+def _show(ctx, calibration, site_name, next_robots):
     if _devices(ctx):
         raise click.UsageError("show reads a file, so it takes no `dotbot swarm -d`")
     from dotbot.calibration.lighthouse2 import (
         read_calibration_file,
         resolve_calibration_path,
     )
-    from dotbot.calibration.multi_station import multi_station_report
+    from dotbot.calibration.multi_station import (
+        appendable_rounds,
+        error_map_from_calibration,
+        multi_station_report,
+        next_round,
+        next_round_lines,
+    )
 
     site, _ = site_from_context(ctx, site_name)
     try:
@@ -976,6 +997,27 @@ def _show(ctx, calibration, site_name):
     )
     for line in multi_station_report(loaded):
         click.echo(line)
+    if next_robots:
+        try:
+            appendable_rounds(loaded)
+        except ValueError as exc:
+            raise click.ClickException(
+                f"--next-round plans a round for collect --spin --append: {exc}"
+            ) from exc
+        error_map = error_map_from_calibration(loaded)
+        if error_map is None:
+            raise click.ClickException(
+                f"{loaded.id8} is not a spin calibration, so it has no error map "
+                "to plan a round from"
+            )
+        targets = next_round(
+            error_map,
+            {s.index: s.valid_mm for s in loaded.stations},
+            loaded.links,
+            robots=next_robots,
+        )
+        for line in next_round_lines(targets):
+            click.echo(line)
 
 
 def _parse_shift(_ctx, _param, value):