diff --git a/dotbot/calibration/conics.py b/dotbot/calibration/conics.py index b2002fd8..48ca0f1d 100644 --- a/dotbot/calibration/conics.py +++ b/dotbot/calibration/conics.py @@ -21,7 +21,7 @@ import datetime import itertools from dataclasses import dataclass, field -from typing import Sequence +from typing import TYPE_CHECKING, Sequence import numpy as np @@ -29,10 +29,14 @@ Calibration, StationSolution, TrackSample, + union_rect, ) from dotbot.robots import ROBOT_DEFAULT, robot_geometry from dotbot.site import Site, starter_layout +if TYPE_CHECKING: + from dotbot.calibration.multi_station import Anchor, JointSolution + # Post-rectification minor/major axis ratio under which a track is not a circle. TRACK_AXIS_RATIO_MIN = 0.95 @@ -56,23 +60,37 @@ GATE_ROUNDS_MAX = 4 +@dataclass(frozen=True) +class CircleKey: + """One physical circle: a robot's spin in one collection round.""" + + name: str + round: int = 0 + + @dataclass(eq=False) class Track: - """One circle's camera points, in the order they were read. + """One circle's camera points, as one station read them, in order. `radius_mm` is the circle's true radius when known; tracks that share a radius are fitted with one. `turn` is +1 when the robot turned counter clockwise as drawn (right wheel faster), -1 clockwise, 0 unknown. + Tracks of several stations with the same `key` are the same circle. """ points: np.ndarray radius_mm: float | None = None turn: int = 0 name: str = "" + round: int = 0 def __post_init__(self): self.points = np.asarray(self.points, dtype=np.float64).reshape(-1, 2) + @property + def key(self) -> CircleKey: + return CircleKey(self.name, self.round) + @dataclass class TrackFit: @@ -88,6 +106,11 @@ class TrackFit: axis_ratio: float rms_mm: float why: str = "" + round: int = 0 + + @property + def key(self) -> CircleKey: + return CircleKey(self.name, self.round) @dataclass @@ -496,6 +519,7 @@ def fit_tracks( axis_ratio=axis_ratio(t, H), rms_mm=rms, why=why.get(t, ""), + round=t.round, ) ) return fits @@ -666,6 +690,8 @@ def field_angle(centres: np.ndarray) -> float: # What a free-mode calibration records as its site's anchor: no mark in the # room is its origin. +# A site made by `site init` records the calibration it came from in its anchor +SELF_DEFINED_ANCHOR = "self-defined from spin calibration " FREE_FRAME_ANCHOR = ( "free mode: zero at the top-left of the rectangle around the spinning " "robots, grown by a robot's footprint; axes along that rectangle" @@ -677,84 +703,90 @@ def solve_calibration( site: Site, robot: str = ROBOT_DEFAULT, tag: str = "", -) -> tuple[Calibration, dict[int, ConicSolution], dict[int, str]]: - """A free-mode calibration from circle tracks, one homography per station. - - The station with the most tracks sets the frame; every other station is - aligned to it through the centres of the circles both saw. Returns the - calibration, each solved station's solution, and why each other station - was not solved. The calibration records `site`'s name only, with - `FREE_FRAME_ANCHOR` for its anchor. + drop_stations: Sequence[int] = (), + anchors: Sequence[Anchor] = (), +) -> tuple[Calibration, JointSolution, dict[int, str]]: + """A free-mode calibration from circle tracks, every station in one frame. + + One station is solved as `solve` solves it. Several are put in one frame + by `multi_station.solve_joint` through the circles they share; one that + is not tied to the rest raises `MultiStationError` unless it is in + `drop_stations`. Returns the calibration, the joint solution, and why + each station with tracks was not solved. The calibration records + `site`'s name only, with `FREE_FRAME_ANCHOR` for its anchor; with + `anchors` it is in `site`'s own frame and records `site`'s anchor. """ - by_station: dict[int, list[Track]] = {} - for sample in samples: - by_station.setdefault(sample.station, []).append( - Track( - points=sample.camera_points(), - radius_mm=sample.radius_mm, - turn=sample.turn, - name=sample.name, - ) - ) + # pylint: disable=import-outside-toplevel,cyclic-import + from dotbot.calibration.multi_station import solve_joint, tracks_by_station + + by_station = tracks_by_station(samples) if not by_station: raise ValueError("no tracks to solve") - margin_mm = robot_geometry(robot).axle_reach_mm - order = sorted(by_station, key=lambda k: (-len(by_station[k]), k)) - reference = order[0] - solutions = {reference: solve(by_station[reference], margin_mm=margin_mm)} - unsolved: dict[int, str] = {} - centres = {t.name: t.centre_mm for t in solutions[reference].tracks} - for station in order[1:]: - try: - own = solve(by_station[station], margin_mm=margin_mm) - except ValueError as exc: - unsolved[station] = str(exc) - continue - common = [t for t in own.tracks if t.name in centres] - if len(common) < 2: - unsolved[station] = ( - f"shares {len(common)} circle(s) with station {reference}; 2 are " - "needed to put both in one frame" - ) - continue - S = similarity( - np.array([t.centre_mm for t in common]), - np.array([centres[t.name] for t in common]), - scale=False, - ) - H = S @ own.homography - own.homography = H / H[2, 2] - why = {t.name: t.why for t in own.dropped} - own.tracks = fit_tracks( - [t for t in by_station[station] if t.name not in why], own.homography - ) - dropped = [t for t in by_station[station] if t.name in why] - own.dropped = fit_tracks( - dropped, own.homography, {t: why[t.name] for t in dropped} - ) - solutions[station] = own - width, height = solutions[reference].field_mm + joint = solve_joint( + by_station, + margin_mm=robot_geometry(robot).axle_reach_mm, + drop=drop_stations, + anchors=anchors, + ) + 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 sol.homography], - points=sum(t.points for t in sol.tracks), - residual_mm=sol.residual_mm, - solved_from="conics-free", + 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=rectangles[station], ) - for station, sol in sorted(solutions.items()) + 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=FREE_FRAME_ANCHOR), - valid_mm=(0, 0, width, height), + site=Site(name=site.name, anchor=anchor), + valid_mm=valid_mm, stations=stations, tracks=list(samples), + links=sorted(joint.graph.links, key=lambda k: (k.a, k.b)), created_at=now, tag=tag, robot=robot, ) - return calibration, solutions, unsolved + return calibration, joint, dict(joint.unsolved) + + +def spin_field(calibration: Calibration) -> tuple[int, int]: + """Width and height of a free-mode calibration's field, from its file. + + The field is what `solve` reports: the box from the frame's zero to the + furthest kept spin centre, grown by a robot's reach. Kept means passing + the health gate under the stored homography. + """ + margin_mm = robot_geometry(calibration.robot).axle_reach_mm + centres = [] + for st in calibration.stations: + tracks = [ + Track(points=t.camera_points(), radius_mm=t.radius_mm, turn=t.turn) + for t in calibration.tracks + if t.station == st.index and len(t.count1) >= TRACK_POINTS_MIN + ] + if not tracks: + continue + gate = health_gate(tracks, st.matrix) + centres += [ + _circle_fit(apply(st.matrix, t.points))[0] for t in tracks if t not in gate + ] + if not centres: + return calibration.valid_mm[2], calibration.valid_mm[3] + hi = np.ceil(np.max(centres, axis=0) + margin_mm) + return int(hi[0]), int(hi[1]) def self_defined_site( @@ -764,30 +796,35 @@ def self_defined_site( ) -> tuple[Site, Calibration]: """A site pack's site from a free-mode calibration, and the calibration in it. - The site is the starter site (`starter_layout`) around the calibration's - own fence, the field (see `solve`): 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, and a tag gets `-` - appended, so the two files answer to different tags. + 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 becomes the + site extent, and a tag gets `-` appended, so the two files answer + to different tags. """ if ( not calibration.stations or calibration.site.anchor != FREE_FRAME_ANCHOR - or any(st.solved_from != "conics-free" for st in calibration.stations) + or any( + st.solved_from not in ("conics-free", "conics-joint") + for st in calibration.stations + ) ): raise ValueError( f"calibration {calibration.id8} is not a free-mode spin calibration; " "a site can only be defined from one" ) - x0, y0, field_w, field_h = calibration.valid_mm + x0, y0, _, _ = calibration.valid_mm if (x0, y0) != (0, 0): raise ValueError( f"a free-mode fence starts at (0, 0), got {list(calibration.valid_mm)}" ) + field_w, field_h = spin_field(calibration) extent, field_area, staging = starter_layout((field_w, field_h), size_mm) dx, dy = field_area.x, field_area.y shift = np.array([[1, 0, dx], [0, 1, dy], [0, 0, 1.0]]) - anchor = f"self-defined from spin calibration {calibration.id8}" + anchor = f"{SELF_DEFINED_ANCHOR}{calibration.id8}" stations = [] for st in calibration.stations: H = shift @ st.matrix @@ -799,6 +836,7 @@ def self_defined_site( points=st.points, residual_mm=st.residual_mm, solved_from=st.solved_from, + valid_mm=(0, 0, int(extent[0]), int(extent[1])), ) ) site = Site( @@ -809,12 +847,14 @@ def self_defined_site( ) placed = Calibration( site=Site(name=name, anchor=anchor), - valid_mm=(0, 0, *extent), + valid_mm=union_rect(st.valid_mm for st in stations), placements=calibration.placements, stations=stations, tracks=calibration.tracks, + links=calibration.links, created_at=calibration.created_at, tag=f"{calibration.tag}-{name}" if calibration.tag else "", robot=calibration.robot, + placed_from=calibration.id, ) return site, placed diff --git a/dotbot/calibration/driver.py b/dotbot/calibration/driver.py index 032f9c20..0bdb32d4 100644 --- a/dotbot/calibration/driver.py +++ b/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, "") diff --git a/dotbot/calibration/lighthouse2.py b/dotbot/calibration/lighthouse2.py index 516e5811..db896bce 100644 --- a/dotbot/calibration/lighthouse2.py +++ b/dotbot/calibration/lighthouse2.py @@ -38,12 +38,13 @@ CALIBRATION_DIR = Path.home() / ".dotbot" CALIBRATION_SUBDIR = "calibrations" CALIBRATION_TOML_GLOB = "calibration-*.toml" -CALIBRATION_SCHEMA_VERSION = 3 +CALIBRATION_SCHEMA_VERSION = 4 # [x_min, y_min, x_max, y_max] in frame mm: outside it a reported position is # implausible and the bot drops it. A site with a known extent supplies its # own fence; this is what a site without one falls back to. VALID_MM_DEFAULT = (0, 0, 10000, 10000) +VALID_MM_MAX = 0xFFFFFFFF LH2_BASESTATION_COUNT_MAX = 16 @@ -182,8 +183,9 @@ class TrackSample: """The reads one station took along one circle a robot traced. `radius_mm` is the circle's true radius and `turn` +1 when the robot - turned counter clockwise as drawn, -1 clockwise; `name` tells the - circles of one capture apart. + turned counter clockwise as drawn, -1 clockwise. A circle is the robot + `name` in collection `round`: the same circle seen by several stations + ties them together. """ station: int @@ -192,6 +194,7 @@ class TrackSample: turn: int count1: list[int] = field(default_factory=list) count2: list[int] = field(default_factory=list) + round: int = 0 def camera_points(self) -> np.ndarray: return camera_points_from_counts( @@ -204,19 +207,41 @@ def camera_points(self) -> np.ndarray: @dataclass class StationSolution: - """One station's solved homography and how well it fits its own evidence.""" + """One station's solved homography and how well it fits its own evidence. + + `valid_mm` is where this station's fixes are believed, frame mm. + """ index: int homography: list[list[float]] points: int residual_mm: float solved_from: str = "direct" + valid_mm: tuple[int, int, int, int] = VALID_MM_DEFAULT @property def matrix(self) -> np.ndarray: return np.array(self.homography, dtype=np.float64) +@dataclass +class LinkRecord: + """How strongly two stations are tied by the circles both kept. + + `a` < `b`. `spread_mm` is the largest distance between two shared + centres, `disagreement_mm` the rms distance between the two stations' + centres of the shared circles, `yaw_sigma_mrad` the relative yaw that + leaves. + """ + + a: int + b: int + shared: int + spread_mm: float + disagreement_mm: float + yaw_sigma_mrad: float + + @dataclass class Calibration: """A whole calibration file, in memory.""" @@ -228,9 +253,14 @@ class Calibration: placements: list[Placement] = field(default_factory=list) stations: list[StationSolution] = field(default_factory=list) tracks: list[TrackSample] = field(default_factory=list) + links: list[LinkRecord] = field(default_factory=list) created_at: str = "" tag: str = "" robot: str = ROBOT_DEFAULT + # The id of the free-frame spin calibration this one was moved from + placed_from: str = "" + # The id of the calibration this one is an edit of, its rectangles changed + edited_from: str = "" path: Optional[Path] = None # The id the file on disk declares, when this came from one. A mismatch # with `id` means the file was hand-edited into a different calibration. @@ -353,8 +383,10 @@ def reprojection_residual_mm( # --- Wire ------------------------------------------------------------------- # One calibration message per station, `swrmt_lh2_calibration_data_t` in the -# swarmit netcore: count, index, the matrix, then the site fields every -# message of a push repeats. swarmit's `helpers.py` packs the same bytes; the +# swarmit netcore: the station mask, the station's index, its matrix and its +# rectangle, then the site name and calibration id every message of a push +# repeats. The net core commits once every station of the mask has arrived, +# keyed by mask and id. swarmit's `helpers.py` packs the same bytes; the # fixture test in each repo pins them. LH2_SITE_NAME_BYTES = 16 LH2_CALIBRATION_ID_BYTES = 8 @@ -425,12 +457,42 @@ def pushed_id(calibration: Calibration) -> str: return calibration.id +def union_rect(rects: Iterable[Sequence[int]]) -> tuple[int, int, int, int]: + """The smallest `[x_min, y_min, x_max, y_max]` holding every rectangle.""" + rects = [tuple(int(v) for v in r) for r in rects] + if not rects: + raise ValueError("no rectangle to take the union of") + return ( + min(r[0] for r in rects), + min(r[1] for r in rects), + max(r[2] for r in rects), + max(r[3] for r in rects), + ) + + +def station_mask(calibration: Calibration) -> int: + """Bit i set for every station i the calibration solved.""" + mask = 0 + for station in calibration.stations: + if not 0 <= station.index < LH2_BASESTATION_COUNT_MAX: + raise ValueError( + f"station {station.index} is outside 0 to " + f"{LH2_BASESTATION_COUNT_MAX - 1}" + ) + mask |= 1 << station.index + return mask + + +def station_label(index: int) -> str: + """How a station is named to an operator: its index and its channel.""" + return f"station {index} (channel {index + 1})" + + def pushable_stations(calibration: Calibration) -> list[StationSolution]: - """The stations in index order, refused when a robot would mis-key them. + """The stations in index order, refused when a robot could not hold them. - The receiver stores a matrix at its index and takes the count as "slots - 0 to count - 1 are valid", so a gap in the numbering would leave a slot - the count claims and nothing filled. + A robot stores each station in the slot of its index, 0 to 15, so any + set of indices in that range will do. """ ordered = sorted(calibration.stations, key=lambda s: s.index) if not ordered: @@ -440,30 +502,30 @@ def pushable_stations(calibration: Calibration) -> list[StationSolution]: f"{len(ordered)} stations exceeds the LH2 limit " f"({LH2_BASESTATION_COUNT_MAX})" ) - if [s.index for s in ordered] != list(range(len(ordered))): - got = ", ".join(str(s.index) for s in ordered) + outside = [s.index for s in ordered if not 0 <= s.index < LH2_BASESTATION_COUNT_MAX] + if outside: raise ValueError( - f"stations must be numbered from zero without gaps to be pushed, got {got}" + f"station index {outside} is outside 0 to {LH2_BASESTATION_COUNT_MAX - 1}" ) return ordered def site_fields_as_bytes(calibration: Calibration) -> bytes: - """valid_mm, site name and id: the 40 bytes after every matrix.""" - return ( - valid_mm_as_bytes(calibration.valid_mm) - + site_name_as_bytes(calibration.site.name) - + calibration_id_as_bytes(pushed_id(calibration)) + """Site name and id: the 24 bytes that close every message and the page.""" + return site_name_as_bytes(calibration.site.name) + calibration_id_as_bytes( + pushed_id(calibration) ) def calibration_messages(calibration: Calibration) -> list[bytes]: """One 84-byte calibration message per station, in index order.""" stations = pushable_stations(calibration) + mask = station_mask(calibration) site_fields = site_fields_as_bytes(calibration) return [ - struct.pack(" str: lines.append(base + ".count1=" + ",".join(str(c) for c in sample.count1)) lines.append(base + ".count2=" + ",".join(str(c) for c in sample.count2)) for track in calibration.tracks: - key = f"track.{track.name}.{track.station}" + key = f"track.{track.name}.{track.round}.{track.station}" lines.append(f"{key}.radius_mm={toml_num(track.radius_mm)}") lines.append(f"{key}.turn={track.turn}") lines.append(f"{key}.count1=" + ",".join(str(c) for c in track.count1)) @@ -545,10 +607,17 @@ def canonical_serialisation(calibration: Calibration) -> str: lines.append(f"{key}.solved_from={station.solved_from}") lines.append(f"{key}.points={station.points}") lines.append(f"{key}.residual_mm={toml_num(station.residual_mm)}") + lines.append(f"{key}.valid_mm=" + ",".join(str(v) for v in station.valid_mm)) lines.append( f"{key}.homography=" + ";".join(",".join(toml_num(v) for v in row) for row in station.homography) ) + for link in sorted(calibration.links, key=lambda k: (k.a, k.b)): + key = f"link.{link.a}.{link.b}" + lines.append(f"{key}.shared={link.shared}") + lines.append(f"{key}.spread_mm={toml_num(link.spread_mm)}") + lines.append(f"{key}.disagreement_mm={toml_num(link.disagreement_mm)}") + lines.append(f"{key}.yaw_sigma_mrad={toml_num(link.yaw_sigma_mrad)}") return "\n".join(sorted(lines)) @@ -609,7 +678,7 @@ def toml_escape(text: str) -> str: def render_calibration(calibration: Calibration) -> str: - """The schema 3 file, as text.""" + """The schema 4 file, as text.""" site = calibration.site out = [ f"schema_version = {CALIBRATION_SCHEMA_VERSION}", @@ -621,6 +690,10 @@ def render_calibration(calibration: Calibration) -> str: ] if calibration.tag: out.append(f'tag = "{toml_escape(calibration.tag)}"') + if calibration.placed_from: + out.append(f'placed_from = "{calibration.placed_from}"') + if calibration.edited_from: + out.append(f'edited_from = "{calibration.edited_from}"') out += [ "", "[site]", @@ -660,6 +733,7 @@ def render_calibration(calibration: Calibration) -> str: "[[track]]", f"station = {track.station}", f'name = "{toml_escape(track.name)}"', + f"round = {track.round}", f"radius_mm = {toml_num(track.radius_mm)}", f"turn = {track.turn}", f"count1 = [{', '.join(str(c) for c in track.count1)}]", @@ -673,16 +747,30 @@ def render_calibration(calibration: Calibration) -> str: f'solved_from = "{station.solved_from}"', f"points = {station.points}", f"residual_mm = {toml_num(station.residual_mm)}", + f"valid_mm = {_toml_int_list(station.valid_mm)}", f"homography = {toml_matrix(station.homography)}", ] + for link in sorted(calibration.links, key=lambda k: (k.a, k.b)): + out += [ + "", + "[[link]]", + f"a = {link.a}", + f"b = {link.b}", + f"shared = {link.shared}", + f"spread_mm = {toml_num(link.spread_mm)}", + f"disagreement_mm = {toml_num(link.disagreement_mm)}", + f"yaw_sigma_mrad = {toml_num(link.yaw_sigma_mrad)}", + ] return "\n".join(out) + "\n" def read_calibration_file(path: Path) -> Calibration: - """Parse a schema 3 calibration file. + """Parse a schema 4 calibration file. - A file of any other schema version is rejected: schema 2 homographies - were solved on another camera point, and schema 1 has no site. + A file of any other schema version is rejected: schema 3 carries one + fence for every station and keys circles by robot alone, schema 2 + homographies were solved on another camera point, and schema 1 has no + site. """ path = Path(path) with open(path, "rb") as handle: @@ -691,7 +779,8 @@ def read_calibration_file(path: Path) -> Calibration: if schema != CALIBRATION_SCHEMA_VERSION: raise ValueError( f"{path}: unsupported calibration schema_version {schema} " - f"(this build supports {CALIBRATION_SCHEMA_VERSION})" + f"(this build supports {CALIBRATION_SCHEMA_VERSION}); collect the " + "calibration again" ) if "frame" in data: raise ValueError( @@ -733,16 +822,23 @@ def read_calibration_file(path: Path) -> Calibration: ) ) - stations = [ - StationSolution( - index=int(raw["index"]), - homography=[[float(v) for v in row] for row in raw["homography"]], - points=int(raw.get("points", 0)), - residual_mm=float(raw.get("residual_mm", 0.0)), - solved_from=raw.get("solved_from", "direct"), + stations = [] + for raw in data.get("station", []): + if "valid_mm" not in raw: + raise ValueError( + f"{path}: station {raw.get('index')} has no valid_mm, which " + f"schema {CALIBRATION_SCHEMA_VERSION} requires" + ) + stations.append( + StationSolution( + index=int(raw["index"]), + homography=[[float(v) for v in row] for row in raw["homography"]], + points=int(raw.get("points", 0)), + residual_mm=float(raw.get("residual_mm", 0.0)), + solved_from=raw.get("solved_from", "direct"), + valid_mm=_checked_rect(raw["valid_mm"], path), + ) ) - for raw in data.get("station", []) - ] tracks = [ TrackSample( @@ -752,9 +848,22 @@ def read_calibration_file(path: Path) -> Calibration: turn=int(raw.get("turn", 0)), count1=[int(c) for c in raw["count1"]], count2=[int(c) for c in raw["count2"]], + round=int(raw.get("round", 0)), ) for raw in data.get("track", []) ] + links = [ + LinkRecord( + a=int(raw["a"]), + b=int(raw["b"]), + shared=int(raw["shared"]), + spread_mm=float(raw["spread_mm"]), + disagreement_mm=float(raw["disagreement_mm"]), + yaw_sigma_mrad=float(raw["yaw_sigma_mrad"]), + ) + for raw in data.get("link", []) + ] + _check_stations(stations, links, _checked_rect(valid_mm, path), path) calibration = Calibration( site=site, @@ -762,15 +871,51 @@ def read_calibration_file(path: Path) -> Calibration: placements=placements, stations=stations, tracks=tracks, + links=links, created_at=metadata.get("created_at", ""), tag=metadata.get("tag", ""), robot=metadata.get("robot", ROBOT_DEFAULT), + placed_from=str(metadata.get("placed_from", "")), + edited_from=str(metadata.get("edited_from", "")), path=path, ) calibration.stored_id = str(metadata.get("id", "")) return calibration +def _checked_rect(values, path) -> tuple[int, int, int, int]: + """A `valid_mm` read from `path`, refused unless it is a uint32 rectangle.""" + rect = tuple(int(v) for v in values) + if ( + len(rect) != 4 + or any(v < 0 or v > VALID_MM_MAX for v in rect) + or rect[0] >= rect[2] + or rect[1] >= rect[3] + ): + raise ValueError( + f"{path}: valid_mm {list(values)} is not [x_min, y_min, x_max, y_max] " + "with x_min < x_max and y_min < y_max, in 0 to 2^32-1" + ) + return rect + + +def _check_stations(stations, links, valid_mm, path) -> None: + """The schema 4 rules between stations, links and the fence.""" + indices = [s.index for s in stations] + if len(set(indices)) != len(indices): + raise ValueError(f"{path}: a station index appears twice: {indices}") + if stations and union_rect(s.valid_mm for s in stations) != valid_mm: + raise ValueError( + f"{path}: [validity] valid_mm {list(valid_mm)} is not the union of " + f"the station rectangles {list(union_rect(s.valid_mm for s in stations))}" + ) + joint = [s for s in stations if s.solved_from.startswith("conics")] + if len(joint) > 1 and not links: + raise ValueError( + f"{path}: {len(joint)} spin-solved stations but no [[link]] between them" + ) + + def resolve_calibration_path( spec: str, root: Optional[Path] = None, @@ -908,6 +1053,15 @@ def _file_metadata(path: Path) -> dict: return {} +def calibration_site_of(spec: str) -> Optional[str]: + """The site of the one calibration file under any site that `spec` + names, None when none or several do.""" + try: + return read_calibration_file(resolve_calibration_path(spec)).site.name + except (OSError, ValueError): + return None + + def load_calibration( spec: str, root: Optional[Path] = None, @@ -975,6 +1129,7 @@ def solve_station(self, station: int) -> StationSolution: homography=[[float(v) for v in row] for row in homography], points=len(camera_points), residual_mm=residual, + valid_mm=self.valid_mm, ) def solve(self) -> list[StationSolution]: diff --git a/dotbot/calibration/multi_station.py b/dotbot/calibration/multi_station.py new file mode 100644 index 00000000..30054e31 --- /dev/null +++ b/dotbot/calibration/multi_station.py @@ -0,0 +1,1425 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# SPDX-License-Identifier: BSD-3-Clause + +"""Several LH2 stations in one frame, from the circles robots spin. + +Each station is first solved on its own circles (`conics.solve`), which +makes it metric through the spin radius but leaves it free to turn and +shift. A circle seen by two stations ties them: the station graph says which +stations are tied and how strongly, a spanning tree of rigid fits seeds one +joint refinement over every station and every circle, and the frame is then +set over every kept centre exactly as free mode sets it for one station. + +The predicted error map propagates the joint solve's covariance onto a floor +grid, so a reviewer sees where the calibration is weak without a camera. +""" + +# pylint: disable=invalid-name,too-many-locals,too-many-arguments + +from __future__ import annotations + +import math +from collections.abc import Collection, Mapping, Sequence +from dataclasses import dataclass, field + +import numpy as np + +from dotbot.calibration.conics import ( + TRACK_AXIS_RATIO_MIN, + CircleKey, + ConicSolution, + Track, + _circle_fit, + apply, + axis_ratio, + field_angle, + fit_tracks, + health_gate, + similarity, + solve, +) +from dotbot.calibration.lighthouse2 import ( + Calibration, + LinkRecord, + TrackSample, + station_label, + union_rect, +) +from dotbot.robots import robot_geometry + +__all__ = [ + "Anchor", + "CircleKey", + "ErrorMap", + "JointSolution", + "MultiStationError", + "StationGraph", + "error_map_from_calibration", + "multi_station_report", + "next_round", + "solve_joint", + "station_graph", +] + +LINK_SHARED_MIN = 2 +LINK_SPREAD_MIN_MM = 300.0 +LINK_SHARED_ADVISED = 4 +LINK_SPREAD_ADVISED_MM = 600.0 +RECT_MARGIN_MM = 300 +ERROR_MAP_CELL_MM = 100.0 +# Cells whose predicted error is over this are counted in the report. +ERROR_MAP_LIMIT_MM = 10.0 + +# Largest share a station's own scale may differ from the joint one. +SCALE_RATIO_TOLERANCE = 0.02 +# A tied circle whose station centres are further apart than this many +# times the typical disagreement is untied. +TIE_GATE_SIGMAS = 3.0 +# Least per-axis centre disagreement the tie gate and a link's yaw sigma +# assume, mm: a fit to two or three circles leaves almost none to measure. +TIE_SIGMA_FLOOR_MM = 1.0 +# Tied circles needed before the typical disagreement means anything. +TIE_GATE_MIN = 4 +TIE_ROUNDS_MAX = 4 + +# Anchors spread less than this share of the fence's diagonal hold the +# frame's yaw over too short a baseline. +ANCHOR_SPAN_SHARE = 0.5 + +# Huber threshold on the radial residual, mm. +ROBUST_MM = 5.0 +# Weight of the prior holding the root station's centres where the seed put +# them: it only removes the rigid freedom of the whole floor. +GAUGE_PRIOR_WEIGHT = 1e-3 +JOINT_ITERATIONS = 60 + +# The median of the distance between two centres each off by sigma per axis, +# in units of sigma * sqrt(2) (Rayleigh). +_RAYLEIGH_MEDIAN = math.sqrt(2 * math.log(2)) + + +@dataclass +class StationGraph: + """Stations as nodes, and a link per pair that shares a kept circle. + + `components` are the groups of stations joined by links that hold + (`link_holds`), largest first. + """ + + stations: list[int] + links: list[LinkRecord] + components: list[list[int]] + + def link(self, a: int, b: int) -> LinkRecord | None: + a, b = min(a, b), max(a, b) + for link in self.links: + if (link.a, link.b) == (a, b): + return link + return None + + +@dataclass +class ErrorMap: + """Predicted 1-sigma position error on a floor grid, mm. + + Cell (row, col) is centred on `origin_mm + (col + 0.5, row + 0.5) * + cell_mm`; NaN where no station's rectangle covers it. + """ + + origin_mm: tuple[float, float] + cell_mm: float + sigma_mm: np.ndarray + worst_mm: float + cells_over_10mm: int + + def centre(self, row: int, col: int) -> tuple[float, float]: + return ( + self.origin_mm[0] + (col + 0.5) * self.cell_mm, + self.origin_mm[1] + (row + 0.5) * self.cell_mm, + ) + + @property + def worst_cell(self) -> tuple[float, float] | None: + if np.all(np.isnan(self.sigma_mm)): + return None + row, col = np.unravel_index(np.nanargmax(self.sigma_mm), self.sigma_mm.shape) + return self.centre(int(row), int(col)) + + @property + def covered(self) -> int: + return int(np.sum(~np.isnan(self.sigma_mm))) + + +@dataclass +class JointSolution: + """Every station in one frame, and the evidence of how well it holds.""" + + homographies: dict[int, np.ndarray] + centres: dict[CircleKey, np.ndarray] + rectangles: dict[int, tuple[int, int, int, int]] + valid_mm: tuple[int, int, int, int] + field_mm: tuple[int, int] + graph: StationGraph + loops: list[tuple[list[int], float]] + station_rms_mm: dict[int, float] + scale_ratio: dict[int, float] + dropped: dict[CircleKey, str] + error_map: ErrorMap + # Each solved station's circles under its final homography, kept and dropped. + solutions: dict[int, ConicSolution] = field(default_factory=dict) + # Stations with tracks that could not be solved, and why. + unsolved: dict[int, str] = field(default_factory=dict) + # Per link (a, b): rms centre disagreement after the seed, before refinement. + seed_disagreement_mm: dict[tuple[int, int], float] = field(default_factory=dict) + warnings: list[str] = field(default_factory=list) + # Per anchor: its known point and how far the solve puts its spin from it. + anchor_residuals: dict[CircleKey, float] = field(default_factory=dict) + # Scale of the best similarity from the solved anchors to the known + # points: 1 when the spin radius and the floor agree. + anchor_scale_ratio: float | None = None + + @property + def anchored(self) -> bool: + return bool(self.anchor_residuals) + + @property + def joint(self) -> bool: + return len(self.homographies) > 1 + + +@dataclass(frozen=True) +class Anchor: + """A robot that spun with its pivot on a known floor point, site mm.""" + + key: CircleKey + point_mm: tuple[float, float] + + +class MultiStationError(ValueError): + """The stations cannot be put in one frame: an island, or a weak tie.""" + + def __init__( + self, + message: str, + islands: Sequence[int] = (), + weak: Sequence[LinkRecord] = (), + ): + super().__init__(message) + self.islands = list(islands) + self.weak = list(weak) + + +def link_holds(link: LinkRecord) -> bool: + """Whether a link is strong enough to tie its two stations at all.""" + return link.shared >= LINK_SHARED_MIN and link.spread_mm >= LINK_SPREAD_MIN_MM + + +def link_advised(link: LinkRecord) -> bool: + """Whether a link is as strong as the report advises.""" + return ( + link.shared >= LINK_SHARED_ADVISED and link.spread_mm >= LINK_SPREAD_ADVISED_MM + ) + + +# --- Graph ------------------------------------------------------------------ + + +def _spread(points: np.ndarray) -> float: + if len(points) < 2: + return 0.0 + diff = points[:, None, :] - points[None, :, :] + return float(np.max(np.linalg.norm(diff, axis=2))) + + +def _tie_stats(pa: np.ndarray, pb: np.ndarray) -> tuple[float, float]: + """rms distance between paired centres, and the relative yaw it leaves. + + The yaw sigma is that of a rigid fit on the pairs: per-axis noise, at + least `TIE_SIGMA_FLOOR_MM`, over the root of the centres' second moment + about their mean. + """ + d = np.linalg.norm(pa - pb, axis=1) + rms = float(np.sqrt(np.mean(d**2))) + moment = float(np.sum((pa - pa.mean(axis=0)) ** 2)) + if len(pa) < 2 or moment <= 0: + return rms, math.inf + sigma = max(rms / math.sqrt(2), TIE_SIGMA_FLOOR_MM) + return rms, 1000.0 * sigma / math.sqrt(moment) + + +def _components( + stations: Sequence[int], links: Sequence[LinkRecord] +) -> list[list[int]]: + parent = {s: s for s in stations} + + def find(s): + while parent[s] != s: + parent[s] = parent[parent[s]] + s = parent[s] + return s + + for link in links: + if link_holds(link) and link.a in parent and link.b in parent: + parent[find(link.a)] = find(link.b) + groups: dict[int, list[int]] = {} + for s in stations: + groups.setdefault(find(s), []).append(s) + return sorted((sorted(g) for g in groups.values()), key=lambda g: (-len(g), g[0])) + + +def _kept_centres(solution: ConicSolution) -> dict[CircleKey, np.ndarray]: + return {t.key: np.array(t.centre_mm) for t in solution.tracks} + + +def _graph_from_centres( + centres: Mapping[int, Mapping[CircleKey, np.ndarray]], + tied: Mapping[CircleKey, set[int]] | None = None, +) -> StationGraph: + """The graph, with each station's centres expressed in one frame or its own. + + A pair's spread is measured in the first station's centres, which are + metric either way; the disagreement is only meaningful when every + station's centres are in one frame. `tied` limits which stations a + circle ties (all that kept it, by default). + """ + stations = sorted(centres) + links = [] + for i, a in enumerate(stations): + for b in stations[i + 1 :]: + shared = sorted( + (set(centres[a]) & set(centres[b])), + key=lambda k: (k.name, k.round), + ) + if tied is not None: + shared = [k for k in shared if {a, b} <= tied.get(k, set())] + if not shared: + continue + pa = np.array([centres[a][k] for k in shared]) + pb = np.array([centres[b][k] for k in shared]) + rms, yaw = _tie_stats(pa, pb) + links.append( + LinkRecord( + a=a, + b=b, + shared=len(shared), + spread_mm=_spread(pa), + disagreement_mm=rms, + yaw_sigma_mrad=yaw, + ) + ) + return StationGraph( + stations=stations, links=links, components=_components(stations, links) + ) + + +def _solve_stations( + tracks: Mapping[int, Sequence[Track]], margin_mm: float +) -> tuple[dict[int, ConicSolution], dict[int, str]]: + solutions, unsolved = {}, {} + for station in sorted(tracks): + try: + solutions[station] = solve(tracks[station], margin_mm=margin_mm) + except ValueError as exc: + unsolved[station] = str(exc) + return solutions, unsolved + + +def station_graph( + tracks: Mapping[int, Sequence[Track]], + solutions: Mapping[int, ConicSolution] | None = None, +) -> StationGraph: + """Which stations the kept circles tie, from each station's own solve. + + Spreads are in each station's own metric frame; disagreements are those + left by a rigid fit of one station's shared centres onto the other's. + """ + if solutions is None: + solutions, _ = _solve_stations(tracks, robot_geometry().axle_reach_mm) + own = {s: _kept_centres(sol) for s, sol in solutions.items()} + graph = _graph_from_centres(own) + for link in graph.links: + keys = sorted( + set(own[link.a]) & set(own[link.b]), key=lambda k: (k.name, k.round) + ) + pa = np.array([own[link.a][k] for k in keys]) + pb = np.array([own[link.b][k] for k in keys]) + if len(keys) >= 2: + pb = apply(similarity(pb, pa, scale=False), pb) + link.disagreement_mm, link.yaw_sigma_mrad = _tie_stats(pa, pb) + else: + link.disagreement_mm, link.yaw_sigma_mrad = 0.0, math.inf + return graph + + +# --- Seed ------------------------------------------------------------------- + + +def _spanning_tree(graph: StationGraph, root: int) -> dict[int, int]: + """Parent of each station in a maximum spanning tree of holding links, + weighted by shared count times spread, rooted at `root`.""" + holding = sorted( + (k for k in graph.links if link_holds(k)), + key=lambda k: (-k.shared * k.spread_mm, k.a, k.b), + ) + parent_uf = {s: s for s in graph.stations} + + def find(s): + while parent_uf[s] != s: + s = parent_uf[s] + return s + + adjacency: dict[int, list[int]] = {s: [] for s in graph.stations} + for link in holding: + ra, rb = find(link.a), find(link.b) + if ra != rb: + parent_uf[ra] = rb + adjacency[link.a].append(link.b) + adjacency[link.b].append(link.a) + parent = {root: root} + queue = [root] + while queue: + s = queue.pop(0) + for n in sorted(adjacency[s]): + if n not in parent: + parent[n] = s + queue.append(n) + return parent + + +def _loops( + graph: StationGraph, + parent: Mapping[int, int], + own: Mapping[int, Mapping[CircleKey, np.ndarray]], +) -> list[tuple[list[int], float]]: + """Each fundamental cycle of the holding links, and how far a point moves + when carried round it by the pairwise rigid fits.""" + tree = {(min(a, b), max(a, b)) for a, b in parent.items() if a != b} + out = [] + + def path_to_root(s): + path = [s] + while parent[path[-1]] != path[-1]: + path.append(parent[path[-1]]) + return path + + def fit(a, b): + keys = sorted(set(own[a]) & set(own[b]), key=lambda k: (k.name, k.round)) + return similarity( + np.array([own[a][k] for k in keys]), + np.array([own[b][k] for k in keys]), + scale=False, + ) + + for link in graph.links: + if not link_holds(link) or (link.a, link.b) in tree: + continue + if link.a not in parent or link.b not in parent: + continue + pa, pb = path_to_root(link.a), path_to_root(link.b) + common = next(s for s in pa if s in pb) + cycle = pa[: pa.index(common) + 1] + list(reversed(pb[: pb.index(common)])) + # cycle runs a -> ... -> common -> ... -> b, closed by the link b -> a + T = np.eye(3) + for x, y in zip(cycle, cycle[1:] + cycle[:1]): + T = fit(x, y) @ T + start = np.mean(list(own[cycle[0]].values()), axis=0) + moved = apply(T, start[None, :])[0] + out.append((cycle, float(np.linalg.norm(moved - start)))) + return out + + +# --- Joint refinement ------------------------------------------------------- + + +def _h_params(H: np.ndarray) -> np.ndarray: + H = H / H[2, 2] + return np.array( + [H[0, 0], H[0, 1], H[0, 2], H[1, 0], H[1, 1], H[1, 2], H[2, 0], H[2, 1]] + ) + + +def _h_matrix(p: np.ndarray) -> np.ndarray: + return np.array([[p[0], p[1], p[2]], [p[3], p[4], p[5]], [p[6], p[7], 1.0]]) + + +def _map(p: np.ndarray, q: np.ndarray) -> np.ndarray: + """H(q) for the 8 parameters `p`.""" + qx, qy = q[:, 0], q[:, 1] + w = p[6] * qx + p[7] * qy + 1.0 + return np.c_[(p[0] * qx + p[1] * qy + p[2]) / w, (p[3] * qx + p[4] * qy + p[5]) / w] + + +def _map_jacobian(p: np.ndarray, q: np.ndarray): + """H(q) for the 8 parameters `p`, and its derivative, (n, 2, 8).""" + qx, qy = q[:, 0], q[:, 1] + w = p[6] * qx + p[7] * qy + 1.0 + u = (p[0] * qx + p[1] * qy + p[2]) / w + v = (p[3] * qx + p[4] * qy + p[5]) / w + G = np.zeros((len(q), 2, 8)) + G[:, 0, 0], G[:, 0, 1], G[:, 0, 2] = qx / w, qy / w, 1 / w + G[:, 1, 3], G[:, 1, 4], G[:, 1, 5] = qx / w, qy / w, 1 / w + G[:, 0, 6], G[:, 0, 7] = -u * qx / w, -u * qy / w + G[:, 1, 6], G[:, 1, 7] = -v * qx / w, -v * qy / w + return np.c_[u, v], G + + +@dataclass +class _Problem: + """The joint least-squares problem: which parameters each read touches.""" + + stations: list[int] + centre_ids: list # CircleKey for a tied circle, (CircleKey, station) otherwise + blocks: list # (station, centre id, camera points, radius) + prior: dict # centre id -> (target xy) + + def __post_init__(self): + self.s_col = {s: 8 * i for i, s in enumerate(self.stations)} + base = 8 * len(self.stations) + self.c_col = {c: base + 2 * i for i, c in enumerate(self.centre_ids)} + self.n = base + 2 * len(self.centre_ids) + + def pack(self, H: Mapping[int, np.ndarray], C: Mapping) -> np.ndarray: + x = np.zeros(self.n) + for s, col in self.s_col.items(): + x[col : col + 8] = _h_params(H[s]) + for c, col in self.c_col.items(): + x[col : col + 2] = C[c] + return x + + def unpack(self, x: np.ndarray): + H = {s: _h_matrix(x[col : col + 8]) for s, col in self.s_col.items()} + C = {c: x[col : col + 2].copy() for c, col in self.c_col.items()} + return H, C + + def residuals(self, x: np.ndarray, prior: bool = True) -> np.ndarray: + """The radial residual of every read, in block order, then the prior's.""" + rows = [] + for s, cid, q, radius in self.blocks: + sc, cc = self.s_col[s], self.c_col[cid] + uv = _map(x[sc : sc + 8], q) + rows.append(np.linalg.norm(uv - x[cc : cc + 2], axis=1) - radius) + if prior: + for cid, target in self.prior.items(): + cc = self.c_col[cid] + rows.append(GAUGE_PRIOR_WEIGHT * (x[cc : cc + 2] - target)) + return np.concatenate(rows) + + def normal( + self, x: np.ndarray, w: np.ndarray, prior: bool = True + ) -> tuple[np.ndarray, np.ndarray]: + """`J.T W² J` and `J.T W² r` for the row weights `w`, block by block. + + Each read touches its station's 8 parameters and its centre's 2, so + the Jacobian is never formed whole. + """ + A = np.zeros((self.n, self.n)) + g = np.zeros(self.n) + row = 0 + for s, cid, q, radius in self.blocks: + sc, cc = self.s_col[s], self.c_col[cid] + uv, G = _map_jacobian(x[sc : sc + 8], q) + d = uv - x[cc : cc + 2] + rho = np.linalg.norm(d, axis=1) + e = d / np.maximum(rho, 1e-12)[:, None] + wb = w[row : row + len(q), None] + row += len(q) + J = np.empty((len(q), 10)) + J[:, :8] = np.einsum("ni,nij->nj", e, G) + J[:, 8:] = -e + J *= wb + idx = np.r_[sc : sc + 8, cc : cc + 2] + A[np.ix_(idx, idx)] += J.T @ J + g[idx] += J.T @ ((rho - radius) * wb[:, 0]) + if prior: + for cid, target in self.prior.items(): + cc = self.c_col[cid] + A[cc, cc] += GAUGE_PRIOR_WEIGHT**2 + A[cc + 1, cc + 1] += GAUGE_PRIOR_WEIGHT**2 + g[cc : cc + 2] += GAUGE_PRIOR_WEIGHT**2 * (x[cc : cc + 2] - target) + return A, g + + def read_rows(self) -> int: + return sum(len(b[2]) for b in self.blocks) + + +def _huber(r: np.ndarray, reads: int) -> np.ndarray: + w = np.ones_like(r) + big = np.abs(r[:reads]) > ROBUST_MM + w[:reads][big] = np.sqrt(ROBUST_MM / np.abs(r[:reads][big])) + return w + + +def _refine(problem: _Problem, x0: np.ndarray) -> np.ndarray: + """Levenberg-Marquardt on the Huber-weighted residuals, Marquardt scaling.""" + reads = problem.read_rows() + x = x0.copy() + r = problem.residuals(x) + w = _huber(r, reads) + A, g = problem.normal(x, w) + cost = float(np.sum((w * r) ** 2)) + lam = 1e-3 + for _ in range(JOINT_ITERATIONS): + D = np.diag(A).copy() + D[D <= 0] = 1e-12 + improved = False + while lam < 1e12: + try: + dx = np.linalg.solve(A + lam * np.diag(D), -g) + except np.linalg.LinAlgError: + lam *= 10 + continue + r_new = problem.residuals(x + dx) + cost_new = float(np.sum((w * r_new) ** 2)) + if np.isfinite(cost_new) and cost_new < cost: + gain = cost - cost_new + x = x + dx + r = problem.residuals(x) + w = _huber(r, reads) + A, g = problem.normal(x, w) + cost = float(np.sum((w * r) ** 2)) + lam = max(lam / 10, 1e-12) + improved = gain > 1e-12 * max(cost, 1e-12) + break + lam *= 10 + if not improved: + break + return x + + +def _covariance(problem: _Problem, x: np.ndarray) -> np.ndarray: + """Parameter covariance with the rigid freedom of the floor removed. + + From the read rows only (no prior): the information matrix, scaled to + unit diagonal, loses its 3 smallest eigen-directions (the gauge), is + inverted, and is scaled by the residual variance. + """ + r = problem.residuals(x, prior=False) + w = _huber(r, problem.read_rows()) + A, _ = problem.normal(x, w, prior=False) + rw = r * w + d = np.sqrt(np.maximum(np.diag(A), 1e-30)) + As = A / np.outer(d, d) + vals, vecs = np.linalg.eigh(As) + gauge = 3 + keep = np.arange(len(vals)) >= gauge + keep &= vals > vals.max() * 1e-14 + inv = (vecs[:, keep] / vals[keep]) @ vecs[:, keep].T + dof = max(len(r) - (problem.n - gauge), 1) + variance = float(np.sum(rw**2) / dof) + return variance * inv / np.outer(d, d) + + +def _error_map( + problem: _Problem, + x: np.ndarray, + rectangles: Mapping[int, Sequence[int]], + cell_mm: float = ERROR_MAP_CELL_MM, +) -> ErrorMap: + cov = _covariance(problem, x) + H, _ = problem.unpack(x) + x0, y0, x1, y1 = union_rect(rectangles.values()) + cols = max(1, int(math.ceil((x1 - x0) / cell_mm))) + rows = max(1, int(math.ceil((y1 - y0) / cell_mm))) + gx = x0 + (np.arange(cols) + 0.5) * cell_mm + gy = y0 + (np.arange(rows) + 0.5) * cell_mm + XX, YY = np.meshgrid(gx, gy) + info = np.zeros((rows, cols)) + for s, (rx0, ry0, rx1, ry1) in rectangles.items(): + inside = (XX >= rx0) & (XX <= rx1) & (YY >= ry0) & (YY <= ry1) + if not inside.any(): + continue + p = np.c_[XX[inside], YY[inside]] + q = apply(np.linalg.inv(H[s]), p) + col = problem.s_col[s] + _, G = _map_jacobian(x[col : col + 8], q) + S = cov[col : col + 8, col : col + 8] + var = np.einsum("nij,jk,nik->n", G, S, G) + var = np.maximum(var, 1e-12) + info[inside] += 1.0 / var + sigma = np.full((rows, cols), np.nan) + covered = info > 0 + sigma[covered] = 1.0 / np.sqrt(info[covered]) + worst = float(np.nanmax(sigma)) if covered.any() else float("nan") + return ErrorMap( + origin_mm=(float(x0), float(y0)), + cell_mm=float(cell_mm), + sigma_mm=sigma, + worst_mm=worst, + cells_over_10mm=int(np.sum(sigma[covered] > ERROR_MAP_LIMIT_MM)), + ) + + +# --- Frame ------------------------------------------------------------------ + + +def _rotation(angle: float) -> np.ndarray: + c, s = np.cos(angle), np.sin(angle) + return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1.0]]) + + +def _upright_turns(mapped: Sequence[np.ndarray], centres: np.ndarray) -> int: + """`conics.upright_turns` over the tracks of every station at once.""" + dirs = [] + for p in mapped: + d = p[0] - _circle_fit(p)[0] + dirs.append(d / np.linalg.norm(d)) + mean = np.mean(dirs, axis=0) + if np.linalg.norm(mean) >= 0.5: + heading = np.arctan2(mean[1], mean[0]) + return int(np.round((np.pi / 2 - heading) / (np.pi / 2))) % 4 + width, height = np.ptp(centres, axis=0) + return 1 if width > height else 0 + + +def _rect_around(points: np.ndarray, margin: float) -> tuple[int, int, int, int]: + lo = np.floor(points.min(axis=0) - margin) + hi = np.ceil(points.max(axis=0) + margin) + lo = np.maximum(lo, 0) + return (int(lo[0]), int(lo[1]), int(hi[0]), int(hi[1])) + + +def _frame( + H: dict[int, np.ndarray], + C: dict, + kept: Mapping[int, Sequence[Track]], + margin_mm: float, +) -> tuple[np.ndarray, tuple[int, int]]: + """The move into the free-mode frame over every kept centre, and the field.""" + centres = np.array(list(C.values())) + T = _rotation(-field_angle(centres)) + mapped = [apply(T @ H[s], t.points) for s in kept for t in kept[s]] + turns = _upright_turns(mapped, apply(T, centres)) + T = np.linalg.matrix_power(_rotation(np.pi / 2), turns) @ T + moved = apply(T, centres) + lo = moved.min(axis=0) - margin_mm + size = np.ceil(moved.max(axis=0) + margin_mm - lo) + T = np.array([[1, 0, -lo[0]], [0, 1, -lo[1]], [0, 0, 1.0]]) @ T + return T, (int(size[0]), int(size[1])) + + +# --- The solve -------------------------------------------------------------- + + +def _island_message( + islands: Sequence[int], main: Sequence[int], graph: StationGraph +) -> tuple[str, list[LinkRecord]]: + lines, weak = [], [] + for s in islands: + touching = [k for k in graph.links if s in (k.a, k.b)] + weak += touching + if touching: + ties = "; ".join( + f"shares {k.shared} circle(s) {k.spread_mm:.0f} mm apart with " + f"{station_label(k.b if k.a == s else k.a)}" + for k in touching + ) + else: + ties = "shares no kept circle with any station" + lines.append(f"{station_label(s)} is not tied to the others: {ties}") + names = ", ".join(station_label(s) for s in main) + lines.append( + f"A tie needs {LINK_SHARED_MIN} or more shared circles at least " + f"{LINK_SPREAD_MIN_MM:.0f} mm apart. Put 2 or more robots, well apart, " + f"where the station and one of {names} both see them and collect " + "another round, or leave it out with " + + " ".join(f"--drop-station {s}" for s in islands) + + "." + ) + return "\n".join(lines), weak + + +def _single( + station: int, + solution: ConicSolution, + tracks: Sequence[Track], + rect_margin_mm: int, + unsolved: dict[int, str], +) -> JointSolution: + """One station, exactly as `conics.solve` solves it.""" + H = solution.homography / solution.homography[2, 2] + kept_names = {t.key for t in solution.tracks} + kept = [t for t in tracks if t.key in kept_names] + centres = {t.key: np.array(t.centre_mm) for t in solution.tracks} + rect = _rect_around(np.array(list(centres.values())), rect_margin_mm) + problem = _Problem( + stations=[station], + centre_ids=list(centres), + blocks=[(station, t.key, t.points, t.radius_mm) for t in kept], + prior={}, + ) + x = problem.pack({station: H}, centres) + graph = StationGraph(stations=[station], links=[], components=[[station]]) + return JointSolution( + homographies={station: H}, + centres=centres, + rectangles={station: rect}, + valid_mm=rect, + field_mm=solution.field_mm, + graph=graph, + loops=[], + station_rms_mm={station: solution.residual_mm}, + scale_ratio={station: 1.0}, + dropped={}, + error_map=_error_map(problem, x, {station: rect}), + solutions={station: solution}, + unsolved=unsolved, + ) + + +def solve_joint( + tracks: Mapping[int, Sequence[Track]], + *, + margin_mm: float | None = None, + rect_margin_mm: int = RECT_MARGIN_MM, + drop: Collection[int] = (), + anchors: Sequence[Anchor] = (), +) -> JointSolution: + """Every station's homography in one metric frame, from circle tracks. + + Without `anchors` the frame is free mode's (see `_solve_free`). With two + or more, the free solution is moved by the one rigid motion that best + puts the anchors' spin centres on their known points: the spin radius + keeps setting the scale, and how far the anchors disagree with it is + reported, never absorbed. + """ + joint = _solve_free( + tracks, margin_mm=margin_mm, rect_margin_mm=rect_margin_mm, drop=drop + ) + if not anchors: + return joint + return _anchored(joint, tracks, anchors, rect_margin_mm) + + +def _solve_free( + tracks: Mapping[int, Sequence[Track]], + *, + margin_mm: float | None = None, + rect_margin_mm: int = RECT_MARGIN_MM, + drop: Collection[int] = (), +) -> JointSolution: + """Every station's homography in one metric frame, from circle tracks. + + Each station is solved on its own circles first; a station that cannot + be is listed in `unsolved` and left out. `drop` leaves stations out on + purpose. Raises MultiStationError when a solved station is not tied to + the rest by links that hold (`link_holds`), naming it. + """ + if margin_mm is None: + margin_mm = robot_geometry().axle_reach_mm + tracks = {s: list(t) for s, t in tracks.items() if s not in set(drop) and t} + if not tracks: + raise ValueError("no tracks to solve") + solutions, unsolved = _solve_stations(tracks, margin_mm) + if not solutions: + raise ValueError( + "no station could be solved: " + + "; ".join(f"{station_label(s)}: {why}" for s, why in unsolved.items()) + ) + graph = station_graph(tracks, solutions) + root = min(solutions, key=lambda s: (-len(solutions[s].tracks), s)) + main = next(c for c in graph.components if root in c) + islands = [s for s in sorted(solutions) if s not in main] + if islands: + message, weak = _island_message(islands, main, graph) + raise MultiStationError(message, islands=islands, weak=weak) + if len(solutions) == 1: + return _single(root, solutions[root], tracks[root], rect_margin_mm, unsolved) + + own = {s: _kept_centres(sol) for s, sol in solutions.items()} + kept = {s: [t for t in tracks[s] if t.key in own[s]] for s in sorted(solutions)} + # Stage 3: seed by composing rigid fits down a maximum spanning tree. + parent = _spanning_tree(graph, root) + H0: dict[int, np.ndarray] = {} + world: dict[int, dict[CircleKey, np.ndarray]] = {} + order = sorted(parent, key=lambda s: _depth(parent, s)) + for s in order: + Hs = solutions[s].homography / solutions[s].homography[2, 2] + if s != root: + p = parent[s] + keys = sorted(set(own[s]) & set(own[p]), key=lambda k: (k.name, k.round)) + S = similarity( + np.array([own[s][k] for k in keys]), + np.array([world[p][k] for k in keys]), + scale=False, + ) + Hs = S @ Hs + Hs = Hs / Hs[2, 2] + H0[s] = Hs + world[s] = {t.key: _circle_fit(apply(Hs, t.points))[0] for t in kept[s]} + seed_graph = _graph_from_centres(world) + seed_disagreement = {(k.a, k.b): k.disagreement_mm for k in seed_graph.links} + + # Stage 4: one refinement over every station and every circle, untying + # circles the stations disagree on. + tied: dict[CircleKey, set[int]] = {} + for s, centres in world.items(): + for k in centres: + tied.setdefault(k, set()).add(s) + dropped: dict[CircleKey, str] = {} + H, C = dict(H0), {} + problem = None + x = None + for tie_round in range(TIE_ROUNDS_MAX + 1): + problem, x0 = _build_problem(kept, world, tied, H, root) + x = _refine(problem, x0) + H, C = problem.unpack(x) + per_station = { + s: {t.key: _circle_fit(apply(H[s], t.points))[0] for t in kept[s]} + for s in kept + } + if tie_round == TIE_ROUNDS_MAX: + break + untie = _gate_ties(per_station, tied) + if not untie: + break + for k, why in untie.items(): + dropped[k] = why + tied[k] = set() + world = per_station + graph_after = _graph_from_centres(per_station, tied) + main_after = next(c for c in graph_after.components if root in c) + islands = [s for s in sorted(kept) if s not in main_after] + if islands: + message, weak = _island_message(islands, main_after, graph_after) + raise MultiStationError( + "after untying the circles the stations disagree on:\n" + message, + islands=islands, + weak=weak, + ) + + # Stage 5: the frame over every kept centre. + T, field = _frame(H, C, kept, margin_mm) + H = {s: (T @ Hs) / (T @ Hs)[2, 2] for s, Hs in H.items()} + C = {c: apply(T, v[None, :])[0] for c, v in C.items()} + x = problem.pack(H, C) + per_station = { + s: {t.key: _circle_fit(apply(H[s], t.points))[0] for t in kept[s]} for s in kept + } + final_graph = _graph_from_centres(per_station, tied) + rectangles = { + s: _rect_around(np.array(list(per_station[s].values())), rect_margin_mm) + for s in sorted(kept) + } + for s, rect in rectangles.items(): + if rect[0] >= rect[2] or rect[1] >= rect[3]: + raise ValueError(f"{station_label(s)}: its rectangle {rect} is empty") + warnings: list[str] = [] + final_solutions: dict[int, ConicSolution] = {} + scale_ratio: dict[int, float] = {} + for s, sol in solutions.items(): + why = {t.key: t.why for t in sol.dropped} + out = [t for t in tracks[s] if t.key in why] + sol.homography = H[s] + sol.tracks = fit_tracks(kept[s], H[s]) + sol.dropped = fit_tracks(out, H[s], {t: why[t.key] for t in out}) + sol.field_mm = field + final_solutions[s] = sol + pts = np.vstack([t.points for t in kept[s]]) + own_mapped = apply(H0[s], pts) + S = similarity(own_mapped, apply(H[s], pts), scale=True) + scale_ratio[s] = float(math.sqrt(abs(np.linalg.det(S[:2, :2])))) + if abs(scale_ratio[s] - 1) > SCALE_RATIO_TOLERANCE: + warnings.append( + f"{station_label(s)}: its own circles' scale is " + f"{100 * (scale_ratio[s] - 1):+.1f} % off the joint one" + ) + for t in kept[s]: + ratio = axis_ratio(t, H[s]) + if ratio < TRACK_AXIS_RATIO_MIN: + warnings.append( + f"{station_label(s)}: circle {_key_text(t.key)} has axis " + f"ratio {ratio:.3f} after the joint solve" + ) + loops = _loops(graph, parent, own) + centres_out: dict[CircleKey, np.ndarray] = {} + for c, v in C.items(): + centres_out.setdefault(c if isinstance(c, CircleKey) else c[0], v) + return JointSolution( + homographies=H, + centres=centres_out, + rectangles=rectangles, + valid_mm=union_rect(rectangles.values()), + field_mm=field, + graph=final_graph, + loops=loops, + station_rms_mm={s: final_solutions[s].residual_mm for s in final_solutions}, + scale_ratio=scale_ratio, + dropped=dropped, + error_map=_error_map(problem, x, rectangles), + solutions=final_solutions, + unsolved=unsolved, + seed_disagreement_mm=seed_disagreement, + warnings=warnings, + ) + + +def _anchored( + joint: JointSolution, + tracks: Mapping[int, Sequence[Track]], + anchors: Sequence[Anchor], + rect_margin_mm: int, +) -> JointSolution: + if len(anchors) < 2: + raise ValueError( + f"{len(anchors)} anchor(s): two or more fix the frame to the floor" + ) + missing = [a for a in anchors if a.key not in joint.centres] + if missing: + raise ValueError( + "no station kept the spin of anchor(s) " + + ", ".join(_key_text(a.key) for a in missing) + ) + got = np.array([joint.centres[a.key] for a in anchors]) + want = np.array([a.point_mm for a in anchors], dtype=float) + T = similarity(got, want, scale=False) + S = similarity(got, want, scale=True) + H = {s: (T @ Hs) / (T @ Hs)[2, 2] for s, Hs in joint.homographies.items()} + centres = {k: apply(T, v[None, :])[0] for k, v in joint.centres.items()} + kept = { + s: [t for t in tracks[s] if t.key in {f.key for f in sol.tracks}] + for s, sol in joint.solutions.items() + } + rectangles = {} + for s, sol in joint.solutions.items(): + why = {t.key: t.why for t in sol.dropped} + out = [t for t in tracks[s] if t.key in why] + sol.homography = H[s] + sol.tracks = fit_tracks(kept[s], H[s]) + sol.dropped = fit_tracks(out, H[s], {t: why[t.key] for t in out}) + rectangles[s] = _rect_around( + np.array([t.centre_mm for t in sol.tracks], dtype=float), rect_margin_mm + ) + rect = rectangles[s] + if rect[0] >= rect[2] or rect[1] >= rect[3]: + raise ValueError( + f"{station_label(s)}: the anchors put its circles below the " + f"site's zero, leaving the rectangle {list(rect)}; check the " + "anchors' points" + ) + valid = union_rect(rectangles.values()) + warnings = list(joint.warnings) + span = _spread(want) + diagonal = math.hypot(valid[2] - valid[0], valid[3] - valid[1]) + if span < ANCHOR_SPAN_SHARE * diagonal: + warnings.append( + f"the anchors span {span:.0f} mm, under half the fence's " + f"{diagonal:.0f} mm diagonal: the frame's yaw is held over a short " + "baseline; put anchors at the ends of the longest path across " + "the stations" + ) + residuals = { + a.key: float(np.linalg.norm(apply(T, g[None, :])[0] - w)) + for a, g, w in zip(anchors, got, want) + } + return JointSolution( + homographies=H, + centres=centres, + rectangles=rectangles, + valid_mm=valid, + field_mm=joint.field_mm, + graph=joint.graph, + loops=joint.loops, + station_rms_mm=joint.station_rms_mm, + scale_ratio=joint.scale_ratio, + dropped=joint.dropped, + error_map=_map_from(H, kept, rectangles), + solutions=joint.solutions, + unsolved=joint.unsolved, + seed_disagreement_mm=joint.seed_disagreement_mm, + warnings=warnings, + anchor_residuals=residuals, + anchor_scale_ratio=float(math.sqrt(abs(np.linalg.det(S[:2, :2])))), + ) + + +def _map_from( + H: Mapping[int, np.ndarray], + kept: Mapping[int, Sequence[Track]], + rectangles: Mapping[int, Sequence[int]], + cell_mm: float = ERROR_MAP_CELL_MM, +) -> ErrorMap: + """The error map of homographies `H` over the kept tracks, every circle + seen by several stations tying them.""" + world = { + s: {t.key: _circle_fit(apply(H[s], t.points))[0] for t in kept[s]} for s in kept + } + tied: dict[CircleKey, set[int]] = {} + for s, centres in world.items(): + for k in centres: + tied.setdefault(k, set()).add(s) + root = min(kept, key=lambda s: (-len(kept[s]), s)) + problem, x = _build_problem(kept, world, tied, dict(H), root) + return _error_map(problem, x, rectangles, cell_mm) + + +def _depth(parent: Mapping[int, int], s: int) -> int: + depth = 0 + while parent[s] != s: + s = parent[s] + depth += 1 + return depth + + +def _key_text(key: CircleKey) -> str: + return key.name if key.round == 0 else f"{key.name} round {key.round}" + + +def _build_problem(kept, world, tied, H, root): + centre_ids, C0 = [], {} + blocks = [] + for s in sorted(kept): + for t in kept[s]: + cid = ( + t.key + if len(tied.get(t.key, ())) >= 2 and s in tied[t.key] + else (t.key, s) + ) + if cid not in C0: + centre_ids.append(cid) + if isinstance(cid, CircleKey): + C0[cid] = np.mean([world[x][cid] for x in tied[cid]], axis=0) + else: + C0[cid] = world[s][t.key] + blocks.append((s, cid, t.points, t.radius_mm)) + prior = {} + for t in kept[root]: + cid = t.key if t.key in C0 else (t.key, root) + prior[cid] = C0[cid].copy() + problem = _Problem( + stations=sorted(kept), centre_ids=centre_ids, blocks=blocks, prior=prior + ) + return problem, problem.pack(H, C0) + + +def _gate_ties( + per_station: Mapping[int, Mapping[CircleKey, np.ndarray]], + tied: Mapping[CircleKey, set[int]], +) -> dict[CircleKey, str]: + """The tied circles whose station centres disagree beyond the gate, and why.""" + spans = {} + for k, stations in tied.items(): + if len(stations) < 2: + continue + pts = np.array([per_station[s][k] for s in sorted(stations)]) + spans[k] = _spread(pts) + if len(spans) < TIE_GATE_MIN: + return {} + scale = max(np.median(list(spans.values())) / _RAYLEIGH_MEDIAN, TIE_SIGMA_FLOOR_MM) + limit = TIE_GATE_SIGMAS * scale + return { + k: ( + f"its centres from {', '.join(station_label(s) for s in sorted(tied[k]))} " + f"are {d:.1f} mm apart, over {limit:.1f} mm" + ) + for k, d in spans.items() + if d > limit + } + + +# --- From a file ------------------------------------------------------------ + + +def tracks_by_station(samples: Sequence[TrackSample]) -> dict[int, list[Track]]: + """Calibration tracks as the solver's camera-point tracks, per station.""" + by_station: dict[int, list[Track]] = {} + for sample in samples: + by_station.setdefault(sample.station, []).append( + Track( + points=sample.camera_points(), + radius_mm=sample.radius_mm, + turn=sample.turn, + name=sample.name, + round=sample.round, + ) + ) + return by_station + + +def error_map_from_calibration( + calibration: Calibration, cell_mm: float = ERROR_MAP_CELL_MM +) -> ErrorMap | None: + """The predicted error map of a spin calibration, from its file alone. + + Uses each station's stored homography and the tracks that pass the + health gate under it; every circle kept by several stations ties them. + None when the file holds no spin-solved station. + """ + stations = { + st.index: st + for st in calibration.stations + if st.solved_from.startswith("conics") + } + by_station = { + s: t for s, t in tracks_by_station(calibration.tracks).items() if s in stations + } + if not by_station: + return None + kept = {} + for s, tracks in by_station.items(): + H = stations[s].matrix + gate = health_gate(tracks, H) + kept[s] = [t for t in tracks if t not in gate and len(t.points) >= 6] + kept = {s: t for s, t in kept.items() if t} + if not kept: + return None + H = {s: stations[s].matrix for s in kept} + rectangles = {s: stations[s].valid_mm for s in kept} + return _map_from(H, kept, rectangles, cell_mm) + + +def appendable_rounds(calibration: Calibration) -> int: + """The round a new collection appended to `calibration` takes. + + Refused unless it is a free-mode spin calibration: a placed or corner + calibration is in a frame a re-solve would not keep. + """ + # pylint: disable=import-outside-toplevel + from dotbot.calibration.conics import FREE_FRAME_ANCHOR, SELF_DEFINED_ANCHOR + from dotbot.calibration.lighthouse2 import calibration_site_of + + if ( + not calibration.tracks + or calibration.site.anchor != FREE_FRAME_ANCHOR + or not all(st.solved_from.startswith("conics") for st in calibration.stations) + ): + refused = ( + f"calibration {calibration.id8} is not a free-mode spin calibration, " + "so a round cannot be appended to it" + ) + anchor = calibration.site.anchor + if calibration.placed_from: + source = calibration.placed_from + how = f"it was placed from spin calibration {source[:8]}" + elif anchor.startswith(SELF_DEFINED_ANCHOR): + source = anchor[len(SELF_DEFINED_ANCHOR) :].strip() + how = f"its site was defined from spin calibration {source[:8]}" + else: + raise ValueError( + f"{refused}; append to the spin calibration it was made from" + ) + where = calibration_site_of(source) + raise ValueError( + f"{refused}: {how}, so append to that one" + + (f" (--append {source[:8]} --site {where})" if where else "") + ) + 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 ----------------------------------------------------------------- + + +def _link_line(link: LinkRecord, before: float | None = None) -> str: + verdict = "" + if not link_holds(link): + verdict = ( + f" not a tie: under {LINK_SHARED_MIN} circles over " + f"{LINK_SPREAD_MIN_MM:.0f} mm" + ) + elif not link_advised(link): + verdict = ( + f" weak: {LINK_SHARED_ADVISED} or more circles over " + f"{LINK_SPREAD_ADVISED_MM:.0f} mm advised" + ) + disagreement = f"{link.disagreement_mm:.1f} mm" + if before is not None: + disagreement = f"{before:.1f} -> {disagreement}" + yaw = ( + "unknown" + if math.isinf(link.yaw_sigma_mrad) + else f"{link.yaw_sigma_mrad:.1f} mrad" + ) + return ( + f" {station_label(link.a)} - {station_label(link.b)}: {link.shared} shared, " + f"spread {link.spread_mm:.0f} mm, disagreement {disagreement}, " + f"yaw {yaw}{verdict}" + ) + + +def _error_map_lines(error_map: ErrorMap | None) -> list[str]: + if error_map is None or error_map.covered == 0: + return [] + worst = error_map.worst_cell + return [ + f"predicted error ({error_map.cell_mm:.0f} mm cells): worst " + f"{error_map.worst_mm:.1f} mm near ({worst[0]:.0f}, {worst[1]:.0f}) mm, " + f"{error_map.cells_over_10mm} of {error_map.covered} cells over " + f"{ERROR_MAP_LIMIT_MM:.0f} mm" + ] + + +def multi_station_report( + calibration: Calibration, joint: JointSolution | None = None +) -> list[str]: + """What the operator reads about how the stations hold together. + + From `joint` when the solve just ran; otherwise from the file's + stations, links and tracks, with the error map recomputed from them. + """ + lines = ["stations:"] + circles: dict[int, int] = {} + for track in calibration.tracks: + circles[track.station] = circles.get(track.station, 0) + 1 + for st in sorted(calibration.stations, key=lambda s: s.index): + rect = list(st.valid_mm) + if joint is not None and st.index in joint.solutions: + sol = joint.solutions[st.index] + kept = f"{len(sol.tracks)} kept, {len(sol.dropped)} dropped circles" + scale = f", scale {joint.scale_ratio.get(st.index, 1.0):.3f}" + elif st.solved_from.startswith("conics"): + kept = f"{circles.get(st.index, 0)} circles" + scale = "" + else: + kept = f"{st.points} marked points" + scale = "" + lines.append( + f" {station_label(st.index)}: {kept}, rms {st.residual_mm:.1f} mm, " + f"rectangle {rect}{scale}" + ) + if joint is not None: + for s, why in sorted(joint.unsolved.items()): + lines.append(f" {station_label(s)}: not solved, {why}") + links = joint.graph.links if joint is not None else calibration.links + if links: + lines.append("links:") + for link in sorted(links, key=lambda k: (k.a, k.b)): + before = None + if joint is not None: + before = joint.seed_disagreement_mm.get((link.a, link.b)) + lines.append(_link_line(link, before)) + if joint is not None: + for key, residual in joint.anchor_residuals.items(): + lines.append( + f"anchor {_key_text(key)}: its spin is {residual:.1f} mm from " + "its known point" + ) + if joint.anchor_scale_ratio is not None: + lines.append( + f"anchor scale ratio {joint.anchor_scale_ratio:.4f}: 1 when the " + "spin radius and the floor agree" + ) + for cycle, closure in joint.loops: + path = " - ".join(str(s) for s in cycle + cycle[:1]) + lines.append(f"loop {path}: closure {closure:.1f} mm") + for key, why in sorted(joint.dropped.items(), key=lambda kv: kv[0].name): + lines.append(f"untied circle {_key_text(key)}: {why}") + error_map = joint.error_map + else: + try: + error_map = error_map_from_calibration(calibration) + except (ValueError, np.linalg.LinAlgError): + error_map = None + lines += _error_map_lines(error_map) + if joint is not None: + lines += [f"warning: {w}" for w in joint.warnings] + return lines diff --git a/dotbot/calibration/placement.py b/dotbot/calibration/placement.py new file mode 100644 index 00000000..5d4ec7d8 --- /dev/null +++ b/dotbot/calibration/placement.py @@ -0,0 +1,362 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# SPDX-License-Identifier: BSD-3-Clause + +"""Moving a spin calibration onto a site as one rigid body. + +A free-mode calibration (`collect --spin`) defines its own frame, metric +through the spin radius. `place_calibration` turns and shifts every station +of it by one `Rigid2D`, so the stations keep their geometry relative to one +another and the scale is never touched. +""" + +from __future__ import annotations + +import dataclasses +import math +from collections.abc import Iterable, Sequence +from dataclasses import dataclass +from pathlib import Path + +import numpy as np + +from dotbot.calibration import lighthouse2 +from dotbot.calibration.conics import Track, fit_tracks +from dotbot.calibration.lighthouse2 import Calibration, StationSolution +from dotbot.site import Site + +# The solvers whose frame is the calibration's own, set by the spinning +# robots rather than by marks in the room: only those may be moved. +FREE_SOLVERS = frozenset({"conics-free", "conics-joint"}) + + +class PlacementRefused(ValueError): + """A calibration that must not be moved, or a move the robots cannot take.""" + + +@dataclass(frozen=True) +class Rigid2D: + """Turn by `theta_deg` about the frame origin, then shift by + (`dx_mm`, `dy_mm`): `p' = R p + d`. + + Positive angles turn +x toward +y, which with y pointing down is + clockwise as drawn. + """ + + dx_mm: float = 0.0 + dy_mm: float = 0.0 + theta_deg: float = 0.0 + + @classmethod + def about( + cls, + pivot: tuple[float, float], + theta_deg: float, + shift: tuple[float, float] = (0.0, 0.0), + ) -> Rigid2D: + """Turn about `pivot` by `theta_deg`, then translate by `shift`.""" + theta = math.radians(theta_deg) + c, s = math.cos(theta), math.sin(theta) + px, py = pivot + return cls( + px - c * px + s * py + shift[0], + py - s * px - c * py + shift[1], + theta_deg, + ) + + def matrix(self) -> np.ndarray: + theta = math.radians(self.theta_deg) + c, s = math.cos(theta), math.sin(theta) + return np.array([[c, -s, self.dx_mm], [s, c, self.dy_mm], [0.0, 0.0, 1.0]]) + + def apply(self, points: Iterable[Sequence[float]]) -> list[tuple[float, float]]: + M = self.matrix() + return [ + ( + float(M[0, 0] * x + M[0, 1] * y + M[0, 2]), + float(M[1, 0] * x + M[1, 1] * y + M[1, 2]), + ) + for x, y in points + ] + + def box(self, rect: Sequence[float]) -> tuple[int, int, int, int]: + """The axis-aligned box `[x0, y0, x1, y1]` round `rect` once moved, + rounded outwards to whole millimetres.""" + x0, y0, x1, y1 = rect + corners = self.apply([(x0, y0), (x1, y0), (x1, y1), (x0, y1)]) + xs = [p[0] for p in corners] + ys = [p[1] for p in corners] + # A quarter turn lands on whole numbers but for rounding noise + return ( + math.floor(min(xs) + 1e-6), + math.floor(min(ys) + 1e-6), + math.ceil(max(xs) - 1e-6), + math.ceil(max(ys) - 1e-6), + ) + + +def is_free_mode(calibration: Calibration) -> bool: + """Whether every station was solved in the calibration's own frame.""" + return bool(calibration.stations) and all( + st.solved_from in FREE_SOLVERS for st in calibration.stations + ) + + +def station_rect(station: StationSolution) -> tuple[int, int, int, int] | None: + """The station's own validity rectangle, for files that carry one.""" + rect = getattr(station, "valid_mm", None) + return tuple(int(v) for v in rect) if rect else None + + +def spin_centres(calibration: Calibration) -> dict[int, list[dict]]: + """Each station's circles in the calibration's frame: name, centre and + radius in mm, one per `[[track]]` that station saw.""" + out: dict[int, list[dict]] = {} + for station in calibration.stations: + samples = [t for t in calibration.tracks if t.station == station.index] + tracks = [ + Track(points=t.camera_points(), radius_mm=t.radius_mm, name=t.name) + for t in samples + if t.count1 + ] + out[station.index] = [ + { + "name": fit.name, + "x": fit.centre_mm[0], + "y": fit.centre_mm[1], + "radius_mm": fit.radius_mm, + } + for fit in fit_tracks(tracks, station.matrix) + ] + return out + + +def _weak(link) -> bool: + from dotbot.calibration.multi_station import ( + LINK_SHARED_ADVISED, + LINK_SPREAD_ADVISED_MM, + ) + + return link.shared < LINK_SHARED_ADVISED or link.spread_mm < LINK_SPREAD_ADVISED_MM + + +def overlay(calibration: Calibration) -> dict: + """What the site editor draws for `calibration`, in its own frame: the + fence, each station's rectangle (its own, else the fence) and spin + centres, and the links between stations.""" + centres = spin_centres(calibration) + fence = [int(v) for v in calibration.valid_mm] + stations = [] + for st in sorted(calibration.stations, key=lambda s: s.index): + rect = station_rect(st) + circles = centres.get(st.index, []) + stations.append( + { + "index": st.index, + "channel": st.index + 1, + "rect": list(rect) if rect is not None else fence, + "centres": [[c["x"], c["y"]] for c in circles], + "circles": circles, + "solved_from": st.solved_from, + } + ) + return { + "id": calibration.id, + "id8": calibration.id8, + "free": is_free_mode(calibration), + "fence": fence, + "stations": stations, + "links": [ + {"a": link.a, "b": link.b, "shared": link.shared, "weak": _weak(link)} + for link in getattr(calibration, "links", []) + ], + "tag": calibration.tag, + "created_at": calibration.created_at, + "site": calibration.site.name, + "anchor": calibration.site.anchor, + } + + +def placed_tag(tag: str, site_name: str, taken: Iterable[str] = ()) -> str: + """The moved calibration's tag: `tag` suffixed with the site, then a + number if that tag is taken.""" + return suffixed_tag(tag, site_name, taken) + + +def suffixed_tag(tag: str, suffix: str, taken: Iterable[str] = ()) -> str: + """`tag` suffixed `-` unless it already is, then a number if that + tag is taken; no tag stays none.""" + if not tag: + return "" + base = tag if tag.endswith(f"-{suffix}") else f"{tag}-{suffix}" + taken = {t.lower() for t in taken} + candidate, n = base, 2 + while candidate.lower() in taken or candidate.lower() == tag.lower(): + candidate, n = f"{base}-{n}", n + 1 + return candidate + + +def place_calibration( + calibration: Calibration, + site: Site, + transform: Rigid2D, + *, + reanchor: bool = False, + taken_tags: Iterable[str] = (), +) -> Calibration: + """`calibration` moved by `transform` into `site`, as a new calibration. + + Every station's homography becomes `T . H`, normalised; a station's own + rectangle becomes the site's extent, else the box round its turned + corners, and the fence their union; placements' + points move with it; tracks and links are copied. The original is not + modified. + + A corner-collected calibration is tied to the floor through the site's + anchor and is refused unless `reanchor`. A move that puts a spin centre, + a station's rectangle (its own, else the calibration's fence) or a + placement's point below zero is refused: robots carry positions as + unsigned millimetres. + """ + if not calibration.stations: + raise PlacementRefused(f"calibration {calibration.id8} has no solved station") + if not is_free_mode(calibration) and not reanchor: + kinds = sorted({st.solved_from for st in calibration.stations}) + raise PlacementRefused( + f"calibration {calibration.id8} was collected at points of the room " + f"(solved from {', '.join(kinds)}), so its frame is the site's " + "anchor: moving it would misplace every robot. Re-anchor it only if " + "the anchor itself moved." + ) + T = transform.matrix() + extent = site.valid_mm + stations = [] + for st in calibration.stations: + H = T @ st.matrix + H = H / H[2, 2] + changes = {"homography": [[float(v) for v in row] for row in H]} + rect = station_rect(st) + if extent is not None: + changes["valid_mm"] = extent + elif rect is not None: + changes["valid_mm"] = transform.box(rect) + stations.append(dataclasses.replace(st, **changes)) + + fence = transform.box(calibration.valid_mm) + below = [ + f"station {st.index} (channel {st.index + 1})'s rectangle " + f"{list(station_rect(st) or fence)}" + for st in stations + if min((station_rect(st) or fence)[:2]) < 0 + ] + below += [ + f"placement {n}'s point ({x:.0f}, {y:.0f})" + for n, placement in enumerate(calibration.placements) + for x, y in transform.apply(placement.points_mm) + if x < 0 or y < 0 + ] + for index, circles in spin_centres( + dataclasses.replace(calibration, stations=stations) + ).items(): + below += [ + f"circle {c['name']} (station {index}, channel {index + 1}) at " + f"({c['x']:.0f}, {c['y']:.0f})" + for c in circles + if c["x"] < 0 or c["y"] < 0 + ] + if below: + more = f" and {len(below) - 3} more" if len(below) > 3 else "" + raise PlacementRefused( + "the move puts " + + ", ".join(below[:3]) + + more + + " below zero, where a robot cannot report a position" + ) + + rects = [station_rect(st) for st in stations] + if rects and all(r is not None for r in rects): + valid_mm = ( + min(r[0] for r in rects), + min(r[1] for r in rects), + max(r[2] for r in rects), + max(r[3] for r in rects), + ) + elif site.valid_mm is not None: + valid_mm = site.valid_mm + else: + valid_mm = fence + try: + lighthouse2.valid_mm_as_bytes(valid_mm) + except ValueError as exc: + raise PlacementRefused(f"the moved calibration's fence: {exc}") from exc + placements = [ + dataclasses.replace( + placement, + points_mm=transform.apply(placement.points_mm), + samples=[dataclasses.replace(s) for s in placement.samples], + ) + for placement in calibration.placements + ] + extra = {} + if hasattr(calibration, "links"): + extra["links"] = list(calibration.links) + return Calibration( + **extra, + site=Site(name=site.name, anchor=site.anchor), + valid_mm=tuple(int(v) for v in valid_mm), + placements=placements, + stations=stations, + tracks=list(calibration.tracks), + created_at=calibration.created_at, + tag=placed_tag(calibration.tag, site.name, taken_tags), + robot=calibration.robot, + placed_from=calibration.id, + ) + + +def site_tags(site_name: str, paths: Iterable[Path] | None = None) -> list[str]: + """The tags of the calibrations of `site_name` among `paths`, by default + the files in its calibration folder.""" + if paths is None: + paths = lighthouse2.site_dir(site_name).glob(lighthouse2.CALIBRATION_TOML_GLOB) + tags = [] + for path in paths: + meta = lighthouse2._file_metadata(path) + if meta.get("tag") and _file_site(path) == site_name: + tags.append(meta["tag"]) + return tags + + +def _file_site(path: Path) -> str: + import tomllib + + try: + with open(path, "rb") as handle: + return tomllib.load(handle).get("site", {}).get("name", "") + except (OSError, tomllib.TOMLDecodeError): + return "" + + +def save_placed(source: Calibration, placed: Calibration) -> tuple[Calibration, Path]: + """Write `placed` beside its site's calibrations, or, when a file of its + id is there already, leave that file as it is; return what is on disk. + + A placement with the source's own id would overwrite the source, and is + refused. + """ + if placed.id == source.id: + raise PlacementRefused( + f"the move leaves calibration {source.id8} where it is; nothing to write" + ) + return save_new(placed) + + +def save_new(calibration: Calibration) -> tuple[Calibration, Path]: + """Write `calibration` beside its site's calibrations, or, when a file of + its id is there already, leave that file as it is; return what is on disk.""" + for path in lighthouse2.site_dir(calibration.site.name).glob( + f"*-{calibration.id8}.toml" + ): + existing = lighthouse2.read_calibration_file(path) + if existing.id == calibration.id: + return existing, path + return calibration, lighthouse2.write_calibration(calibration) diff --git a/dotbot/calibration/push.py b/dotbot/calibration/push.py index 5573d96d..4b7bca6e 100644 --- a/dotbot/calibration/push.py +++ b/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,9 +25,9 @@ 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 = 3 +DEVICE_INFO_VERSION_MIN = 4 # Seconds a pushed robot gets to reset, rejoin and report the pushed id. PUSH_REJOIN_TIMEOUT = 30.0 PUSH_POLL_INTERVAL = 1.0 @@ -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,37 @@ 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 diff --git a/dotbot/calibration/simulated.py b/dotbot/calibration/simulated.py index fc0a7b28..d755b145 100644 --- a/dotbot/calibration/simulated.py +++ b/dotbot/calibration/simulated.py @@ -115,7 +115,7 @@ def status(self) -> dict[str, Any]: self.pushed[-1][:LH2_CALIBRATION_MESSAGE_BYTES] ) info = SimpleNamespace( - info_version=3, lh2_site_name=site, lh2_calibration_id=calibration_id + info_version=4, lh2_site_name=site, lh2_calibration_id=calibration_id ) return {self.device: SimpleNamespace(info_gen=1, info=info)} diff --git a/dotbot/calibration/spin.py b/dotbot/calibration/spin.py index ee0fde6b..16a0f64b 100644 --- a/dotbot/calibration/spin.py +++ b/dotbot/calibration/spin.py @@ -18,7 +18,11 @@ from typing import Any from dotbot.calibration.conics import TRACKS_ADVISED, ConicSolution -from dotbot.calibration.lighthouse2 import LH2CalibrationSample, TrackSample +from dotbot.calibration.lighthouse2 import ( + LH2CalibrationSample, + TrackSample, + station_label, +) from dotbot.calibration.ota import _is_impossible, _parse_records from dotbot.calibration.push import in_app, stop_robots @@ -76,8 +80,9 @@ def reads(self) -> list[LH2CalibrationSample]: """The records of every chunk received, in order; a lost chunk is a gap.""" return [r for k in sorted(self.received) for r in self.received[k]] - def samples(self, radius_mm: float) -> list[TrackSample]: - """One counter clockwise track per station, as raw counts.""" + def samples(self, radius_mm: float, round_: int = 0) -> list[TrackSample]: + """One counter clockwise track per station, as raw counts, of + collection round `round_`.""" by_station: dict[int, list[LH2CalibrationSample]] = {} for record in self.reads(): if not _is_impossible(record): @@ -90,6 +95,7 @@ def samples(self, radius_mm: float) -> list[TrackSample]: turn=1, count1=[r.count1 for r in records], count2=[r.count2 for r in records], + round=round_, ) for station, records in sorted(by_station.items()) ] @@ -288,12 +294,13 @@ def spin_report( "solved from what did" ) for station, solution in sorted(solutions.items()): - lines.append(f"station {station}:") + lines.append(f"{station_label(station)}:") fits = sorted(solution.tracks + solution.dropped, key=lambda t: t.name) for t in fits: verdict = f"dropped: {t.why}" if t.why else "kept" + name = t.name if not t.round else f"{t.name}#{t.round}" lines.append( - f" {t.name:<18} {t.points:4d} reads centre " + f" {name:<18} {t.points:4d} reads centre " f"({t.centre_mm[0]:6.0f}, {t.centre_mm[1]:6.0f}) mm " f"r {t.radius_mm:5.1f} mm ratio {t.axis_ratio:.3f} " f"rms {t.rms_mm:4.1f} mm {verdict}" @@ -308,5 +315,5 @@ def spin_report( "more, spread over the area, hold the frame better" ) for station, why in sorted(unsolved.items()): - lines.append(f"station {station}: not solved, {why}") + lines.append(f"{station_label(station)}: not solved, {why}") return lines diff --git a/dotbot/calibration/station_boxes.py b/dotbot/calibration/station_boxes.py new file mode 100644 index 00000000..7f71a724 --- /dev/null +++ b/dotbot/calibration/station_boxes.py @@ -0,0 +1,177 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# SPDX-License-Identifier: BSD-3-Clause + +"""Changing where each station of a calibration is trusted. + +A station's `valid_mm` is the rectangle its fixes are believed in. `rebox` +makes a new calibration with some of those rectangles changed and everything +else, homographies included, as it was; the calibration it came from is +never written. +""" + +from __future__ import annotations + +import dataclasses +from collections.abc import Iterable, Mapping, Sequence + +from dotbot.calibration import lighthouse2 +from dotbot.calibration.conics import FREE_FRAME_ANCHOR +from dotbot.calibration.lighthouse2 import Calibration, station_label, union_rect +from dotbot.calibration.placement import spin_centres, suffixed_tag +from dotbot.site import Site + +TAG_SUFFIX = "boxes" + +Rect = tuple[int, int, int, int] + + +class BoxesRefused(ValueError): + """A calibration whose rectangles may not be changed here, or a rectangle + a robot cannot take.""" + + +def frame_refusal(file_site: Site, site: Site) -> str | None: + """Why a calibration recorded in `file_site` is not in `site`'s frame, + or None when it is.""" + if file_site.anchor == FREE_FRAME_ANCHOR: + return ( + "it is a free-mode spin calibration, in its own frame: place it on " + "the site first" + ) + try: + lighthouse2.check_calibration_site(file_site, site, None) + except ValueError as exc: + return str(exc) + return None + + +def rect_refusal(rect: Sequence[int], site: Site) -> str | None: + """Why `[x_min, y_min, x_max, y_max]` cannot be a station's rectangle on + `site`, or None when it can.""" + x0, y0, x1, y1 = (int(v) for v in rect) + if x1 <= x0 or y1 <= y0: + return "is empty" + if x0 < 0 or y0 < 0: + return "reaches below zero, where a robot cannot report a position" + if site.extent_mm is not None: + width, height = site.extent_mm + if x1 > width or y1 > height: + return f"reaches outside the site's extent {width} x {height} mm" + try: + lighthouse2.valid_mm_as_bytes((x0, y0, x1, y1)) + except ValueError as exc: + return str(exc) + return None + + +def reboxed( + source: Calibration, + site: Site, + rects: Mapping[int, Sequence[int]], + *, + tag: str | None = None, + taken_tags: Iterable[str] = (), +) -> Calibration: + """`source` with the stations named in `rects` given those rectangles, + as a new calibration; the fence becomes their union. + + `tag` defaults to the source's, suffixed `-boxes` and numbered when + taken. Refused when `source` is not in `site`'s frame, was edited by + hand, or leaves every rectangle as it was, and for a rectangle + `rect_refusal` names. + """ + why = frame_refusal(source.site, site) + if why is not None: + raise BoxesRefused(f"calibration {source.id8} is not in site {site.name}'s frame: {why}") + try: + lighthouse2.pushed_id(source) + except ValueError as exc: + raise BoxesRefused(str(exc)) from exc + indices = {st.index for st in source.stations} + unknown = sorted(set(rects) - indices) + if unknown: + raise BoxesRefused( + f"calibration {source.id8} has no station {', '.join(map(str, unknown))}; " + f"it has {', '.join(map(str, sorted(indices)))}" + ) + refused = [] + for index, rect in sorted(rects.items()): + why = rect_refusal(rect, site) + if why is not None: + refused.append(f"{station_label(index)}'s rectangle {list(rect)} {why}") + if refused: + raise BoxesRefused("; ".join(refused)) + stations = [ + ( + dataclasses.replace(st, valid_mm=tuple(int(v) for v in rects[st.index])) + if st.index in rects + else st + ) + for st in source.stations + ] + if all(a.valid_mm == b.valid_mm for a, b in zip(stations, source.stations)): + raise BoxesRefused( + f"every rectangle of {source.id8} is as it was; nothing to write" + ) + taken = list(taken_tags) + if tag is None: + tag = suffixed_tag(source.tag, TAG_SUFFIX, taken) + elif tag and tag.lower() in {t.lower() for t in taken}: + raise BoxesRefused(f"tag {tag!r} is taken by another calibration of {site.name}") + return dataclasses.replace( + source, + valid_mm=union_rect(st.valid_mm for st in stations), + stations=stations, + tag=tag, + edited_from=source.id, + path=None, + stored_id="", + ) + + +def _inside(rect: Sequence[int], x: float, y: float) -> bool: + return rect[0] <= x <= rect[2] and rect[1] <= y <= rect[3] + + +def coverage_warnings(calibration: Calibration) -> list[str]: + """One line per station whose rectangle leaves out spin circles or + placement points that station saw.""" + out = [] + circles = spin_centres(calibration) + for st in sorted(calibration.stations, key=lambda s: s.index): + names = sorted( + { + c["name"] + for c in circles.get(st.index, []) + if not _inside(st.valid_mm, c["x"], c["y"]) + } + ) + points = sum( + 1 + for placement in calibration.placements + for sample in placement.samples + if sample.station == st.index + and not _inside(st.valid_mm, *placement.points_mm[sample.point]) + ) + left_out = [] + if names: + left_out.append( + f"{len(names)} spin circle(s) ({', '.join(names[:3])}" + + (f" and {len(names) - 3} more" if len(names) > 3 else "") + + ")" + ) + if points: + left_out.append(f"{points} placement point(s)") + if left_out: + out.append( + f"{station_label(st.index)}'s rectangle {list(st.valid_mm)} leaves " + f"out {' and '.join(left_out)} it was calibrated on" + ) + return out + + +def push_line(calibration: Calibration) -> str: + return ( + f"dotbot swarm calibrate-lh2 push {calibration.id8} " + f"--site {calibration.site.name}" + ) diff --git a/dotbot/cli/main.py b/dotbot/cli/main.py index f67e0e77..64f554eb 100644 --- a/dotbot/cli/main.py +++ b/dotbot/cli/main.py @@ -71,7 +71,8 @@ ( "site", "dotbot.cli.site_cmd", - "Sites: add a pack, switch with use, list / show, export one to share.", + "Sites: add a pack, switch with use, list / show, export one to share, " + "new / edit to draw one.", ), ) diff --git a/dotbot/cli/site_cmd.py b/dotbot/cli/site_cmd.py index d428866d..b6680261 100644 --- a/dotbot/cli/site_cmd.py +++ b/dotbot/cli/site_cmd.py @@ -7,7 +7,8 @@ (`dotbot.site_packs`). `add` copies one into ~/.dotbot/sites/, where every folder finds it; `init` writes one around a spin calibration's robots; `use` makes a site the active one; `list` and `show` read; `export` writes one as a -zip. The same folders can be shared with git, cp or unzip. +zip; `new` and `edit` open the site editor on a pack. The same folders can be +shared with git, cp or unzip. """ import json @@ -43,7 +44,7 @@ name="site", help=( "The places you work in: add a site pack, switch with use, " - "list / show, export one to share." + "list / show, export one to share, new / edit to draw one." ), ) def cmd(): @@ -681,6 +682,71 @@ def area(name: str, note: str) -> str: ) +def _editor_socket(port: int): + """A socket listening on 127.0.0.1:`port` for the editor, once its page + is known to be built.""" + import socket + + from dotbot.site_editor import EDITOR_DIR, EDITOR_PAGE + + if not (EDITOR_DIR / EDITOR_PAGE).is_file(): + raise click.ClickException( + f"the site editor page is not built ({EDITOR_DIR / EDITOR_PAGE} is " + "missing). From a PyDotBot checkout, build it with: npm --prefix " + "dotbot/console-web install && npm --prefix dotbot/console-web run " + "build" + ) + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + try: + sock.bind(("127.0.0.1", port)) + except OSError as exc: + sock.close() + raise click.ClickException(f"cannot listen on 127.0.0.1:{port}: {exc}") from exc + sock.listen() + return sock + + +def _serve_editor(name: str, pack: Path, sock, headless: bool) -> None: + """Serve the editor on `pack` from `sock` until Ctrl-C or Done.""" + import threading + import webbrowser + + import uvicorn + + from dotbot.calibration.lighthouse2 import calibration_root + from dotbot.site_editor import EditorState, create_app + + url = f"http://127.0.0.1:{sock.getsockname()[1]}/" + state = EditorState(name, pack, [calibration_root() / name]) + server = None + + def done(): + server.should_exit = True + + app = create_app(state, on_done=done) + server = uvicorn.Server(uvicorn.Config(app, log_level="warning")) + click.echo(f"Editing site {name} ({pack / PACK_FILE})") + click.echo(f"Site editor at {url} - Ctrl-C or Done to stop") + if not headless: + threading.Thread(target=webbrowser.open, args=(url,), daemon=True).start() + try: + server.run(sockets=[sock]) + finally: + sock.close() + + +_PORT = click.option( + "--port", + type=click.IntRange(0, 65535), + default=0, + show_default="a free port", + help="Port on 127.0.0.1 to serve the editor on.", +) +_HEADLESS = click.option( + "--headless", is_flag=True, help="Print the editor's URL; don't open a browser." +) + + @cmd.command() @click.argument("name") @click.option( @@ -726,7 +792,12 @@ def init(ctx, name, calibration, size, force): read_calibration_file, resolve_calibration_path, site_name_as_bytes, - write_calibration, + ) + from dotbot.calibration.placement import ( + Rigid2D, + place_calibration, + save_placed, + site_tags, ) from dotbot.cli._swarm_inject import swarm_connection @@ -756,10 +827,14 @@ def init(ctx, name, calibration, size, force): except ValueError: path = resolve_calibration_path(calibration) source = read_calibration_file(path) - site, placed = self_defined_site(source, name, size) + site, _ = self_defined_site(source, name, size) + field = site.areas["field"] + placed = place_calibration( + source, site, Rigid2D(field.x, field.y, 0.0), taken_tags=site_tags(name) + ) + placed, saved = save_placed(source, placed) except ValueError as exc: raise click.ClickException(str(exc)) from exc - saved = write_calibration(placed) target.mkdir(parents=True, exist_ok=True) (target / PACK_FILE).write_text( _render_site_pack(site, source.id8), encoding="utf-8" @@ -784,3 +859,90 @@ def init(ctx, name, calibration, size, force): f"--lh2-calibration {placed.id8} " "--headless" ) + + +@cmd.command() +@click.argument("name") +@click.option( + "--field", + "field_spec", + default="2m", + show_default=True, + help="The starter field, as for `dotbot config init`: 2m, 2x3m, 1500mm.", +) +@_PORT +@_HEADLESS +@click.pass_context +def new(ctx, name, field_spec, port, headless): + """Write a new site pack, then open the editor on it. + + The pack goes in sites/NAME/ beside the project's dotbot.toml when one is + in use, else in ~/.dotbot/sites/NAME/, so `dotbot site use NAME` finds it + at once. It starts as `dotbot config init` starts a site: a field with + floor round it and a staging strip below. It is not made the active site. + """ + from dotbot.calibration.lighthouse2 import site_name_as_bytes + from dotbot.cli.config_cmd import default_site_toml, parse_field_size + from dotbot.site_packs import site_homes + + config = (ctx.obj or {}).get("config") + try: + check_site_name(name) + site_name_as_bytes(name) # the push carries the name to the robots + existing = resolve_site_entry(config, name) + except (ValueError, ConfigError) as exc: + raise click.ClickException(str(exc)) from exc + if existing is not None: + raise click.ClickException( + f"site {name} already exists ({existing.pack}); open it with " + f"`dotbot site edit {name}`, or pick another name" + ) + field_mm = parse_field_size(field_spec) + _, home = site_homes(config)[0] + target = home / name + if target.exists(): + raise click.ClickException(f"{target} already exists") + sock = _editor_socket(port) + target.mkdir(parents=True) + (target / PACK_FILE).write_text(default_site_toml(field_mm)) + click.echo(f"Wrote {target / PACK_FILE}") + _serve_editor(name, target, sock, headless) + click.echo(f"Work in it with `dotbot site use {name}`, or --site {name}.") + + +@cmd.command() +@click.argument("site", required=False) +@_PORT +@_HEADLESS +@click.pass_context +def edit(ctx, site, port, headless): + """Open the site editor on a site pack, at 127.0.0.1. + + SITE is a site name or a pack folder's path; it defaults to the active + site. + """ + from dotbot.site_packs import is_pack_path, pack_at + + if site is not None and is_pack_path(site): + try: + entry = pack_at(site) + except ConfigError as exc: + raise click.ClickException(str(exc)) from exc + _serve_editor(entry.name, entry.pack, _editor_socket(port), headless) + return + if site is None: + active = active_site(ctx) + if active.entry is not None: + _serve_editor( + active.name, active.entry.pack, _editor_socket(port), headless + ) + return + site = active.name + name = site + entry = _catalog(ctx).get(name) + if entry is None: + raise click.ClickException( + f"unknown site {name!r}; `dotbot site list` names the known ones, " + "and `dotbot site new` makes one" + ) + _serve_editor(name, entry.pack, _editor_socket(port), headless) diff --git a/dotbot/cli/swarm_lh2.py b/dotbot/cli/swarm_lh2.py index 76fdc5a9..cc17275e 100644 --- a/dotbot/cli/swarm_lh2.py +++ b/dotbot/cli/swarm_lh2.py @@ -19,6 +19,8 @@ another id. - `reframe ` - re-express a saved calibration in another site's frame and re-solve it from its stored samples. +- `rebox ` - save a new calibration with some stations' + rectangles changed, everything else as it was. The homography solve lives in PyDotBot (`dotbot.calibration.lighthouse2`); the transport lives in swarmit. @@ -301,10 +303,48 @@ 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( + "--drop-station", + "drop_stations", + multiple=True, + type=click.IntRange(min=0, max=15), + metavar="N", + help=( + "With --spin: leave station N (0 to 15, channel N+1) out of the " + "solve, repeatable. For a station that is not tied to the others by " + "shared circles, which otherwise stops the solve." + ), +) +@click.option( + "--append", + "append", + default=None, + metavar="CALIBRATION", + help=( + "With --spin: collect another round onto a saved spin calibration " + "(a path, its tag or its id prefix) and solve both rounds together; " + "saved as a new file, the old one kept. For a station not yet tied " + "to the others, or a weak tie the report names." + ), +) +@click.option( + "--anchor", + "anchors", + multiple=True, + callback=lambda _ctx, _param, values: [_parse_anchor(v) for v in values], + metavar="ADDRESS=X,Y", + help=( + "With --spin: this robot spins with its pivot on the known site " + "point X,Y mm, repeatable. Two or more put the calibration in the " + "site's own frame instead of free mode's; spread them across the " + "whole site, since anchors bunched together leave the far end's yaw " + "loose." ), ) @click.pass_context @@ -324,6 +364,9 @@ def _collect( push, spin, spin_radius, + drop_stations, + append, + anchors, ): if spin: given = [ @@ -343,10 +386,27 @@ def _collect( raise click.UsageError( f"--spin takes no {', '.join(given)}: the robots spin where they stand" ) - _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius) + _collect_spin( + ctx, + conn, + swarm_id, + site_name, + tag, + push, + spin_radius, + drop_stations, + append, + anchors, + ) return if spin_radius is not None: raise click.UsageError("--spin-radius goes with --spin") + if drop_stations: + raise click.UsageError("--drop-station goes with --spin") + if append is not None: + raise click.UsageError("--append goes with --spin") + if anchors: + raise click.UsageError("--anchor goes with --spin") if _devices(ctx): raise click.UsageError( "`dotbot swarm -d` picks the robots of push and collect --spin; a " @@ -488,16 +548,61 @@ def read_enter() -> None: ) -def _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius): +def _parse_anchor(value: str) -> tuple[str, tuple[float, float]]: + try: + address, point = value.split("=", 1) + x, y = (float(v) for v in point.split(",")) + except ValueError as exc: + raise click.BadParameter( + f"{value!r} is not ADDRESS=X,Y, a robot and its pivot's point in mm" + ) from exc + return address.strip().upper(), (x, y) + + +def _collect_spin( + ctx, + conn, + swarm_id, + site_name, + tag, + push, + spin_radius, + drop_stations=(), + append=None, + anchors=(), +): """`collect --spin`: spin the robots, solve their circles, save, report.""" from dotbot.calibration.conics import solve_calibration - from dotbot.calibration.lighthouse2 import write_calibration + from dotbot.calibration.lighthouse2 import load_calibration, write_calibration + from dotbot.calibration.multi_station import ( + Anchor, + CircleKey, + appendable_rounds, + multi_station_report, + ) from dotbot.calibration.spin import capture_spins, check_spin_robots, spin_report from dotbot.robots import ROBOT_DEFAULT, robot_geometry site, site_source = site_from_context(ctx, site_name) + if len(anchors) == 1: + raise click.UsageError("--anchor takes two or more robots") + earlier, round_ = [], 0 + if append is not None: + try: + base = load_calibration(append, site=site.name) + round_ = appendable_rounds(base) + except ValueError as exc: + raise click.ClickException(str(exc)) from exc + earlier = base.tracks + click.echo( + f"Appending round {round_} to {base.id8} ({len(earlier)} tracks from " + 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) @@ -524,6 +629,13 @@ def _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius): raise click.ClickException(f"spin refused:\n{refusal}") if not robots.robots: raise click.ClickException("no robot answered, so none can spin") + spinning = {address.upper(): address for address in robots.robots} + absent = [address for address, _ in anchors if address not in spinning] + if absent: + raise click.ClickException( + f"anchor robot(s) {', '.join(absent)} would not spin: they are " + "not among the robots ready for the spin" + ) click.echo( f"Spin calibration in site {site.name} (from {site_source}): " f"{len(robots.robots)} robot(s), spin radius {spin_radius:g} mm " @@ -534,23 +646,45 @@ def _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius): spins = capture_spins(client, robots, echo=click.echo) except RuntimeError as exc: raise click.ClickException(str(exc)) from exc - samples = [s for spin in spins.values() for s in spin.samples(spin_radius)] + anchored = [ + Anchor(CircleKey(spinning[address], round_), point) + for address, point in anchors + ] + samples = list(earlier) + [ + s for spin in spins.values() for s in spin.samples(spin_radius, round_) + ] try: - calibration, solutions, unsolved = solve_calibration( - samples, site, robot=ROBOT_DEFAULT, tag=tag or "" + calibration, joint, unsolved = solve_calibration( + samples, + site, + robot=ROBOT_DEFAULT, + tag=tag or "", + drop_stations=drop_stations, + anchors=anchored, ) except ValueError as exc: for line in spin_report(spins, robots.robots, {}, {}): click.echo(line) raise click.ClickException(f"no calibration written: {exc}") from exc - for line in spin_report(spins, robots.robots, solutions, unsolved): + for line in spin_report(spins, robots.robots, joint.solutions, unsolved): click.echo(line) + if len({s.station for s in samples}) > 1 or earlier or anchors: + click.echo("") + for line in multi_station_report(calibration, joint): + click.echo(line) path = write_calibration(calibration) - _, _, width, height = calibration.valid_mm - click.echo( - f"\nThe robots' field is {width} x {height} mm: the calibration's frame " - "is aligned to it, zero at its top-left, not at the site's anchor." - ) + width, height = joint.field_mm + if joint.anchored: + click.echo( + f"\nThe anchors put the calibration in site {site.name}'s own " + f"frame. Robots take fixes inside {list(calibration.valid_mm)}." + ) + else: + click.echo( + f"\nThe robots' field is {width} x {height} mm: the calibration's " + "frame is aligned to it, zero at its top-left, not at the site's " + f"anchor. Robots take fixes inside {list(calibration.valid_mm)}." + ) click.echo(f"Calibration saved to {path}") click.echo( f"Calibration id {calibration.id}, site {site.name}" @@ -560,9 +694,13 @@ def _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius): if push: _gated_push(client, calibration, devices=robots.robots, stop=True) click.echo( - "\nNext, make the field a site of its own (--size WxH centres it in a " - "bigger site):\n" - f" dotbot site init --from-calibration {calibration.id8}\n" + ( + "\n" + if joint.anchored + else "\nNext, make the field a site of its own (--size WxH " + "centres it in a bigger site):\n" + f" dotbot site init --from-calibration {calibration.id8}\n" + ) + ( "" if push @@ -622,8 +760,16 @@ def _collect_spin(ctx, conn, swarm_id, site_name, tag, push, spin_radius): "report another one." ), ) +@click.option( + "--dry-run", + is_flag=True, + help=( + "Print the messages the push would send, one per station, and send " + "nothing; needs no swarm connection." + ), +) @click.pass_context -def _push(ctx, calibration, conn, swarm_id, site_name, site_changed): +def _push(ctx, calibration, conn, swarm_id, site_name, site_changed, dry_run): from dotbot.calibration.lighthouse2 import ( read_calibration_file, resolve_calibration_path, @@ -635,17 +781,45 @@ def _push(ctx, calibration, conn, swarm_id, site_name, site_changed): loaded = read_calibration_file(path) except ValueError as exc: raise click.ClickException(str(exc)) from exc + if dry_run: + _print_messages(loaded) + return client = _swarmit_client(ctx, conn, swarm_id) with client: _gated_push(client, loaded, site_changed=site_changed, devices=_devices(ctx)) +def _print_messages(calibration): + """What a push of `calibration` sends, message by message.""" + from dotbot.calibration.lighthouse2 import ( + calibration_messages, + station_label, + station_mask, + ) + + try: + messages = calibration_messages(calibration) + except ValueError as exc: + raise click.ClickException(str(exc)) from exc + stations = sorted(calibration.stations, key=lambda s: s.index) + click.echo( + f"Would send {len(messages)} message(s), {sum(map(len, messages))} B: " + f"id {calibration.id8}, site {calibration.site.name}, station mask " + f"0x{station_mask(calibration):04X}" + ) + for station, message in zip(stations, messages): + click.echo( + f" {station_label(station.index)}, rectangle {list(station.valid_mm)}: " + f"{message.hex()}" + ) + + def _gated_push(client, calibration, site_changed=False, devices=None, stop=False): """Check the robots' device info, push, and print the stale-id worklist. With `stop`, the named `devices` still in their app are stopped first. """ - from dotbot.calibration.lighthouse2 import calibration_payload + from dotbot.calibration.lighthouse2 import calibration_payload, station_label from dotbot.calibration.push import ( PushRefused, gate_push, @@ -678,8 +852,12 @@ def _gated_push(client, calibration, site_changed=False, devices=None, stop=Fals f"{len(check.other_site)} robot(s) move to site " f"{calibration.site.name} (--site-changed)." ) + stations = ", ".join( + station_label(s.index) + for s in sorted(calibration.stations, key=lambda s: s.index) + ) click.echo( - f"Sending {len(calibration.stations)} calibration matrix/matrices " + f"Sending {stations} " f"({len(payload)} B, id {calibration.id8}, site {calibration.site.name}) " f"to the swarm; {len(check.stale)} robot(s) hold another id..." ) @@ -692,7 +870,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: " @@ -702,6 +880,93 @@ def _gated_push(client, calibration, site_changed=False, devices=None, stop=Fals click.echo(f"Every robot reports {calibration.id8}.") +@cmd.command( + name="show", + help=( + "Print a saved LH2 calibration: its stations and their rectangles, " + "the links tying them and the predicted error map, from the file " + "alone (no robot, no broker). Takes a file path, its tag or its id " + "prefix." + ), +) +@click.argument("calibration") +@click.option( + "--site", + "site_name", + 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, 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 ( + appendable_rounds, + error_map_from_calibration, + multi_station_report, + next_round, + next_round_lines, + ) + + site, _ = site_from_context(ctx, site_name) + try: + path = resolve_calibration_path(calibration, site=site) + loaded = read_calibration_file(path) + except ValueError as exc: + raise click.ClickException(str(exc)) from exc + rounds = sorted({t.round for t in loaded.tracks}) + click.echo(f"Calibration {loaded.id}, site {loaded.site.name}, {path}") + click.echo( + f"Created {loaded.created_at or 'at an unknown time'}" + + (f", tag {loaded.tag!r}" if loaded.tag else "") + + ( + f", {len(loaded.tracks)} tracks over {len(rounds)} round(s)" + if loaded.tracks + else "" + ) + + f", fence {list(loaded.valid_mm)}" + ) + 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): try: x, y = (float(v) for v in value.split(",")) @@ -716,7 +981,9 @@ def _parse_shift(_ctx, _param, value): "Re-express a saved calibration in another site's frame, without a " "capture: shift (and turn) every placement's points, re-solve every " "station from the stored samples, and save a new file with a new id " - "under ~/.dotbot/calibrations//." + "under ~/.dotbot/calibrations//. A spin calibration has no " + "placements: it is moved as one rigid body instead, every station " + "with it, as the site editor places one." ), ) @click.argument("calibration") @@ -742,7 +1009,8 @@ def _parse_shift(_ctx, _param, value): show_default=True, help=( "Degrees to turn the points about the first placement's first point " - "before shifting; positive turns x toward y." + "(a spin calibration: about its zero) before shifting; positive turns " + "x toward y." ), ) @click.pass_context @@ -757,6 +1025,13 @@ def _reframe(ctx, calibration, site_name, shift, rotate): resolve_calibration_path, write_calibration, ) + from dotbot.calibration.placement import ( + Rigid2D, + is_free_mode, + place_calibration, + save_placed, + site_tags, + ) site, _ = site_from_context(ctx, site_name) if site.extent_mm is None and not site.anchor: @@ -766,10 +1041,19 @@ def _reframe(ctx, calibration, site_name, shift, rotate): ) try: source = read_calibration_file(resolve_calibration_path(calibration)) - reframed = reframe_calibration(source, site, shift, rotate) + if is_free_mode(source): + reframed = place_calibration( + source, + site, + Rigid2D(shift[0], shift[1], rotate), + taken_tags=site_tags(site.name), + ) + reframed, path = save_placed(source, reframed) + else: + reframed = reframe_calibration(source, site, shift, rotate) + path = write_calibration(reframed) except ValueError as exc: raise click.ClickException(str(exc)) from exc - path = write_calibration(reframed) click.echo( f"Reframed {source.id8} (site {source.site.name}) into site {site.name}: " f"shift {shift[0]:g},{shift[1]:g} mm, rotate {rotate:g} deg" @@ -798,3 +1082,131 @@ def _reframe(ctx, calibration, site_name, shift, rotate): ) click.echo(f"\nCalibration saved to {path}") click.echo(f"Calibration id {reframed.id}, site {site.name}") + + +def _parse_station_rect(_ctx, _param, values): + out = [] + for value in values: + index, sep, rect = value.partition("=") + try: + index = int(index) + except ValueError: + index = -1 + if not sep or not rect.strip() or not 0 <= index <= 15: + raise click.BadParameter( + f"{value!r}: takes `N=x,y,w,h` (mm) or `N=`, N a station 0 to 15" + ) + out.append((index, rect.strip())) + return out + + +@cmd.command( + name="rebox", + help=( + "Change where stations of a saved calibration are trusted: save a " + "new file, with a new id, whose named stations have the given " + "rectangles and everything else, homographies included, as it was. " + "The calibration must be in the site's frame (placed, not free " + "mode). Sends nothing; push the new id when it looks right." + ), +) +@click.argument("calibration") +@click.option( + "--station", + "stations", + multiple=True, + required=True, + callback=_parse_station_rect, + metavar="N=RECT", + help=( + "Station N's new rectangle: `x,y,w,h` in mm, or an area of the site " + "such as `field+staging`. Repeat for each station to change; the " + "others keep theirs." + ), +) +@click.option( + "--tag", + default=None, + help="Tag of the new calibration. Defaults to the source's, suffixed -boxes.", +) +@click.option( + "--site", + "site_name", + default=None, + help="The site to look the id up under. Defaults to `site` in the dotbot config.", +) +@click.pass_context +def _rebox(ctx, calibration, stations, tag, site_name): + if _devices(ctx): + raise click.UsageError("rebox sends nothing, so it takes no `dotbot swarm -d`") + from dotbot.calibration.lighthouse2 import ( + read_calibration_file, + resolve_calibration_path, + station_label, + ) + from dotbot.calibration.placement import save_new, site_tags + from dotbot.calibration.station_boxes import ( + coverage_warnings, + push_line, + reboxed, + ) + + site, _ = site_from_context(ctx, site_name) + rects = {} + for index, spec in stations: + if index in rects: + raise click.BadParameter( + f"station {index} is given twice", param_hint="--station" + ) + try: + area = site.registry().resolve(spec) + except ValueError as exc: + raise click.BadParameter(str(exc), param_hint="--station") from exc + rects[index] = (area.x, area.y, area.x + area.w, area.y + area.h) + try: + path = resolve_calibration_path(calibration, site=site) + source = read_calibration_file(path) + boxed = reboxed( + source, + site, + rects, + tag=tag, + taken_tags=site_tags(site.name, _site_calibration_files(site)), + ) + boxed, saved = save_new(boxed) + except ValueError as exc: + raise click.ClickException(str(exc)) from exc + click.echo(f"Reboxed {source.id8} (site {source.site.name}):") + before = {s.index: s.valid_mm for s in source.stations} + for station in sorted(boxed.stations, key=lambda s: s.index): + x0, y0, x1, y1 = station.valid_mm + was = before[station.index] + change = "" if was != station.valid_mm else ", unchanged" + click.echo( + f" {station_label(station.index)}: {x0},{y0},{x1 - x0},{y1 - y0} " + f"(x,y,w,h; valid_mm {list(station.valid_mm)}, was {list(was)}{change})" + ) + for line in coverage_warnings(boxed): + click.echo(f"warning: {line}", err=True) + click.echo(f"\nCalibration saved to {saved}") + click.echo( + f"Calibration id {boxed.id}" + + (f", tag {boxed.tag!r}" if boxed.tag else "") + + f", site {boxed.site.name}" + ) + click.echo(f"Send it with:\n {push_line(boxed)}") + + +def _site_calibration_files(site): + """The calibration files `resolve_calibration_path` searches for `site`.""" + from dotbot.calibration.lighthouse2 import ( + CALIBRATION_TOML_GLOB, + calibration_folders, + calibration_root, + ) + + return [ + path + for folder, prefix in calibration_folders(site, calibration_root()) + for path in sorted(folder.glob(prefix + CALIBRATION_TOML_GLOB)) + ] diff --git a/dotbot/config.py b/dotbot/config.py index 10074b49..e2d9f1cc 100644 --- a/dotbot/config.py +++ b/dotbot/config.py @@ -29,10 +29,11 @@ import difflib import os +import re import tomllib from dataclasses import dataclass from pathlib import Path -from typing import Annotated, Any, Mapping, Optional +from typing import Annotated, Any, Literal, Mapping, Optional from pydantic import ( AfterValidator, @@ -167,6 +168,38 @@ class AreaSection(_Strict): role: Role | None = None +class WallSection(_Strict): + """One `[[walls]]` entry: a polyline robots cannot cross, in frame mm.""" + + name: str | None = None + points: list[tuple[int, int]] = Field(min_length=2) + + +class ObstacleSection(_Strict): + """One `[[obstacles]]` entry: a polygon robots cannot enter, in frame mm. + + The last point joins the first. + """ + + name: str | None = None + points: list[tuple[int, int]] = Field(min_length=3) + + +ObjectKind = Literal["charger", "dock", "landmark", "camera"] + + +class ObjectSection(_Strict): + """One `[objects.]` table: a thing on the floor at a pose, frame mm. + + `heading_deg` follows the robots' convention. + """ + + kind: ObjectKind + x: int + y: int + heading_deg: float = 0.0 + + def _is_simulator(conn: str) -> bool: return conn.strip().lower() in ("simulator", "sim") @@ -217,6 +250,29 @@ class SiteSection(_Strict): extent_mm: tuple[int, int] | None = None connection: ConnectionSection | None = None areas: dict[str, AreaSection] = Field(default_factory=dict) + # LH2 base stations by serial, each with the channel it is set to + stations: dict[str, int] = Field(default_factory=dict) + # The photodiode's circle when a robot spins in place on this floor + spin_radius_mm: float | None = Field(default=None, ge=1.0, le=200.0) + walls: list[WallSection] = Field(default_factory=list) + obstacles: list[ObstacleSection] = Field(default_factory=list) + objects: dict[str, ObjectSection] = Field(default_factory=dict) + + @model_validator(mode="after") + def _stations(self) -> SiteSection: + for serial, channel in self.stations.items(): + if not re.fullmatch(r"[0-9A-F]{8}", serial): + raise ValueError(f"station serial {serial!r} is not 8 upper-case hex digits") + if not 1 <= channel <= 16: + raise ValueError(f"station {serial} on channel {channel}, outside 1 to 16") + channels = list(self.stations.values()) + shared = sorted({c for c in channels if channels.count(c) > 1}) + if shared: + raise ValueError( + "two stations on one channel merge into one index: channel " + + ", ".join(str(c) for c in shared) + ) + return self @model_validator(mode="after") def _one_field(self) -> SiteSection: diff --git a/dotbot/console-web/dev/fake_swarmit_server.py b/dotbot/console-web/dev/fake_swarmit_server.py index f961d2ba..aa54e305 100644 --- a/dotbot/console-web/dev/fake_swarmit_server.py +++ b/dotbot/console-web/dev/fake_swarmit_server.py @@ -174,17 +174,19 @@ def device_info(addr: str) -> dict: seed = _seed(addr) name, digest, size = IMAGES[seed % len(IMAGES)] # A minority are uncalibrated, which is the state an operator acts on. - homographies = 0 if seed % 9 == 0 else (2 if seed % 5 == 0 else 1) - flags = LH2_FLAG_VALID | LH2_FLAG_FROM_FLASH if homographies else 0 - noun = "basestation" if homographies == 1 else "basestations" + mask = 0 if seed % 9 == 0 else (0b101 if seed % 5 == 0 else 0b1) + flags = LH2_FLAG_VALID | LH2_FLAG_FROM_FLASH if mask else 0 + stations = [i for i in range(16) if mask >> i & 1] summary = ( "uncalibrated" - if not homographies - else f"{homographies} {noun} (valid, from flash)" + if not mask + else f"station{'s' if len(stations) > 1 else ''} " + f"{', '.join(map(str, stations))} (channel{'s' if len(stations) > 1 else ''} " + f"{', '.join(str(i + 1) for i in stations)}; valid, from flash)" ) return { # A few run sandbox firmware too old for this host's calibrations. - "info_version": 2 if seed % 7 == 0 else 3, + "info_version": 3 if seed % 7 == 0 else 4, "info_gen": 4, "boot_count": 2 + seed % 30, "uptime_s": 60 + seed % 9000, @@ -198,10 +200,10 @@ def device_info(addr: str) -> dict: "image_digest": digest, "image_name": name, "image_version": "", - "lh2_homography_count": homographies, + "lh2_station_mask": mask, "lh2_flags": flags, - "lh2_site_name": FAKE_SITE if homographies else "", - "lh2_calibration_id": FAKE_CALIBRATION_ID if homographies else "", + "lh2_site_name": FAKE_SITE if mask else "", + "lh2_calibration_id": FAKE_CALIBRATION_ID if mask else "", "lh2_summary": summary, "raw": "8f0104" + f"{seed:08x}" * 4, } diff --git a/dotbot/console-web/site-editor.html b/dotbot/console-web/site-editor.html new file mode 100644 index 00000000..22e934fc --- /dev/null +++ b/dotbot/console-web/site-editor.html @@ -0,0 +1,18 @@ + + + + + + DotBot Site Editor + + + + + +
+ + + diff --git a/dotbot/console-web/src/App.tsx b/dotbot/console-web/src/App.tsx index 0475962d..f7e1757a 100644 --- a/dotbot/console-web/src/App.tsx +++ b/dotbot/console-web/src/App.tsx @@ -1,4 +1,4 @@ -import React, { useCallback, useEffect, useMemo, useRef, useState } from "react"; +import React, { Suspense, lazy, useCallback, useEffect, useMemo, useRef, useState } from "react"; import { clearWaypoints, fetchBuild, fetchConnection, putWaypointBatches } from "./api"; import type { WaypointsSent } from "./api"; @@ -45,6 +45,7 @@ import { withView, } from "./savedView"; import { SetupCard } from "./SetupCard"; +import { SITE_EDITOR_BASE, siteEditable } from "./siteEditorPanel"; import { SpreadPanel, SpreadRun } from "./SpreadPanel"; import { describeHazards, hazardCount, planSpread, spreadColor, swap } from "./spread"; import { WaypointSettings, batchFields, loadWaypointSettings, saveWaypointSettings } from "./arrival"; @@ -133,9 +134,17 @@ function robotsNotice(ids: string[], what: string): string { return `${ids.length} robot${ids.length === 1 ? "" : "s"} ${what}: ${names.join(", ")}${more}`; } +// Loaded only when opened +const SiteEditor = lazy(() => import("./siteEditor/SiteEditor").then((m) => ({ default: m.SiteEditor }))); + export const App: React.FC = () => { const { bots, site, cameras, cameraDetections, session, setSession, viewport, wsUp } = useFleet(); + const [canEditSite, setCanEditSite] = useState(false); + const [editingSite, setEditingSite] = useState(false); + useEffect(() => { + void siteEditable().then(setCanEditSite); + }, [site?.name]); // ?theme=dark|light presets the theme (handy for dev/screenshots). const [theme, setTheme] = useState<"dark" | "light">(() => new URLSearchParams(window.location.search).get("theme") === "light" ? "light" : "dark", @@ -474,10 +483,12 @@ export const App: React.FC = () => { // The shortcuts panel: its key opens it with nothing selected and closes // it again; Escape closes it; a key typed into a field is left alone. const [shortcuts, setShortcuts] = useState(false); + // The console's keys, all but the stop key, stay off under the site editor + const keysOff = shortcuts || editingSite; const nothingSelected = selection.size === 0; useEffect(() => { const onKey = (e: KeyboardEvent) => { - if (typingIn(e.target)) return; + if (editingSite || typingIn(e.target)) return; if (e.key === SHORTCUTS_KEY) { if (shortcuts) setShortcuts(false); else if (nothingSelected) setShortcuts(true); @@ -490,12 +501,12 @@ export const App: React.FC = () => { }; window.addEventListener("keydown", onKey); return () => window.removeEventListener("keydown", onKey); - }, [shortcuts, nothingSelected]); + }, [shortcuts, nothingSelected, editingSite]); // Each side panel's key collapses it or expands it again, once per press. useEffect(() => { const onKey = (e: KeyboardEvent) => { - if (shortcuts || e.repeat || typingIn(e.target)) return; + if (keysOff || e.repeat || typingIn(e.target)) return; if (pressed(e, ACTION_KEY.leftPanel)) setRailCollapsed((c) => !c); else if (pressed(e, ACTION_KEY.rightPanel)) setRightCollapsed((c) => !c); else return; @@ -503,7 +514,7 @@ export const App: React.FC = () => { }; window.addEventListener("keydown", onKey); return () => window.removeEventListener("keydown", onKey); - }, [shortcuts]); + }, [keysOff]); // Start and Stop act on the selection at once. With none they act on the // whole fleet, on a second press of the same key or button, or Enter; Esc @@ -529,6 +540,7 @@ export const App: React.FC = () => { const onKey = (e: KeyboardEvent) => { if (e.repeat || typingIn(e.target)) return; if (pressed(e, ACTION_KEY.stop)) requestTestbed("stop"); + else if (editingSite) return; else if (!shortcuts && pressed(e, ACTION_KEY.start)) requestTestbed("start"); // Enter confirms whatever holds the focus, so it never also presses it. else if (e.key === "Enter" && confirmTestbed()) e.stopImmediatePropagation(); @@ -538,7 +550,7 @@ export const App: React.FC = () => { }; window.addEventListener("keydown", onKey, true); return () => window.removeEventListener("keydown", onKey, true); - }, [shortcuts, requestTestbed, confirmTestbed, cancelTestbed]); + }, [shortcuts, editingSite, requestTestbed, confirmTestbed, cancelTestbed]); // replace = set selection to ids · toggle = flip each id · add = union (range select) const onSelect = useCallback((ids: string[], mode: "replace" | "toggle" | "add") => { @@ -730,7 +742,7 @@ export const App: React.FC = () => { const queued = pending.length; useEffect(() => { const onKey = (e: KeyboardEvent) => { - if (shortcuts || typingIn(e.target) || !pressed(e, ACTION_KEY.go)) return; + if (keysOff || typingIn(e.target) || !pressed(e, ACTION_KEY.go)) return; if (selectedCount === 0) showToast("Nothing selected"); else if (drivableCount === 0) showToast("Not drivable"); else if (anyAuto) onStopNav(); @@ -742,7 +754,7 @@ export const App: React.FC = () => { }; window.addEventListener("keydown", onKey); return () => window.removeEventListener("keydown", onKey); - }, [shortcuts, selectedCount, drivableCount, anyAuto, queued, onGo, onStopNav, showToast]); + }, [keysOff, selectedCount, drivableCount, anyAuto, queued, onGo, onStopNav, showToast]); const onClearQueue = useCallback(() => { setPlanned((prev) => prev.filter((m) => m.key !== selKey)); @@ -781,7 +793,7 @@ export const App: React.FC = () => { const [poseMode, setPoseMode] = useState(false); useEffect(() => { const onKey = (e: KeyboardEvent) => { - if (shortcuts || e.repeat || typingIn(e.target)) return; + if (keysOff || e.repeat || typingIn(e.target)) return; if (pressed(e, ACTION_KEY.poseMode)) { setPoseMode((on) => !on); e.preventDefault(); @@ -794,7 +806,7 @@ export const App: React.FC = () => { }; window.addEventListener("keydown", onKey); return () => window.removeEventListener("keydown", onKey); - }, [shortcuts, queued, onRemovePending, showToast]); + }, [keysOff, queued, onRemovePending, showToast]); // Recently-completed missions: a bot flipping AUTO -> MANUAL just arrived. const [doneMissions, setDoneMissions] = useState([]); @@ -1191,6 +1203,16 @@ export const App: React.FC = () => { )} setShortcuts(false)} /> + {editingSite && ( +
+ + setEditingSite(false)} /> + +
+ )} { site={site} hiddenAreas={hiddenAreas} onAreaToggle={onAreaToggle} + onEditSite={canEditSite ? () => setEditingSite(true) : undefined} onZoom={zoomTo} layers={layers} layerRows={layerRows} diff --git a/dotbot/console-web/src/BotMarker.tsx b/dotbot/console-web/src/BotMarker.tsx index fdfb0240..a49114df 100644 --- a/dotbot/console-web/src/BotMarker.tsx +++ b/dotbot/console-web/src/BotMarker.tsx @@ -154,6 +154,8 @@ export interface BotMarkerProps { batteryBars: boolean; /** What the body is filled with: the swarmit state, or the LED colour. */ colorMode: BodyColorMode; + /** The site of the robot's calibration when it is not the map's, "" otherwise. */ + otherSite?: string; attach: (id: string, el: HTMLElement | null) => void; onPointerDown: (e: React.PointerEvent, id: string) => void; onHover: React.Dispatch>; @@ -174,6 +176,7 @@ export const BotMarker = React.memo(function BotMarker({ botCount, batteryBars, colorMode, + otherSite = "", attach, onPointerDown, onHover, @@ -235,6 +238,30 @@ export const BotMarker = React.memo(function BotMarker({ > + {/* calibrated for another site: the position is in another frame */} + {otherSite && ( +
+ ! +
+ )} {/* selection ring, hugging the board */} {selected && (
= ({ bot, ctx }) => { const slipping = bot.link === "stale" || bot.link === "lost"; - const differs = calibrationDiffers(bot, ctx.calibrationId); + const differs = calibrationMismatch(bot, ctx) !== "none"; const old = firmwareTooOld(bot); const bootloader = bootloaderLabel(bot); if (!bootloader && !slipping && !differs && !old) return null; @@ -104,11 +106,7 @@ const Chips: React.FC<{ bot: UnifiedBot; ctx: FleetContext }> = ({ bot, ctx }) = {bootloader && bl {bootloader}} {slipping && {reportsLabel(bot, ctx.now)}} {old && } - {differs && ( - - cal differs - - )} + {differs && }
); }; @@ -266,15 +264,10 @@ export const GridView: React.FC = ({ bots, selection, onSelect, c - differs - - ) - } + title={calibrationDetail(b) || undefined} + extra={} > - {calibrationLabel(b) || dash} + {calibrationSiteLabel(b) || dash} {b.deviceType} {b.severity !== "normal" && {b.resetCause ?? b.severity}} diff --git a/dotbot/console-web/src/Inspector.tsx b/dotbot/console-web/src/Inspector.tsx index 3d628f75..0173a869 100644 --- a/dotbot/console-web/src/Inspector.tsx +++ b/dotbot/console-web/src/Inspector.tsx @@ -1,6 +1,6 @@ import React, { useState } from "react"; -import { reportsWire } from "./botFacts"; +import { heldCalibrationId, reportsWire } from "./botFacts"; import { stateLabel } from "./viewChrome"; import { CameraDetection, CameraRobot, REPORTS_LABEL, UnifiedBot } from "./types"; @@ -30,6 +30,24 @@ export function formatLh2(bot: UnifiedBot): string { return bot.swarmit?.info?.lh2_summary ?? "unknown (no device info)"; } +/** Matches swarmit's LH2_FLAG_SENSOR_FAILED: the bot's LH2 receiver did not initialise this boot. */ +const LH2_FLAG_SENSOR_FAILED = 1 << 2; + +/** "1204, 877", or why there is no position. */ +export function formatPosition(bot: UnifiedBot): string { + if (bot.position) return `${Math.round(bot.position.x)}, ${Math.round(bot.position.y)}`; + if ((bot.swarmit?.info?.lh2_flags ?? 0) & LH2_FLAG_SENSOR_FAILED) return "LH2 sensor failed"; + return "no fix"; +} + +/** The site the held calibration was made for and its short id; null without device info. */ +export function formatLh2Site(bot: UnifiedBot): string | null { + const info = bot.swarmit?.info; + if (!info) return null; + const id = heldCalibrationId(bot).slice(0, 8); + return id ? `${info.lh2_site_name || "none"} (id ${id})` : "none"; +} + /** * The body heading, and where it came from. A travel bearing is the robot's * direction of motion, so it is its heading only while it drives straight; @@ -81,7 +99,7 @@ export function infoText(bot: UnifiedBot): string { out.push(`Reports ${REPORTS_LABEL[bot.link]} (${reportsWire(bot)})`); out.push(`Battery ${bot.battery.toFixed(2)}V`); out.push( - `Position ${bot.position ? `${Math.round(bot.position.x)}, ${Math.round(bot.position.y)}` : "no fix"}`, + `Position ${formatPosition(bot)}`, ); out.push(`Heading ${formatHeading(bot)}`); if (info) { @@ -99,6 +117,8 @@ export function infoText(bot: UnifiedBot): string { } out.push(""); out.push(`LH2 calibration ${formatLh2(bot)}`); + const lh2Site = formatLh2Site(bot); + if (lh2Site !== null) out.push(` site ${lh2Site}`); if (sw?.reset_reason !== undefined) { out.push(""); out.push(`Last reset ${bot.resetCause}`); @@ -188,10 +208,7 @@ const Card: React.FC<{ bot: UnifiedBot; camera: CameraRobot | null; cameras: boo - + {cameras && } @@ -215,6 +232,7 @@ const Card: React.FC<{ bot: UnifiedBot; camera: CameraRobot | null; cameras: boo
+ {formatLh2Site(bot) !== null && } {sw?.reset_reason !== undefined && ( <> diff --git a/dotbot/console-web/src/ListView.tsx b/dotbot/console-web/src/ListView.tsx index 0ce78055..9bfd55e1 100644 --- a/dotbot/console-web/src/ListView.tsx +++ b/dotbot/console-web/src/ListView.tsx @@ -4,7 +4,7 @@ import { appLabel, areaLabel, bootloaderLabel, - calibrationDiffers, + calibrationDetail, calibrationLabel, detailText, FleetContext, @@ -19,8 +19,8 @@ import { rowOpacity } from "./link"; import { store } from "./persisted"; import { UnifiedBot } from "./types"; import { - Badge, BatteryCell, + CalibrationBadge, FilterBar, HeadingGlyph, LedDot, @@ -118,12 +118,10 @@ export const COLUMNS: Column[] = [ shown: true, cell: (b, ctx) => ( - {calibrationLabel(b) || dash} - {calibrationDiffers(b, ctx.calibrationId) && ( - - differs - - )} + + {calibrationLabel(b) || dash} + + ), }, diff --git a/dotbot/console-web/src/LocalizationPanel.tsx b/dotbot/console-web/src/LocalizationPanel.tsx index 175c14a6..2b94c98a 100644 --- a/dotbot/console-web/src/LocalizationPanel.tsx +++ b/dotbot/console-web/src/LocalizationPanel.tsx @@ -6,6 +6,7 @@ import { cameraStatusRows, coverageLabel, extentLabel, + servedCalibrationLabel, stationRows, stationsSummary, } from "./localization"; @@ -186,7 +187,7 @@ export const LocalizationPanel: React.FC = ({
- {coverage.unknown ? "not compared" : coverage.id.slice(0, 8)} + {coverage.unknown ? "not compared" : servedCalibrationLabel(coverage, session?.site || site?.name || "")}
{coverage.unknown diff --git a/dotbot/console-web/src/MapView.tsx b/dotbot/console-web/src/MapView.tsx index ee72431b..5376e7cf 100644 --- a/dotbot/console-web/src/MapView.tsx +++ b/dotbot/console-web/src/MapView.tsx @@ -2,6 +2,7 @@ import React, { useCallback, useId, useMemo, useRef, useState } from "react"; import { cameraStreamUrl } from "./api"; import { areaColor } from "./areaColor"; +import { OBJECT_COLOUR, OBJECT_SIZE_MM, facing } from "./siteObjects"; import { CalibrationLayer } from "./CalibrationLayer"; import { calibrationSpans, hatchBox, spanTitle } from "./calibrationSpan"; import { @@ -31,6 +32,7 @@ import { ticksInSite, } from "./grid"; import { hasHeading } from "./BotGlyph"; +import { calibrationOtherSite } from "./botFacts"; import { BotMarker, MARKER_SCALE_VAR, WAYPOINT_MIN_PX, botDraw as drawBot } from "./BotMarker"; import { useMarkerPerMm } from "./markerScale"; import { BodyColorMode, DEFAULT_BODY_COLOR_MODE } from "./bodyColor"; @@ -1213,6 +1215,66 @@ export const MapView: React.FC = (props) => { {a.name} ))} + {/* The site's walls and obstacles */} + {(props.site?.obstacles ?? []).map((o, i) => ( + + {o.name || "obstacle"} + + ))} + {(props.site?.walls ?? []).map((w, i) => ( + + {w.name || "wall"} + + ))} + {(props.site?.objects ?? []).map((o) => { + const { fx, fy } = areaToFraction({ x: o.x, y: o.y }, props.viewport); + const cx = fx * boxW; + const cy = fy * boxH; + const r = (OBJECT_SIZE_MM / 2) * (boxW / props.viewport.w); + const [dx, dy] = facing(o.heading_deg); + return ( + + + {`${o.name} (${o.kind})`} + + + + ); + })} {/* The loaded calibration: each placement's span outlined, and the rest of the site hatched, where positions are extrapolated. The hatch is the site masked by the spans, so @@ -1569,6 +1631,7 @@ export const MapView: React.FC = (props) => { botCount={props.bots.length} batteryBars={props.layers.batteryBars} colorMode={colorMode} + otherSite={calibrationOtherSite(b, props.site)} attach={attachBot} onPointerDown={onBotPointerDown} onHover={setHoverId} diff --git a/dotbot/console-web/src/RightPane.tsx b/dotbot/console-web/src/RightPane.tsx index 56284d42..bbd93b04 100644 --- a/dotbot/console-web/src/RightPane.tsx +++ b/dotbot/console-web/src/RightPane.tsx @@ -512,6 +512,8 @@ interface RightPaneProps { site: Site | null; hiddenAreas: Set; onAreaToggle: (name: string) => void; + // Open the site editor, when the controller lets this browser edit its site + onEditSite?: () => void; // Zoom the map to a named area: the row is where the area's name lives. onZoom?: (name: string) => void; layers: Layers; @@ -663,7 +665,27 @@ export const RightPane: React.FC = (props) => { /> )} -
Areas
+
+
Areas
+ {props.onEditSite && ( + + )} +
{siteAreas.length === 0 && (
This site defines no areas. diff --git a/dotbot/console-web/src/barrierLayer.test.tsx b/dotbot/console-web/src/barrierLayer.test.tsx new file mode 100644 index 00000000..6f7f6c14 --- /dev/null +++ b/dotbot/console-web/src/barrierLayer.test.tsx @@ -0,0 +1,52 @@ +import { render, screen } from "@testing-library/react"; +import { describe, expect, it } from "vitest"; + +import { MapView } from "./MapView"; +import type { Site } from "./types"; + +const SITE: Site = { + name: "c405-arena", + anchor: "", + extent_mm: [2000, 4000], + areas: [], + walls: [{ name: "door", points: [[0, 0], [0, 4000]] }], + obstacles: [{ name: "", points: [[1500, 2500], [1700, 2500], [1700, 2700]] }], + objects: [{ name: "charger-1", kind: "charger", x: 800, y: 3800, heading_deg: 180 }], +}; + +describe("MapView barriers and objects", () => { + it("draws the site's walls, obstacles and objects", () => { + render( + {}} + onGeom={() => {}} + onSelect={() => {}} + onAddWaypoint={() => {}} + site={SITE} + onZoom={() => {}} + />, + ); + expect(screen.getByTestId("wall-door").getAttribute("points")?.split(" ")).toHaveLength(2); + expect(screen.getByTestId("obstacle-0").getAttribute("points")?.split(" ")).toHaveLength(3); + expect(screen.getByTestId("object-charger-1")).toHaveTextContent("charger-1 (charger)"); + }); +}); diff --git a/dotbot/console-web/src/botChrome.test.tsx b/dotbot/console-web/src/botChrome.test.tsx index 100172df..a0a6b393 100644 --- a/dotbot/console-web/src/botChrome.test.tsx +++ b/dotbot/console-web/src/botChrome.test.tsx @@ -521,3 +521,30 @@ describe("the possible footprint", () => { expect(layer("a", "reach")!.getAttribute("fill")).not.toBe("none"); }); }); + +describe("a robot calibrated for another site", () => { + const held = (site: string) => + ({ + device: "DotBotV3", + status: "Running", + battery: 2900, + pos_x: 0, + pos_y: 0, + info: { info_version: 4, lh2_calibration_id: "288a810d1234abcd", lh2_site_name: site }, + }) as UnifiedBot["swarmit"]; + + it("is marked on the map, and a robot of this site or of no known site is not", () => { + render( + , + ); + expect(screen.getByTestId("other-site-away").getAttribute("title")).toContain("workshop"); + expect(screen.queryByTestId("other-site-home")).toBeNull(); + expect(screen.queryByTestId("other-site-none")).toBeNull(); + }); +}); diff --git a/dotbot/console-web/src/botFacts.ts b/dotbot/console-web/src/botFacts.ts index a47cd6ca..2b2ac8ad 100644 --- a/dotbot/console-web/src/botFacts.ts +++ b/dotbot/console-web/src/botFacts.ts @@ -38,9 +38,26 @@ export function firmwareTooOld(bot: UnifiedBot): boolean { return !!info && (info.info_version ?? 0) < DEVICE_INFO_VERSION_MIN; } +/** The full LH2 calibration id the robot holds, "" for none or unknown. An + * empty station mask means no homography is held, whatever id the device + * info still carries. */ +export function heldCalibrationId(bot: UnifiedBot): string { + const info = bot.swarmit?.info; + if (!info || info.lh2_station_mask === 0) return ""; + return info.lh2_calibration_id ?? ""; +} + /** The LH2 calibration id the robot holds: its first 8 characters, "" for none or unknown. */ export function calibrationLabel(bot: UnifiedBot): string { - return (bot.swarmit?.info?.lh2_calibration_id ?? "").slice(0, 8); + return heldCalibrationId(bot).slice(0, 8); +} + +/** "288a810d · virmond": the held calibration's short id and its site, "" for none or unknown. */ +export function calibrationSiteLabel(bot: UnifiedBot): string { + const id = calibrationLabel(bot); + if (!id) return ""; + const site = bot.swarmit?.info?.lh2_site_name ?? ""; + return site ? `${id} · ${site}` : id; } /** Whether the robot reports a calibration other than the one the controller serves. */ @@ -48,7 +65,47 @@ export function calibrationDiffers(bot: UnifiedBot, calibrationId: string): bool if (!calibrationId || firmwareTooOld(bot)) return false; const info = bot.swarmit?.info; if (!info) return false; - return (info.lh2_calibration_id ?? "").toLowerCase() !== calibrationId.toLowerCase(); + return heldCalibrationId(bot).toLowerCase() !== calibrationId.toLowerCase(); +} + +/** The site of the calibration the robot holds when it is not `site`, "" otherwise or when either is unknown. */ +export function calibrationOtherSite(bot: UnifiedBot, site: Site | null): string { + const info = bot.swarmit?.info; + if (!info || firmwareTooOld(bot) || !heldCalibrationId(bot)) return ""; + const held = info.lh2_site_name ?? ""; + const active = site?.name ?? ""; + if (!held || !active || held.toLowerCase() === active.toLowerCase()) return ""; + return held; +} + +/** How the robot's calibration differs from what the controller serves: a + * calibration of another site puts its positions in another frame, which + * outranks a different id of the same site. */ +export type CalibrationMismatch = "none" | "other-site" | "other-id"; + +export function calibrationMismatch(bot: UnifiedBot, ctx: Pick): CalibrationMismatch { + if (calibrationOtherSite(bot, ctx.site)) return "other-site"; + if (calibrationDiffers(bot, ctx.calibrationId)) return "other-id"; + return "none"; +} + +/** The stations whose homography the robot holds, from its station mask: [0, 1]. */ +export function calibrationStations(bot: UnifiedBot): number[] | null { + const mask = bot.swarmit?.info?.lh2_station_mask; + if (mask === undefined || mask === null) return null; + const out: number[] = []; + for (let i = 0; i < 32; i++) if (mask & (1 << i)) out.push(i); + return out; +} + +/** "virmond · 288a810d1234abcd · stations 0, 1": the held calibration in full, "" for none or unknown. */ +export function calibrationDetail(bot: UnifiedBot): string { + const id = heldCalibrationId(bot); + if (!id) return ""; + const parts = [bot.swarmit?.info?.lh2_site_name ?? "", id]; + const stations = calibrationStations(bot); + if (stations) parts.push(stations.length ? `stations ${stations.join(", ")}` : "no stations"); + return parts.filter(Boolean).join(" · "); } /** "1204, 877 mm", or "" with no fix. */ @@ -139,7 +196,9 @@ export function reportsDetail(bot: UnifiedBot, now: number): string { export function warnings(bot: UnifiedBot, ctx: FleetContext): string[] { const out: string[] = []; if (firmwareTooOld(bot)) out.push("bootloader too old for this controller: reflash swarmit-sandbox"); - else if (calibrationDiffers(bot, ctx.calibrationId)) { + else if (calibrationOtherSite(bot, ctx.site)) { + out.push(`holds a calibration of site ${calibrationOtherSite(bot, ctx.site)}: its position is in another frame`); + } else if (calibrationDiffers(bot, ctx.calibrationId)) { const held = calibrationLabel(bot) || "none"; out.push(`holds calibration ${held}, not ${ctx.calibrationId.slice(0, 8)}: push it`); } diff --git a/dotbot/console-web/src/editSiteButton.test.tsx b/dotbot/console-web/src/editSiteButton.test.tsx new file mode 100644 index 00000000..7180efad --- /dev/null +++ b/dotbot/console-web/src/editSiteButton.test.tsx @@ -0,0 +1,55 @@ +import { fireEvent, render, screen } from "@testing-library/react"; +import { describe, expect, it, vi } from "vitest"; + +import { RightPane } from "./RightPane"; +import type { Site } from "./types"; +import type { Calibration } from "./useCalibration"; + +const SITE: Site = { name: "lab", anchor: "", extent_mm: [2000, 2000], areas: [] }; +const LAYERS = { + batteryBars: true, + waypoints: true, + hotSpots: false, + dotBots: true, + trails: false, + crashedOnly: false, + lostBots: false, + allWaypoints: false, + calibratedSpan: true, +}; + +function pane(onEditSite?: () => void) { + return render( + {}} + collapsed={false} + setCollapsed={() => {}} + bots={[]} + site={SITE} + hiddenAreas={new Set()} + onAreaToggle={() => {}} + onEditSite={onEditSite} + layers={LAYERS} + layerRows={[]} + onLayerToggle={() => {}} + session={null} + calibration={{} as Calibration} + device="" + onDeviceChange={() => {}} + onCalibrationDone={() => {}} + />, + ); +} + +describe("Edit site", () => { + it("is offered only when the controller lets this browser edit its site", () => { + const { unmount } = pane(); + expect(screen.queryByText("Edit site")).toBeNull(); + unmount(); + const onEditSite = vi.fn(); + pane(onEditSite); + fireEvent.click(screen.getByText("Edit site")); + expect(onEditSite).toHaveBeenCalled(); + }); +}); diff --git a/dotbot/console-web/src/fleetViews.test.tsx b/dotbot/console-web/src/fleetViews.test.tsx index 9186e61e..cab21780 100644 --- a/dotbot/console-web/src/fleetViews.test.tsx +++ b/dotbot/console-web/src/fleetViews.test.tsx @@ -6,7 +6,10 @@ import { ageLabel, appLabel, areaLabel, + calibrationDetail, calibrationDiffers, + calibrationMismatch, + calibrationSiteLabel, detailText, FleetContext, firmwareTooOld, @@ -30,7 +33,7 @@ const site: Site = { }; const info = (over: Partial = {}): SwarmitDeviceInfo => ({ - info_version: 3, + info_version: 4, bl_version: "1.25.0", net_version: "1.25.0", boot_count: 3, @@ -96,6 +99,16 @@ describe("botFacts", () => { expect(calibrationDiffers(other, "")).toBe(false); }); + it("names the held calibration's short id and its site", () => { + const withSite = bot("A", { swarmit: { ...bot("A").swarmit!, info: info({ lh2_site_name: "virmond" }) } }); + expect(calibrationSiteLabel(withSite)).toBe("abcd1234 · virmond"); + expect(calibrationSiteLabel(bot("A"))).toBe("abcd1234"); + expect(calibrationSiteLabel(bot("A", { swarmit: null }))).toBe(""); + const emptied = bot("A", { swarmit: { ...bot("A").swarmit!, info: info({ lh2_station_mask: 0, lh2_site_name: "virmond" }) } }); + expect(calibrationSiteLabel(emptied)).toBe(""); + expect(calibrationDiffers(emptied, ctx.calibrationId)).toBe(true); + }); + it("picks the smallest area the robot stands in", () => { expect(areaLabel(bot("A"), site)).toBe("field"); expect(areaLabel(bot("A", { position: { x: 100, y: 100 } }), site)).toBe("bench"); @@ -231,3 +244,66 @@ describe("GridView", () => { spy.mockRestore(); }); }); + +describe("a calibration other than the one served", () => { + beforeEach(() => window.localStorage.clear()); + afterEach(cleanup); + + const held = (over: Partial) => + ({ swarmit: { ...bot("A").swarmit!, info: info({ lh2_site_name: "lab", ...over }) } }) as Partial; + const matching = bot("CCCC000000000001", held({})); + const otherId = bot("CCCC000000000002", held({ lh2_calibration_id: "99999999eeee" })); + const otherSite = bot("CCCC000000000003", held({ lh2_site_name: "workshop", lh2_calibration_id: "99999999eeee" })); + const fleet = [matching, otherId, otherSite]; + const rowOf = (id: string) => screen.getAllByRole("row").find((r) => r.textContent?.includes(id))!; + + it("ranks another site above another id, and needs both site names to compare", () => { + expect(calibrationMismatch(matching, ctx)).toBe("none"); + expect(calibrationMismatch(otherId, ctx)).toBe("other-id"); + expect(calibrationMismatch(otherSite, ctx)).toBe("other-site"); + // Same id, other site: still another frame. + expect(calibrationMismatch(bot("A", held({ lh2_site_name: "workshop" })), ctx)).toBe("other-site"); + expect(calibrationMismatch(bot("A", held({ lh2_site_name: "" })), ctx)).toBe("none"); + expect(calibrationMismatch(otherSite, { ...ctx, site: null })).toBe("other-id"); + expect(calibrationMismatch(bot("A", held({ lh2_site_name: "LAB" })), ctx)).toBe("none"); + expect(calibrationMismatch(bot("A", held({ info_version: 2, lh2_site_name: "workshop" })), ctx)).toBe("none"); + expect(detailText(otherSite, ctx)).toContain("! holds a calibration of site workshop"); + expect(detailText(otherSite, ctx)).not.toContain("push it"); + }); + + it("spells the held calibration out in full, with its stations", () => { + expect(calibrationDetail(bot("A", held({ lh2_station_mask: 0b11 })))).toBe("lab · abcd1234ffff · stations 0, 1"); + expect(calibrationDetail(bot("A", held({ lh2_station_mask: 0 })))).toBe(""); + expect(calibrationDetail(bot("A", held({ lh2_site_name: "" })))).toBe("abcd1234ffff"); + expect(calibrationDetail(bot("A", held({ lh2_calibration_id: "" })))).toBe(""); + }); + + it("is flagged once per robot in the list, the site louder than the id", () => { + render( {}} ctx={ctx} />); + const m = rowOf("CCCC000000000001"); + expect(within(m).queryByText("differs")).toBeNull(); + expect(within(m).queryByText(/other site/)).toBeNull(); + const i = rowOf("CCCC000000000002"); + expect(within(i).getByText("differs").getAttribute("data-tone")).toBe("warn"); + expect(within(i).queryByText(/other site/)).toBeNull(); + const s = rowOf("CCCC000000000003"); + expect(within(s).getByText("other site: workshop").getAttribute("data-tone")).toBe("alert"); + expect(within(s).queryByText("differs")).toBeNull(); + }); + + it("carries the full calibration as the list cell's tooltip", () => { + const withMask = bot("CCCC000000000004", held({ lh2_station_mask: 0b11 })); + render( {}} ctx={ctx} />); + expect(screen.getByText("abcd1234").getAttribute("title")).toBe("lab · abcd1234ffff · stations 0, 1"); + }); + + it("is flagged the same way on compact and full grid cards", () => { + render( {}} ctx={ctx} />); + expect(screen.getAllByText("cal differs")).toHaveLength(1); + expect(screen.getAllByText("other site: workshop")).toHaveLength(1); + fireEvent.click(screen.getByRole("button", { name: "Full" })); + expect(screen.getAllByText("differs")).toHaveLength(1); + expect(screen.getAllByText("other site: workshop")).toHaveLength(1); + expect(screen.getByText("99999999 · workshop").getAttribute("title")).toBe("workshop · 99999999eeee"); + }); +}); diff --git a/dotbot/console-web/src/heldWaypoints.test.ts b/dotbot/console-web/src/heldWaypoints.test.ts index a9b0e893..c14386ce 100644 --- a/dotbot/console-web/src/heldWaypoints.test.ts +++ b/dotbot/console-web/src/heldWaypoints.test.ts @@ -35,6 +35,12 @@ describe("heldWaypoints", () => { expect(rows.map((r) => r.status)).toEqual(["under way", "done"]); }); + it("reads a batch held for a heading as under way, even between moves", () => { + const waiting = { state: "in_progress" as const, index: 0, reason: "waiting for a heading", code: "AWAITING_HEADING" }; + const rows = heldWaypoints([bot("A", [start, start], { mission: waiting })]); + expect(rows[0]).toMatchObject({ status: "waiting for a heading", tone: "run" }); + }); + it("picks the rows of a selection", () => { expect(heldAmong(heldWaypoints(bots), new Set(["D", "A"])).map((r) => r.id)).toEqual(["D"]); }); diff --git a/dotbot/console-web/src/heldWaypoints.ts b/dotbot/console-web/src/heldWaypoints.ts index 2965f10c..bc4a63cc 100644 --- a/dotbot/console-web/src/heldWaypoints.ts +++ b/dotbot/console-web/src/heldWaypoints.ts @@ -14,6 +14,7 @@ export interface HeldRow { function statusOf(b: UnifiedBot, n: number): Pick { const m = b.mission; + if (m?.code === "AWAITING_HEADING") return { status: m.reason ?? m.code, tone: "run" }; if (b.nav === "auto") { const at = m?.state === "in_progress" && m.index !== null ? Math.min(m.index + 1, n) : null; return { status: at === null ? "under way" : `to ${at} of ${n}`, tone: "run" }; diff --git a/dotbot/console-web/src/inspector.test.ts b/dotbot/console-web/src/inspector.test.ts index d5e241af..5d292c4c 100644 --- a/dotbot/console-web/src/inspector.test.ts +++ b/dotbot/console-web/src/inspector.test.ts @@ -4,6 +4,8 @@ import { cameraRobotFor, formatCameraHeading, formatLh2, + formatLh2Site, + formatPosition, formatUptime, infoText, } from "./Inspector"; @@ -68,6 +70,23 @@ describe("formatLh2", () => { }); }); +describe("formatLh2Site", () => { + const info = { bl_version: "", net_version: "", boot_count: 0, uptime_s: 0, image_name: "", image_version: "", image_digest: "" }; + const withInfo = (over: object) => bot({ swarmit: node({ info: { ...info, ...over } }) }); + + it("names the site and the short calibration id the robot holds", () => { + expect(formatLh2Site(withInfo({ lh2_site_name: "virmond", lh2_calibration_id: "288a810d1234abcd" }))).toBe( + "virmond (id 288a810d)", + ); + }); + + it("says none for a robot holding no calibration, and nothing without device info", () => { + expect(formatLh2Site(withInfo({}))).toBe("none"); + expect(formatLh2Site(withInfo({ lh2_station_mask: 0, lh2_site_name: "virmond", lh2_calibration_id: "288a810d1234abcd" }))).toBe("none"); + expect(formatLh2Site(bot({ swarmit: null }))).toBeNull(); + }); +}); + describe("infoText", () => { it("is plain text carrying the identity and the reset cause", () => { const t = infoText( @@ -146,3 +165,13 @@ describe("the camera's heading for the selected robot", () => { expect(formatCameraHeading(robot)).toBe("not seen"); }); }); + +describe("formatPosition", () => { + const info = { bl_version: "", net_version: "", boot_count: 0, uptime_s: 0, image_name: "", image_version: "", image_digest: "" }; + it("names a failed LH2 sensor instead of no fix", () => { + const failed = bot({ position: null, swarmit: node({ info: { ...info, lh2_station_mask: 3, lh2_flags: 0b111 } }) }); + expect(formatPosition(failed)).toBe("LH2 sensor failed"); + expect(formatPosition(bot({ position: null, swarmit: node({ info: { ...info, lh2_flags: 0b011 } }) }))).toBe("no fix"); + expect(formatPosition(bot())).toBe("1552, 267"); + }); +}); diff --git a/dotbot/console-web/src/localization.test.ts b/dotbot/console-web/src/localization.test.ts index 7b83a269..8293d15a 100644 --- a/dotbot/console-web/src/localization.test.ts +++ b/dotbot/console-web/src/localization.test.ts @@ -56,7 +56,7 @@ const bot = (id: string, calibrationId?: string): UnifiedBot => calibrationId === undefined ? null : ({ - info: { info_version: 3, lh2_calibration_id: calibrationId }, + info: { info_version: 4, lh2_calibration_id: calibrationId }, } as unknown as SwarmitNode), }) as UnifiedBot; diff --git a/dotbot/console-web/src/localization.ts b/dotbot/console-web/src/localization.ts index 0d05627a..e0e3d816 100644 --- a/dotbot/console-web/src/localization.ts +++ b/dotbot/console-web/src/localization.ts @@ -39,7 +39,7 @@ export interface Coverage { } /** The device-info version of firmware that takes this host's calibrations; older needs a reflash. */ -export const DEVICE_INFO_VERSION_MIN = 3; +export const DEVICE_INFO_VERSION_MIN = 4; /** Who carries `savedId`, from the calibration id each robot's device info reports. */ export function calibrationCoverage(bots: UnifiedBot[], savedId: string): Coverage { @@ -53,13 +53,19 @@ export function calibrationCoverage(bots: UnifiedBot[], savedId: string): Covera for (const bot of bots) { const info = bot.swarmit?.info; if (!info || (info.info_version ?? 0) < DEVICE_INFO_VERSION_MIN) unchecked.push(bot.id); - else if ((info.lh2_calibration_id ?? "").toLowerCase() === wanted) carrying += 1; + else if (info.lh2_station_mask !== 0 && (info.lh2_calibration_id ?? "").toLowerCase() === wanted) carrying += 1; else stale.push(bot.id); } } return { id: savedId, carrying, total: bots.length, stale, unchecked, unknown: !wanted }; } +/** "288a810d (virmond)": the served calibration's short id and the site it was captured on. */ +export function servedCalibrationLabel(coverage: Coverage, site: string): string { + const id = coverage.id.slice(0, 8); + return site ? `${id} (${site})` : id; +} + /** "11 of 12 bots", or the absence when the fleet is empty. */ export function coverageLabel(coverage: Coverage): string { if (coverage.total === 0) return "no bots on the control plane"; diff --git a/dotbot/console-web/src/missions.test.ts b/dotbot/console-web/src/missions.test.ts index be7f535c..8e06c799 100644 --- a/dotbot/console-web/src/missions.test.ts +++ b/dotbot/console-web/src/missions.test.ts @@ -135,6 +135,15 @@ describe("missionReport", () => { expect(missionReport(undefined)).toBeNull(); }); + it("reports a batch the controller holds for a heading as under way", () => { + expect(missionReport({ waypoints_status: 2, waypoints_awaiting_heading: true })).toEqual({ + state: "in_progress", + index: 0, + reason: "waiting for a heading", + code: "AWAITING_HEADING", + }); + }); + it("names every reason the controller reports in words", () => { const reasons = ["NO_HEADING", "TURN", "PROGRESS", "HEADING_LOST", "HOLD", "SETTLE", "STOP", "DIRECT", "CONTROL_MODE"]; for (const code of reasons) { diff --git a/dotbot/console-web/src/pushTarget.test.tsx b/dotbot/console-web/src/pushTarget.test.tsx index ed1140ba..acc4fa14 100644 --- a/dotbot/console-web/src/pushTarget.test.tsx +++ b/dotbot/console-web/src/pushTarget.test.tsx @@ -76,7 +76,7 @@ const bot = (id: string, calibrationId: string): UnifiedBot => ({ id, swarmit: { - info: { info_version: 3, lh2_calibration_id: calibrationId }, + info: { info_version: 4, lh2_calibration_id: calibrationId }, } as unknown as SwarmitNode, }) as UnifiedBot; @@ -87,12 +87,13 @@ const renderPanel = ( selection: Set, onPush = vi.fn(), busy = false, + session: CalibrationSession = saved, ) => { render( {}} @@ -148,3 +149,16 @@ describe("the Localization tab's push row", () => { expect(onPush).not.toHaveBeenCalled(); }); }); + +describe("the Localization tab's calibration on the fleet", () => { + it("names the served calibration's id and its site beside its coverage", () => { + renderPanel([bot("AA", SAVED), bot("BB", "")], new Set(), vi.fn(), false, { ...saved, site: "virmond" }); + expect(screen.getByText("3f9a1c07 (virmond)")).toBeTruthy(); + expect(screen.getByText("on 1 of 2 bots")).toBeTruthy(); + }); + + it("names only the id when the session carries no site", () => { + renderPanel([bot("AA", SAVED)], new Set()); + expect(screen.getByText("3f9a1c07")).toBeTruthy(); + }); +}); diff --git a/dotbot/console-web/src/siteEditor/CalibrationPanel.tsx b/dotbot/console-web/src/siteEditor/CalibrationPanel.tsx new file mode 100644 index 00000000..e165e1a2 --- /dev/null +++ b/dotbot/console-web/src/siteEditor/CalibrationPanel.tsx @@ -0,0 +1,179 @@ +import React, { useState } from "react"; + +import type { Placement } from "./Canvas"; +import type { Rigid2D } from "./rigid"; +import type { CalibrationListing, PlacedCalibration } from "./types"; + +// Placing a spin calibration on the site: pick one by id, move it on the +// canvas or here, and save it as a new calibration of this site. + +const small: React.CSSProperties = { fontSize: 11, color: "var(--muted)", marginTop: 6 }; +const field: React.CSSProperties = { + width: "100%", + padding: "5px 7px", + background: "var(--elevated)", + color: "var(--text)", + border: "1px solid var(--hairline)", + borderRadius: 4, + fontFamily: "var(--font-mono)", + fontSize: 12, +}; +const button: React.CSSProperties = { + background: "transparent", + color: "var(--text)", + border: "1px solid var(--hairline)", + borderRadius: 4, + padding: "4px 10px", + cursor: "pointer", + fontSize: 12, +}; + +export interface CalibrationPanelProps { + listing: CalibrationListing[]; + placement: Placement | null; + reanchor: boolean; + placed: PlacedCalibration | null; + /** Why Save is not possible now, if it is not. */ + blocked: string | null; + onLoad: (spec: string) => void; + onMove: (move: Rigid2D) => void; + onReanchor: (on: boolean) => void; + onSave: () => void; + onClear: () => void; +} + +function Num(props: { name: string; value: number; onChange: (v: number) => void }) { + return ( + + ); +} + +export function CalibrationPanel(props: CalibrationPanelProps) { + const [spec, setSpec] = useState(""); + const overlay = props.placement?.overlay ?? null; + const move = props.placement?.move ?? null; + const refused = !!overlay && !overlay.free && !props.reanchor; + + return ( +
+
+ Place a calibration +
+
+ A spin calibration defines its own frame. Load one by id, drag it onto the site and turn it with the + handle; Save writes a new calibration and leaves the original as it is. +
+
{ + e.preventDefault(); + if (spec.trim()) props.onLoad(spec.trim()); + }} + > + setSpec(e.target.value)} + style={field} + /> + + {props.listing.map((c) => ( + + ))} + + +
+ {props.listing.length > 0 && ( +
+ {props.listing.length} calibration file{props.listing.length === 1 ? "" : "s"} found; type an id prefix, + the newest is never picked for you. +
+ )} + + {overlay && move && ( +
+
+ {overlay.id8} {overlay.tag ? `(${overlay.tag}) ` : ""}from site {overlay.site},{" "} + {overlay.stations.map((st) => `station ${st.index} (channel ${st.channel})`).join(", ")} +
+ {!overlay.free && ( +
+ This calibration was collected at points of the room: its frame is the site's anchor, so moving it + misplaces every robot. Only re-anchor it if the anchor itself moved. + +
+ )} +
+ props.onMove({ ...move, dx_mm })} /> + props.onMove({ ...move, dy_mm })} /> +
+ props.onMove({ ...move, theta_deg })} /> +
Scale is locked: the file is metric through the spin radius.
+
+ + +
+ {props.blocked &&
{props.blocked}
} +
+ )} + + {props.placed && ( +
+ Saved {props.placed.id8} (from {props.placed.source}) to {props.placed.path}. Robots still hold the old + placement and draw in the wrong place until it is pushed: + 
{props.placed.push}
+ {props.placed.warnings.map((w) => ( +
+ warning: {w} +
+ ))} +
+ )} +
+ ); +} diff --git a/dotbot/console-web/src/siteEditor/Canvas.tsx b/dotbot/console-web/src/siteEditor/Canvas.tsx new file mode 100644 index 00000000..1fb64c3d --- /dev/null +++ b/dotbot/console-web/src/siteEditor/Canvas.tsx @@ -0,0 +1,924 @@ +import React, { useEffect, useRef, useState } from "react"; + +import { areaColor } from "../areaColor"; +import { convexHull } from "../calibrationSpan"; +import { OBJECT_COLOUR, OBJECT_SIZE_MM, facing } from "../siteObjects"; +import { AREA_FALLBACK, siteViewMarginMm, withMargin } from "../frame"; +import { + axisTicks, + canvasPx, + frameMm, + gridStepMm, + gridSubStepMm, + metreLabel, + pxPerMm, + rulerStepMm, + ticksInSite, +} from "../grid"; +import type { Area } from "../types"; +import { FRAME_CAMERA, viewGeom } from "../zoom"; +import { BARRIER_MIN_POINTS, HANDLES, effectiveRole, finishPoints, movePoints, moveRect, rectFromDrag, resizeRect, snap } from "./edit"; +import type { Handle, Rect } from "./edit"; +import { STATION_COLOURS } from "./boxes"; +import { applyMove, movedBox, movedCorners, snapAngle, turnAbout } from "./rigid"; +import type { Point, Rigid2D } from "./rigid"; +import type { + BarrierKind, + CalibrationBackdrop, + CalibrationOverlay, + CameraBackdrop, + EditArea, + EditBarrier, + EditObject, + SpinCircle, +} from "./types"; + +// The site drawn to scale: the extent, the metric grid and its rulers, and +// every area in its role colour. Areas are moved, resized and drawn here; the +// numbers themselves live in the inspector. + +export type Tool = "select" | "area" | "calibration" | "boxes" | "wall" | "obstacle" | "object"; + +/** A selected wall or obstacle. */ +export interface BarrierRef { + kind: BarrierKind; + index: number; +} + +const TOOL_KIND: Partial> = { wall: "walls", obstacle: "obstacles" }; + +/** A calibration drawn over the site, and the move that places it. */ +export interface Placement { + overlay: CalibrationOverlay; + move: Rigid2D; +} + +/** One station's rectangle being edited, frame mm, and the circles it saw. */ +export interface StationBox { + index: number; + channel: number; + rect: Rect; + circles: SpinCircle[]; +} +const ROTATE_HANDLE_PX = 28; + +const HANDLE_PX = 9; +const AREA_TINT = 0.12; + +const DRAG_THRESHOLD_PX = 3; + +type Drag = + | { kind: "move"; index: number; start: Rect; from: { x: number; y: number }; px: { x: number; y: number } } + | { kind: "resize"; index: number; start: Rect; handle: Handle } + | { kind: "draw"; from: { x: number; y: number }; to: { x: number; y: number } } + | { kind: "place-move"; start: Rigid2D; from: { x: number; y: number } } + | { kind: "barrier-move"; ref: BarrierRef; start: [number, number][]; from: { x: number; y: number } } + | { kind: "barrier-vertex"; ref: BarrierRef; vertex: number } + | { kind: "object-move"; index: number; start: { x: number; y: number }; from: { x: number; y: number } } + | { kind: "place-turn"; start: Rigid2D; pivot: Point; angle: number } + | { kind: "box-move"; k: number; start: Rect; from: { x: number; y: number }; px: { x: number; y: number } } + | { kind: "box-resize"; k: number; start: Rect; handle: Handle }; + +export interface CanvasProps { + extent: [number, number] | null; + areas: EditArea[]; + hidden: Set; + selected: number | null; + tool: Tool; + snapMm: number; + onSelect: (index: number | null) => void; + /** `key` names the gesture, so one drag undoes as one step. */ + onChange: (index: number, rect: Rect, key: string) => void; + onDraw: (rect: Rect) => void; + placement?: Placement | null; + onPlacement?: (move: Rigid2D) => void; + walls?: EditBarrier[]; + obstacles?: EditBarrier[]; + selectedBarrier?: BarrierRef | null; + onSelectBarrier?: (ref: BarrierRef | null) => void; + /** `key` names the gesture, so one drag undoes as one step. */ + onBarrierChange?: (ref: BarrierRef, points: [number, number][], key: string) => void; + onBarrierDraw?: (kind: BarrierKind, points: [number, number][]) => void; + objects?: EditObject[]; + selectedObject?: number | null; + onSelectObject?: (index: number | null) => void; + onObjectChange?: (index: number, at: { x: number; y: number }, key: string) => void; + onObjectPlace?: (at: { x: number; y: number }) => void; + /** A calibration's station rectangles, edited with the boxes tool. */ + stationBoxes?: StationBox[] | null; + /** The box being edited, by its position in `stationBoxes`. */ + selectedBox?: number | null; + onSelectBox?: (k: number | null) => void; + /** `key` names the gesture, so one drag undoes as one step. */ + onBoxChange?: (k: number, rect: Rect, key: string) => void; + /** Read-only layers under the areas: only the ones switched on. */ + backdrops?: { calibrations: CalibrationBackdrop[]; cameras: CameraBackdrop[] }; +} + +function handlePoint(r: Rect, h: Handle): { x: number; y: number } { + const x = h.includes("w") ? r.x : h.includes("e") ? r.x + r.w : r.x + r.w / 2; + const y = h.includes("n") ? r.y : h.includes("s") ? r.y + r.h : r.y + r.h / 2; + return { x, y }; +} + +const CURSOR: Record = { + n: "ns-resize", + s: "ns-resize", + e: "ew-resize", + w: "ew-resize", + ne: "nesw-resize", + sw: "nesw-resize", + nw: "nwse-resize", + se: "nwse-resize", +}; + +export function Canvas(props: CanvasProps) { + const wrapRef = useRef(null); + const [size, setSize] = useState({ w: 800, h: 600 }); + const [drag, setDrag] = useState(null); + const [altHeld, setAltHeld] = useState(false); + // A wall or obstacle being drawn, point by point, and where the pointer is + const [draft, setDraft] = useState<{ kind: BarrierKind; points: [number, number][] } | null>(null); + const [hover, setHover] = useState<[number, number] | null>(null); + const draftKind = TOOL_KIND[props.tool] ?? null; + + useEffect(() => { + if (!draftKind) setDraft(null); + }, [draftKind]); + + const finishDraft = () => { + if (!draft) return; + const points = finishPoints(draft.points); + if (points.length >= BARRIER_MIN_POINTS[draft.kind]) props.onBarrierDraw?.(draft.kind, points); + setDraft(null); + }; + + useEffect(() => { + if (!draft) return; + const onKey = (e: KeyboardEvent) => { + if (e.key === "Enter") finishDraft(); + else if (e.key === "Escape") setDraft(null); + else return; + e.stopPropagation(); + }; + window.addEventListener("keydown", onKey, true); + return () => window.removeEventListener("keydown", onKey, true); + }); + + useEffect(() => { + const el = wrapRef.current; + if (!el) return; + const measure = () => { + const r = el.getBoundingClientRect(); + if (r.width > 0 && r.height > 0) setSize({ w: r.width, h: r.height }); + }; + measure(); + const observer = new ResizeObserver(measure); + observer.observe(el); + return () => observer.disconnect(); + }, []); + + const extentArea: Area = props.extent + ? { x: 0, y: 0, w: props.extent[0], h: props.extent[1] } + : AREA_FALLBACK; + const viewport = withMargin(extentArea, siteViewMarginMm(extentArea)); + const geom = viewGeom(size.w, size.h, viewport); + const cam = FRAME_CAMERA; + const px = (axis: "x" | "y", mm: number) => canvasPx(axis, mm, viewport, geom, cam); + const mm = (axis: "x" | "y", p: number) => frameMm(axis, p, viewport, geom, cam); + const scale = pxPerMm("x", viewport, geom, cam); + const rectPx = (r: Rect) => ({ + x: px("x", r.x), + y: px("y", r.y), + width: r.w * scale, + height: r.h * scale, + }); + + const step = gridStepMm(scale); + const sub = gridSubStepMm(scale); + const ruler = rulerStepMm(step, scale); + const extentForTicks = props.extent ? extentArea : null; + const xs = axisTicks("x", viewport, geom, cam, step); + const ys = axisTicks("y", viewport, geom, cam, step); + const subXs = sub ? axisTicks("x", viewport, geom, cam, sub) : []; + const subYs = sub ? axisTicks("y", viewport, geom, cam, sub) : []; + const labelXs = ticksInSite(axisTicks("x", viewport, geom, cam, ruler), "x", extentForTicks); + const labelYs = ticksInSite(axisTicks("y", viewport, geom, cam, ruler), "y", extentForTicks); + + const colourAreas: Area[] = props.areas.map((a) => ({ ...a, role: effectiveRole(a) })); + const snapNow = (e: { altKey: boolean }) => (e.altKey ? 1 : props.snapMm); + + const toFrame = (e: React.PointerEvent) => { + const r = wrapRef.current!.getBoundingClientRect(); + return { x: mm("x", e.clientX - r.left), y: mm("y", e.clientY - r.top) }; + }; + + const gesture = useRef(0); + const begin = (e: React.PointerEvent, next: Drag) => { + gesture.current += 1; + e.stopPropagation(); + wrapRef.current?.setPointerCapture?.(e.pointerId); + setDrag(next); + }; + + const onBackgroundDown = (e: React.PointerEvent) => { + if (e.button !== 0) return; + if (draftKind) { + const p = toFrame(e); + const s = snapNow(e); + const point: [number, number] = [snap(p.x, s), snap(p.y, s)]; + setDraft((d) => (d && d.kind === draftKind ? { ...d, points: [...d.points, point] } : { kind: draftKind, points: [point] })); + return; + } + if (props.tool === "object") { + const p = toFrame(e); + const s = snapNow(e); + props.onObjectPlace?.({ x: snap(p.x, s), y: snap(p.y, s) }); + return; + } + if (props.tool === "area") { + const p = toFrame(e); + begin(e, { kind: "draw", from: p, to: p }); + } else if (props.tool === "boxes") { + props.onSelectBox?.(null); + } else { + props.onSelect(null); + props.onSelectBarrier?.(null); + props.onSelectObject?.(null); + } + }; + + const onBoxDown = (e: React.PointerEvent, k: number) => { + const box = props.stationBoxes?.[k]; + if (e.button !== 0 || props.tool !== "boxes" || !box) return; + props.onSelectBox?.(k); + begin(e, { kind: "box-move", k, start: box.rect, from: toFrame(e), px: { x: e.clientX, y: e.clientY } }); + }; + + const onBoxHandleDown = (e: React.PointerEvent, k: number, handle: Handle) => { + const box = props.stationBoxes?.[k]; + if (e.button !== 0 || !box) return; + begin(e, { kind: "box-resize", k, start: box.rect, handle }); + }; + + const onObjectDown = (e: React.PointerEvent, index: number) => { + if (e.button !== 0 || props.tool !== "select") return; + const o = props.objects?.[index]; + if (!o) return; + props.onSelectObject?.(index); + begin(e, { kind: "object-move", index, start: { x: o.x, y: o.y }, from: toFrame(e) }); + }; + + const onBarrierDown = (e: React.PointerEvent, ref: BarrierRef) => { + if (e.button !== 0 || props.tool !== "select") return; + const b = (ref.kind === "walls" ? props.walls : props.obstacles)?.[ref.index]; + if (!b) return; + props.onSelectBarrier?.(ref); + begin(e, { kind: "barrier-move", ref, start: b.points, from: toFrame(e) }); + }; + + const onVertexDown = (e: React.PointerEvent, ref: BarrierRef, vertex: number) => { + if (e.button !== 0) return; + begin(e, { kind: "barrier-vertex", ref, vertex }); + }; + + const onAreaDown = (e: React.PointerEvent, index: number) => { + if (e.button !== 0 || props.tool !== "select") return; + const a = props.areas[index]; + props.onSelect(index); + begin(e, { + kind: "move", + index, + start: { x: a.x, y: a.y, w: a.w, h: a.h }, + from: toFrame(e), + px: { x: e.clientX, y: e.clientY }, + }); + }; + + const onHandleDown = (e: React.PointerEvent, index: number, handle: Handle) => { + if (e.button !== 0) return; + const a = props.areas[index]; + begin(e, { kind: "resize", index, start: { x: a.x, y: a.y, w: a.w, h: a.h }, handle }); + }; + + const placement = props.placement ?? null; + const fence = placement ? placement.overlay.fence : null; + const placedBox = placement && fence ? movedBox(placement.move, fence) : null; + const placeCentre: Point | null = placedBox + ? [(placedBox[0] + placedBox[2]) / 2, (placedBox[1] + placedBox[3]) / 2] + : null; + const angleTo = (c: Point, p: { x: number; y: number }) => + (Math.atan2(p.y - c[1], p.x - c[0]) * 180) / Math.PI; + + const onPlacementDown = (e: React.PointerEvent) => { + if (e.button !== 0 || props.tool !== "calibration" || !placement) return; + begin(e, { kind: "place-move", start: placement.move, from: toFrame(e) }); + }; + + const onTurnDown = (e: React.PointerEvent) => { + if (e.button !== 0 || !placement || !placeCentre) return; + begin(e, { kind: "place-turn", start: placement.move, pivot: placeCentre, angle: angleTo(placeCentre, toFrame(e)) }); + }; + + const onMove = (e: React.PointerEvent) => { + setAltHeld(e.altKey); + if (draft) { + const p = toFrame(e); + const s = snapNow(e); + setHover([snap(p.x, s), snap(p.y, s)]); + } + if (!drag) return; + const p = toFrame(e); + const s = snapNow(e); + if (drag.kind === "object-move") { + props.onObjectChange?.( + drag.index, + { x: snap(drag.start.x + p.x - drag.from.x, s), y: snap(drag.start.y + p.y - drag.from.y, s) }, + `canvas-${gesture.current}`, + ); + } else if (drag.kind === "barrier-move") { + props.onBarrierChange?.(drag.ref, movePoints(drag.start, p.x - drag.from.x, p.y - drag.from.y, s), `canvas-${gesture.current}`); + } else if (drag.kind === "barrier-vertex") { + const b = (drag.ref.kind === "walls" ? props.walls : props.obstacles)?.[drag.ref.index]; + if (b) { + const points = b.points.map((q, i): [number, number] => (i === drag.vertex ? [snap(p.x, s), snap(p.y, s)] : q)); + props.onBarrierChange?.(drag.ref, points, `canvas-${gesture.current}`); + } + } else if (drag.kind === "place-move") { + const dx = snap(drag.start.dx_mm + p.x - drag.from.x, s); + const dy = snap(drag.start.dy_mm + p.y - drag.from.y, s); + props.onPlacement?.({ ...drag.start, dx_mm: dx, dy_mm: dy }); + } else if (drag.kind === "place-turn") { + const total = snapAngle(drag.start.theta_deg + angleTo(drag.pivot, p) - drag.angle, e.altKey); + const turned = turnAbout(drag.start, drag.pivot, total - drag.start.theta_deg); + // The zero lands on the snap grid too, so the numbers stay round + props.onPlacement?.({ + theta_deg: total, + dx_mm: s > 1 ? snap(turned.dx_mm, s) : Math.round(turned.dx_mm * 10) / 10, + dy_mm: s > 1 ? snap(turned.dy_mm, s) : Math.round(turned.dy_mm * 10) / 10, + }); + } else if (drag.kind === "box-move") { + if (Math.hypot(e.clientX - drag.px.x, e.clientY - drag.px.y) < DRAG_THRESHOLD_PX) return; + props.onBoxChange?.(drag.k, moveRect(drag.start, p.x - drag.from.x, p.y - drag.from.y, s), `canvas-${gesture.current}`); + } else if (drag.kind === "box-resize") { + props.onBoxChange?.(drag.k, resizeRect(drag.start, drag.handle, p, s), `canvas-${gesture.current}`); + } else if (drag.kind === "move") { + // A click that selects is not a move, however the pointer jitters + if (Math.hypot(e.clientX - drag.px.x, e.clientY - drag.px.y) < DRAG_THRESHOLD_PX) return; + props.onChange(drag.index, moveRect(drag.start, p.x - drag.from.x, p.y - drag.from.y, s), `canvas-${gesture.current}`); + } else if (drag.kind === "resize") { + props.onChange(drag.index, resizeRect(drag.start, drag.handle, p, s), `canvas-${gesture.current}`); + } else { + setDrag({ ...drag, to: p }); + } + }; + + const onUp = (e: React.PointerEvent) => { + if (drag?.kind === "draw") { + const r = rectFromDrag(drag.from, toFrame(e), snapNow(e)); + if (r) props.onDraw(r); + } + setDrag(null); + }; + + // Largest first, so a small area inside a large one stays on top to grab. + const order = props.areas + .map((a, i) => i) + .filter((i) => !props.hidden.has(props.areas[i].name) || i === props.selected) + .sort((i, j) => props.areas[j].w * props.areas[j].h - props.areas[i].w * props.areas[i].h); + const sel = props.selected !== null ? props.areas[props.selected] : null; + const preview = drag?.kind === "draw" ? rectFromDrag(drag.from, drag.to, props.snapMm) : null; + const extentPx = rectPx(extentArea); + const boxes = props.stationBoxes ?? null; + // Largest first and the one being edited last, so each can be grabbed + const boxOrder = (boxes ?? []) + .map((_, k) => k) + .sort((i, j) => { + if (i === props.selectedBox) return 1; + if (j === props.selectedBox) return -1; + return boxes![j].rect.w * boxes![j].rect.h - boxes![i].rect.w * boxes![i].rect.h; + }); + const selBox = boxes && props.selectedBox !== null && props.selectedBox !== undefined ? boxes[props.selectedBox] ?? null : null; + + return ( +
setDrag(null)} + onDoubleClick={finishDraft} + style={{ + position: "relative", + flex: 1, + minWidth: 0, + minHeight: 0, + overflow: "hidden", + background: "var(--canvas)", + cursor: props.tool === "area" || props.tool === "object" || draftKind ? "crosshair" : "default", + touchAction: "none", + userSelect: "none", + }} + > + + + + {subXs.map((t) => ( + + ))} + {subYs.map((t) => ( + + ))} + + + {xs.map((t) => ( + + ))} + {ys.map((t) => ( + + ))} + + + + {labelXs.map((t) => ( + + {metreLabel(t.mm, ruler)} + + ))} + {labelYs.map((t) => ( + + {metreLabel(t.mm, ruler)} + + ))} + + {props.backdrops?.cameras.map((cam) => + cam.rect && cam.still ? ( + + ) : null, + )} + {props.backdrops?.calibrations.map((cal) => { + const outline = (pts: [number, number][]) => + convexHull(pts) + .map(([x, y]) => `${px("x", x)},${px("y", y)}`) + .join(" "); + return ( + + {cal.placements + .filter((pts) => pts.length >= 3) + .map((pts, k) => ( + + ))} + {cal.centres.length >= 3 && ( + + )} + {cal.centres.map(([x, y], k) => ( + + ))} + + ); + })} + {order.map((i) => { + const a = props.areas[i]; + const colour = areaColor(colourAreas[i], colourAreas); + const role = effectiveRole(a); + const composite = a.name.includes("+"); + const r = rectPx(a); + return ( + + onAreaDown(e, i)} + > + {a.name} + + {r.width > 40 && r.height > 18 && ( + + {a.name} + + )} + + ); + })} + {(["obstacles", "walls"] as BarrierKind[]).map((kind) => + ((kind === "walls" ? props.walls : props.obstacles) ?? []).map((b, i) => { + const ref = { kind, index: i }; + const chosen = props.selectedBarrier?.kind === kind && props.selectedBarrier.index === i; + const pts = b.points.map(([x, y]) => `${px("x", x)},${px("y", y)}`).join(" "); + const common = { + points: pts, + stroke: chosen ? "var(--accent)" : "var(--text)", + style: { cursor: props.tool === "select" ? "move" : undefined }, + onPointerDown: (e: React.PointerEvent) => onBarrierDown(e, ref), + }; + return kind === "walls" ? ( + + {b.name || `wall ${i + 1}`} + + ) : ( + + {b.name || `obstacle ${i + 1}`} + + ); + }), + )} + {props.selectedBarrier && props.tool === "select" && + ((props.selectedBarrier.kind === "walls" ? props.walls : props.obstacles) ?? [])[props.selectedBarrier.index]?.points.map( + ([x, y], v) => ( + onVertexDown(e, props.selectedBarrier!, v)} + /> + ), + )} + {(props.objects ?? []).map((o, i) => { + const r = Math.max(5, (OBJECT_SIZE_MM / 2) * scale); + const [cx, cy] = [px("x", o.x), px("y", o.y)]; + const [dx, dy] = facing(o.heading_deg); + const chosen = props.selectedObject === i; + return ( + onObjectDown(e, i)} + > + + + + {o.name} + + + ); + })} + {draft && ( + + 1 ? [draft.points[0]] : [])] + .map(([x, y]) => `${px("x", x)},${px("y", y)}`) + .join(" ")} + fill="none" + stroke="var(--accent)" + strokeWidth={2} + strokeDasharray="5 4" + /> + {draft.points.map(([x, y], k) => ( + + ))} + + )} + {boxes && ( + + {boxOrder.map((k) => { + const box = boxes[k]; + const colour = STATION_COLOURS[k % STATION_COLOURS.length]; + const r = rectPx(box.rect); + const editing = props.tool === "boxes"; + const chosen = editing && props.selectedBox === k; + return ( + + onBoxDown(e, k)} + > + {`station ${box.index} (channel ${box.channel})`} + + + {`station ${box.index} (channel ${box.channel})`} + + {box.circles.map((c) => ( + + ))} + + ); + })} + {selBox && props.tool === "boxes" && props.selectedBox !== null && props.selectedBox !== undefined && ( + + + {`${selBox.rect.w} x ${selBox.rect.h} mm at (${selBox.rect.x}, ${selBox.rect.y})`} + + {HANDLES.map((h) => { + const p = handlePoint(selBox.rect, h); + return ( + onBoxHandleDown(e, props.selectedBox!, h)} + /> + ); + })} + + )} + + )} + {placement && fence && placedBox && placeCentre && ( + + {placement.overlay.stations.map((st, k) => { + const colour = STATION_COLOURS[k % STATION_COLOURS.length]; + const corners = movedCorners(placement.move, st.rect) + .map(([x, y]) => `${px("x", x)},${px("y", y)}`) + .join(" "); + return ( + + + {placement.overlay.stations.length > 1 && ( + + {`station ${st.index} (channel ${st.channel})`} + + )} + {st.circles.map((c) => { + const [x, y] = applyMove(placement.move, [c.x, c.y]); + return ( + + + + + ); + })} + + ); + })} + {placement.overlay.links.map((link) => { + const a = placement.overlay.stations.find((st) => st.index === link.a); + const b = placement.overlay.stations.find((st) => st.index === link.b); + if (!a || !b) return null; + const centre = (rect: number[]) => + applyMove(placement.move, [(rect[0] + rect[2]) / 2, (rect[1] + rect[3]) / 2]); + const [ax, ay] = centre(a.rect); + const [bx, by] = centre(b.rect); + return ( + + {`${link.shared} shared circle${link.shared === 1 ? "" : "s"}${link.weak ? ", weak" : ""}`} + + ); + })} + `${px("x", x)},${px("y", y)}`) + .join(" ")} + fill="var(--accent)" + fillOpacity={props.tool === "calibration" ? 0.06 : 0} + stroke="none" + style={{ + cursor: props.tool === "calibration" ? "move" : "default", + pointerEvents: props.tool === "calibration" ? "all" : "none", + }} + onPointerDown={onPlacementDown} + /> + + {`${placement.overlay.id8} ${placement.move.theta_deg} deg`} + + {props.tool === "calibration" && ( + + + + turn: 90 degree steps, Alt for free + + + )} + + )} + {sel && props.selected !== null && props.tool === "select" && ( + + + {`${sel.w} x ${sel.h} mm at (${sel.x}, ${sel.y})`} + + {HANDLES.map((h) => { + const p = handlePoint(sel, h); + return ( + onHandleDown(e, props.selected!, h)} + /> + ); + })} + + )} + {preview && ( + + )} + + + + anchor (0, 0) + + + +
+ {draftKind + ? `click to add points, double-click or Enter to finish, Esc to cancel; snap ${props.snapMm} mm, Alt for free` + : altHeld + ? "free placement (Alt)" + : props.tool === "calibration" + ? `snap ${props.snapMm} mm and 90 deg, Alt for free; scale is locked` + : props.tool === "boxes" + ? `drag a station's box or its handles; snap ${props.snapMm} mm, Alt for free` + : `snap ${props.snapMm} mm, Alt for free`} +
+
+ ); +} diff --git a/dotbot/console-web/src/siteEditor/Inspector.tsx b/dotbot/console-web/src/siteEditor/Inspector.tsx new file mode 100644 index 00000000..450f089c --- /dev/null +++ b/dotbot/console-web/src/siteEditor/Inspector.tsx @@ -0,0 +1,548 @@ +import React, { useState } from "react"; + +import { OBJECT_KINDS } from "../siteObjects"; +import type { AreaRole, SiteObjectKind } from "../types"; +import type { BarrierRef } from "./Canvas"; +import { BARRIER_MIN_POINTS, ROLES, declaredRole, effectiveRole, parsePoints } from "./edit"; +import type { Issue } from "./edit"; +import type { Backdrops, BarrierKind, EditArea, EditBarrier, EditObject, SiteModel } from "./types"; + +// The right pane: every field is a key of site.toml, and what it shows is +// what Save writes. + +const label: React.CSSProperties = { + display: "block", + fontSize: 11, + color: "var(--muted)", + margin: "10px 0 3px", +}; +const input: React.CSSProperties = { + width: "100%", + padding: "5px 7px", + background: "var(--elevated)", + color: "var(--text)", + border: "1px solid var(--hairline)", + borderRadius: 4, + fontFamily: "var(--font-mono)", + fontSize: 12, +}; +const section: React.CSSProperties = { + borderBottom: "1px solid var(--hairline)", + padding: "12px 14px", +}; +const heading: React.CSSProperties = { + fontSize: 11, + fontWeight: 600, + letterSpacing: 0.6, + textTransform: "uppercase", + color: "var(--muted)", +}; + +function NumberField(props: { + name: string; + value: number; + onChange: (v: number) => void; + min?: number; +}) { + return ( + + ); +} + +export interface InspectorProps { + name: string; + path: string; + site: SiteModel; + selected: number | null; + hidden: Set; + issues: Issue[]; + /** `key` names the field, so typing into one undoes as one step. */ + onSite: (site: SiteModel, key: string) => void; + onArea: (index: number, area: EditArea, key: string) => void; + onSelect: (index: number | null) => void; + onDelete: (index: number) => void; + onToggle: (name: string) => void; + onAdd: () => void; + /** Shown at the top of the pane, for a tool with its own controls. */ + extra?: React.ReactNode; + selectedBarrier?: BarrierRef | null; + onSelectBarrier?: (ref: BarrierRef | null) => void; + onBarrier?: (ref: BarrierRef, patch: Partial, key: string) => void; + onDeleteBarrier?: (ref: BarrierRef) => void; + selectedObject?: number | null; + onSelectObject?: (index: number | null) => void; + onObject?: (index: number, patch: Partial, key: string) => void; + onDeleteObject?: (index: number) => void; + backdrops?: Backdrops | null; + /** Keys `cal:` and `cam:` of the backdrops switched on. */ + shownBackdrops?: Set; + onToggleBackdrop?: (key: string) => void; +} + +const KIND_LABEL: Record = { walls: "wall", obstacles: "obstacle" }; + +function BarrierInspector(props: { + barrier: EditBarrier; + kind: BarrierKind; + index: number; + onChange: (patch: Partial, key: string) => void; + onDelete: () => void; +}) { + const { barrier, kind } = props; + const shown = barrier.points.map(([x, y]) => `${x}, ${y}`).join("\n"); + const [text, setText] = useState(shown); + const [editing, setEditing] = useState(false); + const parsed = parsePoints(text); + const tooFew = !!parsed && parsed.length < BARRIER_MIN_POINTS[kind]; + return ( +
+
{KIND_LABEL[kind]}
+ +