diff --git a/dotbot/camera/detection/pose.py b/dotbot/camera/detection/pose.py index ccf7583e..39d9f369 100644 --- a/dotbot/camera/detection/pose.py +++ b/dotbot/camera/detection/pose.py @@ -36,37 +36,30 @@ from dotbot.robots import robot_geometry _GEOMETRY = robot_geometry() +_CENTRE = _GEOMETRY.outline_centre + + +def _robot_frame(point): + """A board-frame point in this module's robot frame (y flipped to forward).""" + return (point.x - _CENTRE.x, _CENTRE.y - point.y) + + +def _robot_frame_path(path): + """One board-frame path as an array in this module's robot frame.""" + return np.array([_robot_frame(p) for p in path], float) + # Outline centre to the axle, backwards. -AXLE_BEHIND_CENTRE_MM = 24.5 +AXLE_BEHIND_CENTRE_MM = -_robot_frame(_GEOMETRY.axle_midpoint)[1] # Outline centre to the LH2 photodiode, forwards. The lighthouse reports the # photodiode's position, so this is the offset that makes the camera's point # and the lighthouse's point the same point. -PHOTODIODE_AHEAD_MM = _GEOMETRY.board_length_mm / 2 - _GEOMETRY.diode_to_front_mm - -# Board outline in the robot frame, transcribed from the Edge.Cuts layer of -# the v3 main board: an 84 mm nose and the step down to the 57 mm tail at -# y = +1.5. -OUTLINE_MM = np.array( - [ - (-42.0, 47.5), - (42.0, 47.5), - (42.0, 40.5), - (43.0, 39.5), - (47.0, 39.5), - (47.0, 1.5), - (28.5, 1.5), - (28.5, -47.5), - (-28.5, -47.5), - (-28.5, 1.5), - (-47.0, 1.5), - (-47.0, 39.5), - (-43.0, 39.5), - (-42.0, 40.5), - ], - float, -) +PHOTODIODE_AHEAD_MM = _GEOMETRY.diode_ahead_of_centre_mm + +# Board outline in the robot frame: an 84 mm nose and the step down to the +# 57 mm tail at y = +1.5. +OUTLINE_MM = _robot_frame_path(_GEOMETRY.outline_path) # Outline centre to the tip of the nose, forwards: the outline's own extent. NOSE_AHEAD_MM = float(OUTLINE_MM[:, 1].max()) @@ -78,17 +71,9 @@ TAIL_HALF_MM = float(np.abs(OUTLINE_MM[OUTLINE_MM[:, 1] < 1.5][:, 0]).max()) NOSE_BAND_MM = TAIL_HALF_MM + 4.5 -# The two motor connectors, in the same robot frame. -CONN_MM = [ - np.array([(-19.5, -29.5), (-7.5, -29.5), (-7.5, -19.5), (-19.5, -19.5)], float), - np.array([(7.5, -29.5), (19.5, -29.5), (19.5, -19.5), (7.5, -19.5)], float), -] - -# The tyres, as the template draws them: the track between their centres, -# and one tyre's width and depth. -TRACK_MM = 85.0 -TYRE_W_MM = 18.0 -TYRE_D_MM = 40.0 +# The two motor connectors and the two tyres, in the same robot frame. +CONN_MM = [_robot_frame_path(c) for c in _GEOMETRY.connector_paths] +WHEELS_MM = [_robot_frame_path(w) for w in _GEOMETRY.wheel_paths] # The template's canvas, as a half-width in millimetres: the robot at any # heading, with room for the search to slide it. @@ -385,29 +370,10 @@ def poly(canvas, pts): wheel = np.zeros_like(board) conn = np.zeros_like(board) poly(board, OUTLINE_MM) - axle = -AXLE_BEHIND_CENTRE_MM - for side in (-1, 1): - x0 = side * TRACK_MM / 2 - TYRE_W_MM / 2 - x1 = side * TRACK_MM / 2 + TYRE_W_MM / 2 - poly( - wheel, - [ - (x0, axle - TYRE_D_MM / 2), - (x1, axle - TYRE_D_MM / 2), - (x1, axle + TYRE_D_MM / 2), - (x0, axle + TYRE_D_MM / 2), - ], - ) - for side in (-13.5, 13.5): - poly( - conn, - [ - (side - 6, axle - 5), - (side + 6, axle - 5), - (side + 6, axle + 5), - (side - 6, axle + 5), - ], - ) + for tyre in WHEELS_MM: + poly(wheel, tyre) + for connector in CONN_MM: + poly(conn, connector) self.maps = dict( green=np.clip(board - wheel - conn, 0, 1), dark=wheel, red=conn ) diff --git a/dotbot/camera/detection/propose.py b/dotbot/camera/detection/propose.py index e283f991..c3a55b1c 100644 --- a/dotbot/camera/detection/propose.py +++ b/dotbot/camera/detection/propose.py @@ -24,7 +24,9 @@ import numpy as np -ROBOT_MM = 95.0 # DotBot footprint, a physical fact +from dotbot.robots import robot_geometry + +ROBOT_MM = robot_geometry().envelope_mm # the footprint a robot occupies # Design constants, all in units of the robot or of the floor's own noise. SAMPLES_PER_ROBOT = 12.0 # working grid: a robot spans this many px diff --git a/dotbot/camera/detection/robot.py b/dotbot/camera/detection/robot.py index d52ab53c..7a7f5b3a 100644 --- a/dotbot/camera/detection/robot.py +++ b/dotbot/camera/detection/robot.py @@ -28,6 +28,7 @@ NOSE_AHEAD_MM, OUTLINE_MM, PHOTODIODE_AHEAD_MM, + WHEELS_MM, OutlineFit, Template, axes, @@ -197,7 +198,8 @@ def frame_pose(pose: Pose, area: Area, mm_per_px: float) -> dict: """One pose in frame millimetres and both heading conventions. Every `*_mm` is frame millimetres, x right and y down, the same frame as - the area and as an LH2 position. + the area and as an LH2 position. `outline_mm` is the board path and + `wheels_mm` the two tyre rectangles, both already turned to the heading. `heading_atan2_deg` is the detector's own convention: `atan2(dy, dx)` in the y-down frame, so 0 = +x and +90 = +y. `heading_deg` is the robot @@ -208,14 +210,18 @@ def frame_pose(pose: Pose, area: Area, mm_per_px: float) -> dict: origin = np.array([float(area.x), float(area.y)]) centre = origin + np.asarray(pose.centre_px, float) * mm_per_px right, forward = axes(pose.heading_atan2_deg) - outline = [ - centre + p[0] * right + p[1] * forward for p in np.asarray(OUTLINE_MM, float) - ] + + def place(path): + return [ + _mm(centre + p[0] * right + p[1] * forward) for p in np.asarray(path, float) + ] + return { "centre_mm": _mm(centre), "photodiode_mm": _mm(centre + forward * PHOTODIODE_AHEAD_MM), "nose_mm": _mm(centre + forward * NOSE_AHEAD_MM), - "outline_mm": [_mm(p) for p in outline], + "outline_mm": place(OUTLINE_MM), + "wheels_mm": [place(wheel) for wheel in WHEELS_MM], "heading_deg": round(wrap180(pose.heading_atan2_deg - 90.0), 1), "heading_atan2_deg": round(wrap180(pose.heading_atan2_deg), 1), "green_flare": round(pose.green_flare, 3), diff --git a/dotbot/console-web/dev/simulator_init_state.toml b/dotbot/console-web/dev/simulator_init_state.toml index 8515ba73..45e64942 100644 --- a/dotbot/console-web/dev/simulator_init_state.toml +++ b/dotbot/console-web/dev/simulator_init_state.toml @@ -45,3 +45,11 @@ calibrated = 0xff pos_x = 250 pos_y = 900 direction = 135 + +# No `direction`: advertises no heading, so the console's no-heading rendering +# has a live producer. +[[dotbots]] +address = "F00DFACE77777777" +calibrated = 0xff +pos_x = 900 +pos_y = 1400 diff --git a/dotbot/console-web/src/App.tsx b/dotbot/console-web/src/App.tsx index 876232c4..586c2209 100644 --- a/dotbot/console-web/src/App.tsx +++ b/dotbot/console-web/src/App.tsx @@ -25,7 +25,8 @@ import { ListView } from "./ListView"; import { Camera, Layers, MapView, ViewGeom } from "./MapView"; import { MrtaToggle } from "./MrtaToggle"; import { RightPane, RightTab } from "./RightPane"; -import { loadRobotShapes, saveRobotShapes } from "./robotShapes"; +import { usePanel } from "./panels"; +import { RobotDrawing, loadRobotDrawing, saveRobotDrawing } from "./robotDrawing"; import { VIEW_SETTLE_MS, loadSavedViews, @@ -61,10 +62,14 @@ import { useMrta } from "./useMrta"; import { useOrchestration } from "./useOrchestration"; import { Camera as ZoomCamera, + SITE_ZOOM, + ViewCentre, + cameraAtCentre, cameraForArea, cameraForZoom, + centreOfView, padArea, - visibleArea, + viewGeom, zoomFromSearch, zoomMax, } from "./zoom"; @@ -154,7 +159,13 @@ export const App: React.FC = () => { crashedOnly: false, }); const [rightTab, setRightTab] = useState("layers"); - const [rightCollapsed, setRightCollapsed] = useState(false); + const [rightCollapsed, setRightCollapsed, setRightCollapsedUnsaved] = usePanel("right"); + // ?rail=collapsed starts the left panel as its icon strip, whatever this + // browser stored. + const [railCollapsed, setRailCollapsed] = usePanel( + "left", + new URLSearchParams(window.location.search).get("rail") === "collapsed" ? true : undefined, + ); const [conn, setConn] = useState(null); const [build, setBuild] = useState(null); @@ -216,14 +227,14 @@ export const App: React.FC = () => { [updateRobotOpacity], ); - // Whether DotBots are drawn as the robot or as a plain mark, per browser. - const [robotShapes, updateRobotShapes] = usePersisted( - loadRobotShapes, - saveRobotShapes, + // Whether DotBots are drawn as their bodies or their sensor points, per browser. + const [robotDrawing, updateRobotDrawing] = usePersisted( + loadRobotDrawing, + saveRobotDrawing, ); - const onRobotShapesToggle = useCallback( - () => updateRobotShapes((prev) => !prev), - [updateRobotShapes], + const onRobotDrawing = useCallback( + (next: RobotDrawing) => updateRobotDrawing(() => next), + [updateRobotDrawing], ); // The rail's action opens the tab that sets a session up; the session @@ -237,7 +248,10 @@ export const App: React.FC = () => { const zoomTo = useCallback( (name: string) => { if (!geom) return; - const next: ZoomCamera | null = cameraForZoom(name, site, viewport, geom); + // Refitted to this viewport: the site can land in the same commit that + // reshapes the box, before the map has reported its new geometry. + const g = viewGeom(geom.w, geom.h, viewport); + const next: ZoomCamera | null = cameraForZoom(name, site, viewport, g); if (next) setCam(next); }, [geom, site, viewport], @@ -246,9 +260,10 @@ export const App: React.FC = () => { // What the map opens on, once the canvas has a size and the site is known. // `?zoom=` is an instruction and wins; failing that the map // returns to the floor this browser was last looking at, which is stored as - // a rectangle and fitted here, so a window of another size lands on the - // same floor rather than on the same pixels. Neither, and it opens on the - // whole site, as a map with nothing remembered always has. + // its centre point and pixels per millimetre, so a window of another size + // shows the same floor at the same size rather than the same pixels. + // Neither, and it opens on the whole site, as a map with nothing remembered + // always has. const [openingViews] = useState(loadSavedViews); const openedRef = useRef(false); useEffect(() => { @@ -259,10 +274,13 @@ export const App: React.FC = () => { zoomTo(asked); return; } - const rect = viewFor(openingViews, site.name, viewport); - if (rect) { - setCam(cameraForArea(rect, viewport, geom, zoomMax(site, viewport, geom))); + const view = viewFor(openingViews, site.name, viewport); + if (view) { + const g = viewGeom(geom.w, geom.h, viewport); + setCam(cameraAtCentre(view, viewport, g, zoomMax(site, viewport, g))); + return; } + zoomTo(SITE_ZOOM); }, [geom, site, viewport, openingViews, zoomTo]); // Remembered once the camera settles: a pan would otherwise write storage @@ -272,16 +290,20 @@ export const App: React.FC = () => { if (!openedRef.current || !geom || !site) return; const timer = window.setTimeout(() => { saveSavedViews( - withView(loadSavedViews(), site.name, visibleArea(cam, viewport, geom)), + withView(loadSavedViews(), site.name, centreOfView(cam, viewport, geom)), ); }, VIEW_SETTLE_MS); return () => window.clearTimeout(timer); }, [cam, geom, site, viewport]); // Calibration mode takes over the right pane and the viewport, and gives - // both back on Done: the tab that was open before, and the camera that was - // on it. - const beforeCalibration = useRef<{ tab: RightTab; cam: Camera } | null>(null); + // both back on Done: the tab that was open before, the pane collapsed again + // if it was, and the view that was on it. + const beforeCalibration = useRef<{ + tab: RightTab; + view: ViewCentre | null; + forcedOpen: boolean; + } | null>(null); const geomRef = useRef(geom); geomRef.current = geom; const viewportRef = useRef(viewport); @@ -292,32 +314,75 @@ export const App: React.FC = () => { rightTabRef.current = rightTab; const camRef = useRef(cam); camRef.current = cam; + const rightCollapsedRef = useRef(rightCollapsed); + rightCollapsedRef.current = rightCollapsed; + + // A camera waiting for the canvas a pane toggle is about to give the map, + // placed once the map reports its new geometry. + const pendingCam = useRef<((g: ViewGeom) => Camera) | null>(null); + const placeCam = useCallback( + (place: ((g: ViewGeom) => Camera) | null, afterResize: boolean) => { + pendingCam.current = afterResize ? place : null; + const g = geomRef.current; + if (!afterResize && place && g) { + setCam(place(viewGeom(g.w, g.h, viewportRef.current))); + } + }, + [], + ); + useEffect(() => { + const place = pendingCam.current; + if (!place || !geom) return; + pendingCam.current = null; + setCam(place(viewGeom(geom.w, geom.h, viewportRef.current))); + }, [geom]); useEffect(() => { if (session && !beforeCalibration.current) { - beforeCalibration.current = { tab: rightTabRef.current, cam: camRef.current }; + const g = geomRef.current; + const collapsed = rightCollapsedRef.current; + beforeCalibration.current = { + tab: rightTabRef.current, + view: g ? centreOfView(camRef.current, viewportRef.current, g) : null, + forcedOpen: collapsed, + }; setRightTab("calibrate"); - setRightCollapsed(false); + // Opened for the session, not by the operator: not remembered. + if (collapsed) setRightCollapsedUnsaved(false); const rect = sessionRect(session); - if (rect && rect.w > 0 && rect.h > 0 && geomRef.current) { - setCam( - cameraForArea( - padArea(rect), - viewportRef.current, - geomRef.current, - zoomMax(siteRef.current, viewportRef.current, geomRef.current), - ), - ); - } + const fit = + rect && rect.w > 0 && rect.h > 0 + ? (to: ViewGeom) => + cameraForArea( + padArea(rect), + viewportRef.current, + to, + zoomMax(siteRef.current, viewportRef.current, to), + ) + : null; + placeCam(fit, collapsed); return; } if (!session && beforeCalibration.current) { - const { tab, cam: previous } = beforeCalibration.current; + const { tab, view, forcedOpen } = beforeCalibration.current; beforeCalibration.current = null; setRightTab(tab); - setCam(previous); + const collapse = forcedOpen && !rightCollapsedRef.current; + if (collapse) setRightCollapsed(true); + placeCam( + view + ? (to: ViewGeom) => + cameraAtCentre( + view, + viewportRef.current, + to, + zoomMax(siteRef.current, viewportRef.current, to), + ) + : null, + collapse, + ); } - }, [session]); + }, [session, placeCam, setRightCollapsed, setRightCollapsedUnsaved]); // Fetched once: the controller cannot change transport without restarting. useEffect(() => { @@ -346,6 +411,19 @@ export const App: React.FC = () => { return () => window.removeEventListener("keydown", onKey); }, [shortcuts, nothingSelected]); + // 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 (pressed(e, ACTION_KEY.leftPanel)) setRailCollapsed((c) => !c); + else if (pressed(e, ACTION_KEY.rightPanel)) setRightCollapsed((c) => !c); + else return; + e.preventDefault(); + }; + window.addEventListener("keydown", onKey); + return () => window.removeEventListener("keydown", onKey); + }, [shortcuts]); + // replace = set selection to ids · toggle = flip each id · add = union (range select) const onSelect = useCallback((ids: string[], mode: "replace" | "toggle" | "add") => { setSelection((prev) => { @@ -675,6 +753,8 @@ export const App: React.FC = () => { {/* Body row: testbed rail + view area */}
{ siteExtent={siteExtentArea(site)} selection={selection} layers={layers} - robotShapes={robotShapes} + robotDrawing={robotDrawing} plannedMissions={planned.map((m) => { const owner = bots.find((b) => m.ids.includes(b.id) && b.led); return { @@ -820,8 +900,8 @@ export const App: React.FC = () => { layers={layers} layerRows={layerRows} onLayerToggle={(key) => setLayers((prev) => ({ ...prev, [key]: !prev[key] }))} - robotShapes={robotShapes} - onRobotShapesToggle={onRobotShapesToggle} + robotDrawing={robotDrawing} + onRobotDrawing={onRobotDrawing} cameras={cameras} cameraDetections={cameraDetections} cameraOpacity={cameraOpacity} diff --git a/dotbot/console-web/src/BotGlyph.tsx b/dotbot/console-web/src/BotGlyph.tsx index 8703d329..14cd632f 100644 --- a/dotbot/console-web/src/BotGlyph.tsx +++ b/dotbot/console-web/src/BotGlyph.tsx @@ -1,50 +1,26 @@ import React from "react"; -import { headingToGlyphRotation } from "./frame"; +import type { RobotDrawing } from "./robotDrawing"; +import type { BotPose, LH2Position, RgbLed } from "./types"; -// The map marker, traced from the DotBot v3 board outline: one PCB, wide at the -// front and narrower between the wheels, with the tyres outboard of the narrow -// section. Geometry is authored nose-up in a 32-unit box centred on the bot, so -// one unit is size/32 px and every number below scales with the size prop. -export const BOT_GLYPH_BOX = 48; - -// What the robot itself spans inside that box - 25 of the 32 units - for -// callers matching the glyph to a real-world footprint. -export const BOT_GLYPH_SPAN = (BOT_GLYPH_BOX * 25) / 32; - -/** - * The robot's footprint on the floor. The v3 PCB outline is 94.00 mm across - * by 95.00 mm deep, and the tyres fill the steps in its sides, so what the - * robot occupies is about that square. - */ -export const BOT_FOOTPRINT_MM = 95; +// The map marker. One colour rule holds at every level: the robot's fill is +// its swarmit state, a known heading is a white bar, and the LED colour is one +// sensor mark on the photodiode - hollow when the colour is unknown. +// +// The robot's shape is not authored here. The controller expands each +// photodiode fix into a body pose against its own geometry record and ships +// the board path with it, already rotated into the arena frame, so this module +// scales that path to the screen and nothing more. +// +// A pose whose heading the robot never reported is drawn as the sensor point +// alone. A guessed body is worse than a point, and a point is honest about +// what is known. Around it the possible footprint shows the room the body can +// take: the reach and the core radii the controller ships with the pose, +// which hold whichever way the robot faces. /** Screen pixels a bot is drawn at however far the map zooms out. */ export const BOT_MIN_PX = 8; -/** - * The footprint a bot is drawn at, in screen pixels: its true size, floored - * where true size would be a speck too small to see or to click. - */ -export function botFootprintPx(pxPerMm: number): number { - return Math.max(BOT_MIN_PX, BOT_FOOTPRINT_MM * pxPerMm); -} - -/** The glyph box that draws a footprint of `footprintPx`. */ -export function glyphBoxPx(footprintPx: number): number { - return (footprintPx * BOT_GLYPH_BOX) / BOT_GLYPH_SPAN; -} - -const BOARD = - "M-10.7,-11.9 L10.7,-11.9 Q12,-11.9 12,-10.6 L12,-2.4 Q12,-1.1 10.7,-1.1 " + - "L6.9,-1.1 L6.9,10.6 Q6.9,11.9 5.6,11.9 L-5.6,11.9 Q-6.9,11.9 -6.9,10.6 " + - "L-6.9,-1.1 L-10.7,-1.1 Q-12,-1.1 -12,-2.4 L-12,-10.6 Q-12,-11.9 -10.7,-11.9 Z"; - -const TREAD_Y = [2.2, 5.3, 8.4]; - -/** How much of the robot is drawn: the board, or a mark. */ -export type GlyphLevel = "detail" | "dot"; - /** * Screen pixels of footprint the board outline needs before it reads. Judged * from the map at each zoom level, with a bot turned 45 degrees so the @@ -56,6 +32,33 @@ export const GLYPH_DETAIL_PX = 16; /** Past this many bots the map drops a level: the detail is lost in a crowd. */ export const GLYPH_CROWD_BOTS = 200; +/** + * How solid a body built on a travel bearing is drawn. The bearing is the + * robot's direction of travel, which is its heading only while it drives + * straight, so the body is drawn as an estimate rather than as a measurement. + */ +export const TRAVEL_BODY_OPACITY = 0.78; + +/** On-screen diameter of a robot drawn as its sensor point, whatever the zoom. */ +export const SENSOR_POINT_PX = 14; + +/** Width of the heading bar a sensor point carries when its heading is known. */ +const SENSOR_BAR_PX = 2.5; + +/** On-screen diameter below which the possible footprint's ring is not drawn. */ +export const FOOTPRINT_MIN_PX = 26; + +const WHITE = "rgba(255,255,255,.95)"; +const DARK = "rgba(0,0,0,.7)"; + +/** A sensor mark's fill for an LED commanded off. */ +export const LED_OFF = "#161616"; +const SHADOW = + "drop-shadow(0 0 .9px rgba(0,0,0,.6)) drop-shadow(0 1px 2px rgba(0,0,0,.45))"; + +/** How much of the robot is drawn: the board, or a mark. */ +export type GlyphLevel = "detail" | "dot"; + /** * Which glyph a bot of this on-screen size gets. Zoom decides it; a crowded * map is marks at any zoom, since detail nobody can pick apart only costs @@ -66,62 +69,398 @@ export function glyphLevel(footprintPx: number, botCount: number): GlyphLevel { return footprintPx >= GLYPH_DETAIL_PX ? "detail" : "dot"; } +/** + * One robot's body to draw, in millimetres from the pose's photodiode, so it + * can be hung off the point the map already places the bot at. + * + * `spanMm` is the body's own size, measured along its heading and across it, + * rather than the box it happens to occupy in the arena frame - a square board + * turned 45 degrees spans half again as much there, and the chrome around it + * would grow and shrink as the robot turned. + */ +export interface BotBody { + outline: LH2Position[]; + wheels: LH2Position[][]; + centre: LH2Position; + nose: LH2Position; + spanMm: number; + /** The robot's plan-view size, from the pose. */ + envelopeMm: number; + source: BotPose["heading_source"]; +} + +/** Whether a pose says enough about the robot's orientation to draw a body. */ +export function hasHeading(pose: BotPose | null | undefined): boolean { + return !!pose && pose.heading_source !== "none"; +} + +/** + * The body to draw around the pose's photodiode, or null when there is + * nothing to draw one from: no pose, no heading, or an outline too short to + * be a polygon. + */ +export function botBody(pose: BotPose | null | undefined): BotBody | null { + if (!pose || !hasHeading(pose)) return null; + if (pose.outline.length < 3) return null; + const sensor = pose.photodiode; + const rel = (p: LH2Position): LH2Position => ({ + x: p.x - sensor.x, + y: p.y - sensor.y, + }); + const outline = pose.outline.map(rel); + const theta = (pose.heading_deg * Math.PI) / 180; + const forward: LH2Position = { x: -Math.sin(theta), y: Math.cos(theta) }; + const across: LH2Position = { x: -forward.y, y: forward.x }; + const extent = (axis: LH2Position): number => { + const along = outline.map((p) => p.x * axis.x + p.y * axis.y); + return Math.max(...along) - Math.min(...along); + }; + return { + outline, + wheels: pose.wheels.map((wheel) => wheel.map(rel)), + centre: rel(pose.centre), + nose: rel(pose.nose), + spanMm: Math.max(extent(forward), extent(across)), + envelopeMm: pose.envelope_mm, + source: pose.heading_source, + }; +} + +/** + * The footprint a bot is drawn at, in screen pixels: its true size, floored + * where true size would be a speck too small to see or to click. A bot with + * no body is that floor, being a point rather than an area. + */ +export function botFootprintPx(pxPerMm: number, spanMm: number): number { + return Math.max(BOT_MIN_PX, spanMm * pxPerMm); +} + +/** + * What one robot is drawn as. `board` is the full glyph; `mark` a disc with + * a heading bar and no rim, for a board too small to read; `disc` the real-size + * envelope with its heading, for a board big enough but lost in a crowd; + * `sensor` the photodiode point with the possible footprint around it, whose + * radii are in screen pixels and null where they are not drawn. + */ +export type RobotShape = + | { kind: "board"; body: BotBody } + | { kind: "mark"; body: BotBody } + | { kind: "disc"; body: BotBody; radiusPx: number } + | { + kind: "sensor"; + ringPx: number | null; + corePx: number | null; + crowded: boolean; + /** The heading as a bar out from the point, when the pose has one. */ + bar: HeadingBarShape | null; + }; + +/** A unit vector in screen axes, and how far out from the point it is drawn. */ +export interface HeadingBarShape { + dir: LH2Position; + lengthPx: number; +} + +export interface RobotDraw { + shape: RobotShape; + /** Where the chrome around the robot sits, in mm from the photodiode. */ + centre: LH2Position; + /** The size on screen the chrome is laid out around. */ + footprintPx: number; + /** Whether the chrome turns with the heading, hugging a drawn board. */ + turned: boolean; + /** A board built on the travel bearing, drawn fainter as an estimate. */ + estimate: boolean; + battery: boolean; +} + +/** How a robot with this pose is drawn at this zoom, in a fleet of `botCount`. */ +export function robotDraw( + pose: BotPose | null | undefined, + drawing: RobotDrawing, + pxPerMm: number, + botCount: number, +): RobotDraw { + const body = drawing.mode === "body" ? botBody(pose) : null; + if (body) { + const footprintPx = botFootprintPx(pxPerMm, body.spanMm); + const flat = { centre: body.centre, turned: false, estimate: false }; + if (glyphLevel(footprintPx, botCount) === "detail") { + return { + shape: { kind: "board", body }, + centre: body.centre, + footprintPx, + turned: true, + estimate: body.source === "travel", + battery: true, + }; + } + // Too small to read is a mark; readable but crowded out is the envelope. + if (footprintPx < GLYPH_DETAIL_PX) { + return { ...flat, shape: { kind: "mark", body }, footprintPx, battery: false }; + } + const discPx = botFootprintPx(pxPerMm, body.envelopeMm); + return { + ...flat, + shape: { kind: "disc", body, radiusPx: discPx / 2 }, + footprintPx: discPx, + battery: false, + }; + } + + const reachPx = pose ? pose.reach_mm * pxPerMm : 0; + const corePx = pose ? pose.core_mm * pxPerMm : 0; + const ring = drawing.footprint && 2 * reachPx >= FOOTPRINT_MIN_PX; + const core = drawing.footprint && 2 * corePx >= SENSOR_POINT_PX + 4; + const envelopePx = botFootprintPx(pxPerMm, pose ? pose.envelope_mm : 0); + return { + shape: { + kind: "sensor", + ringPx: ring ? reachPx : null, + corePx: core ? corePx : null, + crowded: botCount > GLYPH_CROWD_BOTS, + bar: headingBar(pose, Math.max(SENSOR_POINT_PX, reachPx)), + }, + centre: { x: 0, y: 0 }, + footprintPx: ring ? 2 * reachPx : SENSOR_POINT_PX, + turned: false, + estimate: false, + battery: glyphLevel(envelopePx, botCount) === "detail", + }; +} + +/** The pose's heading as a bar `lengthPx` long, or null when it has none. */ +function headingBar(pose: BotPose | null | undefined, lengthPx: number): HeadingBarShape | null { + if (!pose || !hasHeading(pose)) return null; + const dx = pose.nose.x - pose.centre.x; + const dy = pose.nose.y - pose.centre.y; + const n = Math.hypot(dx, dy); + if (n === 0) return null; + return { dir: { x: dx / n, y: dy / n }, lengthPx }; +} + +/** How far from the photodiode the body reaches, in millimetres, tyres included. */ +function reachMm(body: BotBody): number { + const points = [...body.outline, ...body.wheels.flat()]; + return Math.max(...points.map((p) => Math.hypot(p.x, p.y))); +} + interface BotGlyphProps { - color: string; - heading: number | null; // degrees, 0 = +y, positive clockwise in the arena frame - // A DotBot: drawn as the board even when it reports no heading, nose-up. - dotBot?: boolean; - size?: number; - level?: GlyphLevel; + /** The swarmit state colour: the robot's fill at every level. */ + state: string; + /** The LED colour the controller commanded, or null when it is unknown. */ + led: RgbLed | null; + shape: RobotShape; + pxPerMm: number; + /** The circle's diameter, for `mark`. */ + footprintPx: number; } -const body = (color: string, board: boolean, level: GlyphLevel) => { - if (!board) return ; - // Too small for a front to read: position and state are all that is left. - if (level === "dot") return ; +const Frame: React.FC<{ half: number; children: React.ReactNode; filter?: boolean }> = ({ + half, + children, + filter = true, +}) => { + const side = 2 * (half + 3); return ( - <> - - - + + {children} + + ); +}; + +/** The CSS colour a sensor mark is filled with, or null for a hollow one. */ +export function ledFill(led: RgbLed | null): string | null { + if (!led) return null; + if (led.red === 0 && led.green === 0 && led.blue === 0) return LED_OFF; + return `rgb(${led.red},${led.green},${led.blue})`; +} + +/** + * The one mark in the LED colour, at the origin: a white rim inside a dark + * one, so it reads on a fill of its own colour and on either theme. + */ +const SensorMark: React.FC<{ r: number; led: RgbLed | null }> = ({ r, led }) => { + const rim = Math.max(0.6, r / 3); + const halo = Math.max(0.5, rim * 0.8); + const fill = ledFill(led); + if (fill === null) { + return ( + + + - - {TREAD_Y.map((y) => ( - - - - - ))} + ); + } + return ( + + + + + ); +}; + +/** A white bar from `from` along `dir`, `length` px long. */ +const HeadingBar: React.FC<{ + from: LH2Position; + dir: LH2Position; + length: number; + width: number; + layer: string; +}> = ({ from, dir, length, width, layer }) => ( + +); + +const unit = (from: LH2Position, to: LH2Position): LH2Position => { + const dx = to.x - from.x; + const dy = to.y - from.y; + const n = Math.hypot(dx, dy) || 1; + return { x: dx / n, y: dy / n }; +}; + +const SensorPoint: React.FC<{ + state: string; + led: RgbLed | null; + ringPx: number | null; + corePx: number | null; + crowded: boolean; + bar: HeadingBarShape | null; +}> = ({ state, led, ringPx, corePx, crowded, bar }) => { + const pointR = SENSOR_POINT_PX / 2; + return ( + + {ringPx !== null && ( + <> + {/* a solid casing under the dashes, so the ring stands off a + camera picture or a floor of its own colour */} + + + + )} + {corePx !== null && ( + + )} + {bar && ( + + {/* a dark edge, so the white bar reads off a light floor too */} + + + + )} + {/* the point the lighthouse reported: the sensor mark, rimmed in the + state colour */} + + + - - + ); }; -export const BotGlyph: React.FC = ({ - color, - heading, - dotBot = false, - size = BOT_GLYPH_BOX, - level = "detail", -}) => { - const board = dotBot || heading !== null; +/** + * The bot as one SVG whose origin is the pose's photodiode, so the caller places + * it at the point it already has and the body falls where the pose puts it. + */ +export const BotGlyph: React.FC = ({ state, led, shape, pxPerMm, footprintPx }) => { + if (shape.kind === "sensor") return ; + const body = shape.body; + const px = (p: LH2Position): LH2Position => ({ + x: p.x * pxPerMm, + y: p.y * pxPerMm, + }); + const centre = px(body.centre); + const dir = unit(centre, px(body.nose)); + const offset = Math.hypot(centre.x, centre.y); + + if (shape.kind === "mark" || shape.kind === "disc") { + const r = shape.kind === "disc" ? shape.radiusPx : footprintPx / 2; + // Floors keep the bar and the mark legible on the smallest circle. + const barW = shape.kind === "disc" ? Math.max(1.2, r / 5) : Math.min(3, Math.max(1.2, r / 3.2)); + const markR = shape.kind === "disc" ? Math.max(2, Math.min(7, r * 0.2)) : Math.max(1.1, r * 0.16); + return ( + + + + + + ); + } + + const stroke = Math.max(0.6, footprintPx / 40); return ( - - {body(color, board, level)} - + + {/* the tyres, at the place and size the record gives them: the board is + drawn over them, so only what sticks out shows */} + {body.wheels.map((wheel, i) => ( + `${p.x * pxPerMm},${p.y * pxPerMm}`).join(" ")} + fill="var(--tyre)" + stroke="rgba(0,0,0,.45)" + strokeWidth={stroke} + /> + ))} + `${p.x * pxPerMm},${p.y * pxPerMm}`).join(" ")} + fill={state} + stroke="rgba(0,0,0,.45)" + strokeWidth={stroke} + /> + + ); }; diff --git a/dotbot/console-web/src/CalibrationLayer.tsx b/dotbot/console-web/src/CalibrationLayer.tsx index c2bb467a..91cce29a 100644 --- a/dotbot/console-web/src/CalibrationLayer.tsx +++ b/dotbot/console-web/src/CalibrationLayer.tsx @@ -1,13 +1,12 @@ import React from "react"; -import { BOT_GLYPH_BOX, BotGlyph } from "./BotGlyph"; import { insideFromCorner, noseHeading, outstandingIndex, sessionRect, } from "./calibration"; -import { areaToFraction } from "./frame"; +import { areaToFraction, headingToGlyphRotation } from "./frame"; import type { Area, CalibrationSession } from "./types"; // Calibration mode drawn over the map: the rectangle the session's points @@ -24,7 +23,37 @@ const pctOf = (x: number, y: number, box: Area) => { // The numbers are chrome, not objects on the floor, so they keep their size // whatever the map's real-scale layer does to the robot glyphs. -const GLYPH_SIZE = BOT_GLYPH_BOX * 0.7; +const GLYPH_SIZE = 34; + +/** + * Where to stand the robot and which way its nose goes. A placement is an + * instruction rather than a measurement, so it is drawn as a direction in a + * box and not as the board: the board outline on the map is a pose the + * controller measured, and nothing here has one. + */ +const Placement: React.FC<{ heading: number }> = ({ heading }) => ( + + + + +); const Marker: React.FC<{ index: number; @@ -60,7 +89,7 @@ const Marker: React.FC<{ opacity: 0.9, }} > - +
)}
`0x${(v >>> 0).toString(16).padStart(8, "0")}`; // FaultType values that actually populate the fault status registers. A @@ -52,6 +63,7 @@ export function infoText(bot: UnifiedBot): string { out.push( `Position ${bot.position ? `${Math.round(bot.position.x)}, ${Math.round(bot.position.y)}` : "no fix"}`, ); + out.push(`Heading ${formatHeading(bot)}`); if (info) { out.push(""); out.push(`Image ${info.image_name || "(unnamed)"}`); @@ -156,6 +168,7 @@ const Card: React.FC<{ bot: UnifiedBot }> = ({ bot }) => { k="Position" v={bot.position ? `${Math.round(bot.position.x)}, ${Math.round(bot.position.y)}` : "no fix"} /> + {info && ( <> diff --git a/dotbot/console-web/src/Joystick.tsx b/dotbot/console-web/src/Joystick.tsx index 91a401d7..11904928 100644 --- a/dotbot/console-web/src/Joystick.tsx +++ b/dotbot/console-web/src/Joystick.tsx @@ -1,6 +1,7 @@ import React, { useEffect, useRef, useState } from "react"; import { putMoveRaw } from "./api"; +import { hasHeading } from "./BotGlyph"; import { UnifiedBot } from "./types"; // v1 drive pad: 64px rounded square, crosshair guides, LED-colored knob with @@ -67,6 +68,7 @@ export const Pad: React.FC = ({ targets, disabled }) => { const knobPx = { x: (knob.x / CONTROL_R) * R, y: (knob.y / CONTROL_R) * R }; const single = targets.length === 1 ? targets[0] : null; + const heading = single?.pose && hasHeading(single.pose) ? single.pose.heading_deg : null; const led = single ? single.led ? `rgb(${single.led.red},${single.led.green},${single.led.blue})` @@ -150,7 +152,7 @@ export const Pad: React.FC = ({ targets, disabled }) => { transition: active ? "none" : "transform .15s ease", }} > - {single.heading !== null && ( + {heading !== null && (
= ({ targets, disabled }) => { borderLeft: "5px solid transparent", borderRight: "5px solid transparent", borderBottom: "9px solid rgba(255,255,255,.92)", - transform: `translate(-50%, -50%) rotate(${-single.heading}deg) translateY(-13px)`, + transform: `translate(-50%, -50%) rotate(${-heading}deg) translateY(-13px)`, }} /> )} diff --git a/dotbot/console-web/src/MapView.tsx b/dotbot/console-web/src/MapView.tsx index 53f74f5c..daebca46 100644 --- a/dotbot/console-web/src/MapView.tsx +++ b/dotbot/console-web/src/MapView.tsx @@ -29,7 +29,8 @@ import { scaleBar, ticksInSite, } from "./grid"; -import { BotGlyph, botFootprintPx, glyphBoxPx, glyphLevel } from "./BotGlyph"; +import { BotGlyph, TRAVEL_BODY_OPACITY, botFootprintPx, robotDraw } from "./BotGlyph"; +import { DEFAULT_ROBOT_DRAWING, RobotDrawing } from "./robotDrawing"; import { MAP_MODIFIER, SHORTCUTS_KEY, holds, roleOf } from "./shortcuts"; import { ResetBadge, batteryColor, batteryPct, stateColor } from "./viewChrome"; @@ -50,6 +51,7 @@ import { ZOOM_MIN, cameraForArea, clampCam, + refitCam, scaleForFraction, steppedScale, viewCentre, @@ -101,8 +103,8 @@ interface MapViewProps { siteExtent: Area | null; selection: Set; layers: Layers; - // False draws every bot as a plain mark, whatever it is and wherever it faces. - robotShapes?: boolean; + // Whether robots are drawn as their bodies or their sensor points. + robotDrawing?: RobotDrawing; // Local queues, not yet sent: the robots each is bound to, and its points. plannedMissions: { ids: string[]; waypoints: LH2Position[]; led: string | null }[]; cam: Camera; @@ -153,10 +155,12 @@ const BOTTOM_LINE_PX = const DRAG_MIN_PX = 5; // The selection ring hugs the robot: its footprint plus this on every side. const SELECTION_PAD_PX = 3; -// A waypoint diamond is a fraction of the robot it belongs to, floored where -// the robot is a dot. -const WAYPOINT_OF_FOOTPRINT = 0.35; -const WAYPOINT_MIN_PX = 5; +// A waypoint diamond is a fraction of the body of the robot it belongs to, +// held between a floor it can still be seen at and a cap that keeps it a +// marker rather than an object: a waypoint is a point on the floor. +export const WAYPOINT_OF_BODY = 0.3; +export const WAYPOINT_MIN_PX = 7; +export const WAYPOINT_MAX_PX = 14; // How much of an area's colour washes its floor. const AREA_TINT = 0.05; @@ -192,6 +196,8 @@ export const MapView: React.FC = (props) => { const [box, setBox] = useState(() => viewGeom(1000, 600, props.viewport)); const onGeomRef = useRef(props.onGeom); onGeomRef.current = props.onGeom; + const setCamRef = useRef(setCam); + setCamRef.current = setCam; const viewportRef = useRef(props.viewport); viewportRef.current = props.viewport; // Track the canvas size live (rail open/close, window resize): the arena @@ -202,11 +208,23 @@ export const MapView: React.FC = (props) => { React.useEffect(() => { const el = wrapRef.current; if (!el) return; + // The first measure replaces a placeholder; a later one is a resize, and + // the camera is carried across it so the floor does not jump. + let measured = false; + let centre = { x: 0, y: 0 }; const update = () => { const r = el.getBoundingClientRect(); const g = viewGeom(r.width, r.height, viewportRef.current); + const prev = geomRef.current; + const prevCentre = centre; + centre = { x: r.left + r.width / 2, y: r.top + r.height / 2 }; setBox(g); geomRef.current = g; + if (measured && (g.w !== prev.w || g.h !== prev.h)) { + const shift = { x: centre.x - prevCentre.x, y: centre.y - prevCentre.y }; + setCamRef.current((c) => refitCam(c, prev, g, shift)); + } + measured = true; onGeomRef.current(g); }; update(); @@ -523,26 +541,32 @@ export const MapView: React.FC = (props) => { geomNow.h - RULER_CONTROLS_PX, ); + const drawing = props.robotDrawing ?? DEFAULT_ROBOT_DRAWING; // The robot is an object on the floor, so it is drawn at the floor's own // scale: zooming in tells the truth about how much room it takes. Zooming // out floors it at a size that can still be seen and clicked. - const footprintPx = botFootprintPx(perMm); - const glyphPx = glyphBoxPx(footprintPx); - // How much of the robot is worth drawing at that size, with the fleet's own - // size as the tie-breaker. - const level = glyphLevel(footprintPx, props.bots.length); - // What sits around the robot - selection, badges, labels - is chrome, and - // keeps its size on screen whatever the camera does. - const selectionPx = footprintPx + SELECTION_PAD_PX * 2; - const waypointPx = Math.max(WAYPOINT_MIN_PX, footprintPx * WAYPOINT_OF_FOOTPRINT); - // What sits on top of the robot shrinks with it, to a floor, so a bot the - // size of a dot is not buried under its own indicators. - const drivePx = Math.max(3, Math.min(10, footprintPx * 0.32)); - const batteryPx = Math.max(14, Math.min(28, footprintPx)); - // A robot drawn as a mark is one nobody reads per-robot detail on, so its - // own indicators go with the board: the selection ring and the reset badge - // stay, being how a robot is found rather than what it says. - const indicators = level === "detail"; + const botDraw = (b: UnifiedBot) => { + const draw = robotDraw(b.pose, drawing, perMm, props.bots.length); + const { footprintPx } = draw; + return { + draw, + footprintPx, + // What sits around the robot - selection, badges, labels - is chrome, + // and keeps its size on screen whatever the camera does. + selectionPx: footprintPx + SELECTION_PAD_PX * 2, + // Sized from the body, never from the possible footprint's ring. + waypointPx: Math.min( + WAYPOINT_MAX_PX, + Math.max( + WAYPOINT_MIN_PX, + (draw.shape.kind === "sensor" + ? botFootprintPx(perMm, b.pose ? b.pose.envelope_mm : 0) + : footprintPx) * WAYPOINT_OF_BODY, + ), + ), + batteryPx: Math.max(14, Math.min(28, footprintPx)), + }; + }; return (
= (props) => { /> )} {/* What the camera makes of the robot standing on this - floor: its board outline, a line from the centre to + floor: its board outline and its two tyres, the same + parts the map glyph draws, a line from the centre to the nose so the heading is readable, and a dot on the photodiode, which is the point the lighthouse reports and so the one the two can be compared at. */} @@ -677,6 +702,19 @@ export const MapView: React.FC = (props) => { data-testid={`camera-detection-${camera.area}`} style={{ pointerEvents: "none" }} > + {(pose.wheels_mm ?? []).map((wheel, i) => ( + + ))} = (props) => { props.bots.flatMap((b) => { if (!props.selection.has(b.id) || b.waypoints.length === 0) return []; const led = ledCss(b); + const { waypointPx } = botDraw(b); return b.waypoints.map((w, i) => { const q = pctPos(w); return ( @@ -815,7 +854,7 @@ export const MapView: React.FC = (props) => { transform: `translate(-50%, -50%) rotate(45deg) scale(${chrome})`, background: led, border: `1.5px solid ${led}`, - boxShadow: `0 0 7px ${led}`, + boxShadow: `0 0 4px ${led}`, pointerEvents: "none", }} /> @@ -825,8 +864,12 @@ export const MapView: React.FC = (props) => { {props.layers.waypoints && props.plannedMissions .filter((m) => m.ids.some((id) => props.selection.has(id))) - .flatMap((m) => - m.waypoints.map((p, i) => { + .flatMap((m) => { + const owner = props.bots.find((b) => m.ids.includes(b.id)); + const waypointPx = owner + ? botDraw(owner).waypointPx + : WAYPOINT_MIN_PX; + return m.waypoints.map((p, i) => { const q = pctPos(p); const led = m.led ?? "var(--accent)"; return ( @@ -843,15 +886,15 @@ export const MapView: React.FC = (props) => { transform: `translate(-50%, -50%) rotate(45deg) scale(${chrome})`, background: "transparent", border: `1.5px dashed ${led}`, - boxShadow: `0 0 7px ${led}`, + boxShadow: `0 0 4px ${led}`, pointerEvents: "none", }} /> ); - }), - )} + }); + })} - {/* bots (v1 glyph: state-colored body, LED pip, drive dot, chip label) */} + {/* bots: the glyph, its chrome and the chip label */} {props.layers.dotBots && props.bots .filter((b) => b.position) @@ -864,15 +907,30 @@ export const MapView: React.FC = (props) => { const solid = robotOpacityAt(robotFades, at); const selected = props.selection.has(b.id); const hovered = hoverId === b.id; - const led = ledCss(b); const stc = stateColor(b.state); const pct = batteryPct(b); const blink = b.state === "Programming" || b.state === "Resetting"; + // A robot drawn as a mark is one nobody reads per-robot detail + // on, so its own indicators go with the board: the selection + // ring and the reset badge stay, being how a robot is found + // rather than what it says. + const { draw, footprintPx, selectionPx, batteryPx } = botDraw(b); + // The board is drawn where the pose puts it, which is not + // where the photodiode is: the chrome goes with the board, so + // the ring and the label stay around the robot rather than + // around its sensor. + const bodyDx = draw.centre.x * perMm; + const bodyDy = draw.centre.y * perMm; + // A body built on the travel bearing is an estimate: it is + // right while the robot drives straight and wrong the rest of + // the time, so it is drawn as one. + const bodySolid = solid * (draw.estimate ? TRAVEL_BODY_OPACITY : 1); // The board turns with the heading; the ring around it turns // too, so it hugs the board whichever way the robot faces. - const shaped = props.robotShapes ?? true; - const turned = shaped && level === "detail" && b.heading !== null; - const turn = turned ? headingToGlyphRotation(b.heading!) : 0; + const turned = draw.turned; + const turn = turned + ? headingToGlyphRotation(b.pose!.heading_deg) + : 0; // How far below the centre a turned box reaches, as a // fraction of its half side. const reach = turned @@ -917,6 +975,17 @@ export const MapView: React.FC = (props) => { height: 0, }} > + {/* Everything that marks the robot out rides on its body, + which is not where its sensor is. */} +
{/* last-reset warning, centred over the glyph body */}
= (props) => { height: selectionPx, transform: `translate(-50%, -50%) rotate(${turn}deg)`, border: "1.5px solid var(--accent)", - borderRadius: 3, + // Square around a board, round around anything round. + borderRadius: turned ? 3 : "50%", boxShadow: "0 0 0 3px color-mix(in srgb, var(--accent) 14%, transparent)", }} /> )} {/* battery bar */} - {props.layers.batteryBars && indicators && ( + {props.layers.batteryBars && draw.battery && (
= (props) => {
)} - {/* body and heading are one glyph: it rotates as a piece */} -
- -
- {/* drive dot: white ring at center = drivable; its FILL is - the LED color (experiment: merges the v1 LED pip into the - drive indicator - see design-feedback) */} - {b.drivable && indicators && ( -
- )} {/* chip label: selected or hovered only */} {(selected || hovered) && (
= (props) => { {b.id.slice(-4).toUpperCase()}
)} +
+ {/* The body, hung off the photodiode fix this container + sits on: the pose puts it where it belongs. */} +
+ +
); })} diff --git a/dotbot/console-web/src/Minimap.tsx b/dotbot/console-web/src/Minimap.tsx index e1ef0777..b8bedcc6 100644 --- a/dotbot/console-web/src/Minimap.tsx +++ b/dotbot/console-web/src/Minimap.tsx @@ -1,7 +1,7 @@ import React, { useEffect, useRef, useState } from "react"; import { areaColor } from "./areaColor"; -import { areaToFraction, siteExtentArea } from "./frame"; +import { areaToFraction, fractionToArea, siteExtentArea } from "./frame"; import { MINIMAP_TARGET_PX, gridStepMm } from "./grid"; import { minimapLabel } from "./localization"; import { stateColor } from "./viewChrome"; @@ -53,6 +53,9 @@ export const Minimap: React.FC = ({ return () => ro.disconnect(); }, []); + // The whole site, never the viewport: the box below is what moves. + const box: Area = siteExtentArea(site) ?? viewport; + const viewportRect = () => { if (!geom) return null; const { w, h, boxW, boxH } = geom; @@ -72,12 +75,14 @@ export const Minimap: React.FC = ({ const r = el.getBoundingClientRect(); const fx = Math.max(0, Math.min(1, (clientX - r.left) / r.width)); const fy = Math.max(0, Math.min(1, (clientY - r.top) / r.height)); + // A point of the minimap's site, as a fraction of the drawn viewport. + const at = areaToFraction(fractionToArea(fx, fy, box), viewport); setCam((c) => clampCam( { ...c, - tx: -(fx - 0.5) * geom.boxW * c.scale, - ty: -(fy - 0.5) * geom.boxH * c.scale, + tx: -(at.fx - 0.5) * geom.boxW * c.scale, + ty: -(at.fy - 0.5) * geom.boxH * c.scale, }, geom, ), @@ -85,8 +90,6 @@ export const Minimap: React.FC = ({ }; const rect = viewportRect(); - // The whole site, never the viewport: the box below is what moves. - const box: Area = siteExtentArea(site) ?? viewport; // viewportRect speaks fractions of the drawn viewport; the minimap draws // the site, so the box has to be re-expressed against it. const onBox = (fraction: number, axis: "x" | "y") => { diff --git a/dotbot/console-web/src/PanelToggle.tsx b/dotbot/console-web/src/PanelToggle.tsx new file mode 100644 index 00000000..dcee39e8 --- /dev/null +++ b/dotbot/console-web/src/PanelToggle.tsx @@ -0,0 +1,31 @@ +import React from "react"; + +import { ACTION_KEY } from "./shortcuts"; + +import "./mapChrome.css"; + +// The button that collapses or expands a side panel. The chevron points the +// way the panel will move. + +export const PanelToggle: React.FC<{ + side: "left" | "right"; + collapsed: boolean; + onToggle: () => void; +}> = ({ side, collapsed, onToggle }) => { + const label = `${collapsed ? "Expand" : "Collapse"} the ${side} panel`; + const key = side === "left" ? ACTION_KEY.leftPanel : ACTION_KEY.rightPanel; + const pointsRight = (side === "left") === collapsed; + return ( + + ); +}; diff --git a/dotbot/console-web/src/RightPane.tsx b/dotbot/console-web/src/RightPane.tsx index 566ef69f..f4a356ac 100644 --- a/dotbot/console-web/src/RightPane.tsx +++ b/dotbot/console-web/src/RightPane.tsx @@ -13,6 +13,8 @@ import { } from "./cameraLayer"; import { InspectorBody } from "./Inspector"; import { DETECTION_TEXT } from "./localization"; +import { PanelToggle } from "./PanelToggle"; +import type { DrawMode, RobotDrawing } from "./robotDrawing"; import { SetupCard } from "./SetupCard"; import { StepCard } from "./StepCard"; import type { Layers } from "./MapView"; @@ -118,6 +120,83 @@ export const CheckRow: React.FC<{
); +const DRAW_MODES: { value: DrawMode; text: string }[] = [ + { value: "body", text: "Body" }, + { value: "sensor", text: "Sensor" }, +]; + +const DRAWING_HINT: Record = { + body: [ + "A robot with no heading is drawn as its sensor point.", + "No heading: its sensor, ringed by where its body could be.", + ], + sensor: [ + "Every robot is drawn as the point its sensor reports.", + "Every robot is its sensor, ringed by where its body could be.", + ], +}; + +// Body or Sensor, and whether a sensor point carries its possible footprint. +const RobotDrawingControl: React.FC<{ + value: RobotDrawing; + onChange: (next: RobotDrawing) => void; +}> = ({ value, onChange }) => ( +
+
+ {DRAW_MODES.map((m) => ( +
onChange({ ...value, mode: m.value })} + onKeyDown={(e) => { + if (e.key === "Enter" || e.key === " ") { + e.preventDefault(); + onChange({ ...value, mode: m.value }); + } + }} + style={{ + flex: 1, + textAlign: "center", + padding: "4px 0", + borderRadius: 5, + fontSize: 12, + fontWeight: 500, + cursor: "pointer", + background: value.mode === m.value ? "var(--accent)" : "transparent", + color: value.mode === m.value ? "#fff" : "var(--muted)", + }} + > + {m.text} +
+ ))} +
+ onChange({ ...value, footprint: !value.footprint })} + /> +
+ {DRAWING_HINT[value.mode][value.footprint ? 1 : 0]} +
+
+); + // One of a camera row's two opacities: a labelled track and what it reads. // The label is given a width so the tracks line up under each other, the two // being read against one another. @@ -325,8 +404,8 @@ interface RightPaneProps { layers: Layers; layerRows: { key: keyof Layers; label: string }[]; onLayerToggle: (key: keyof Layers) => void; - robotShapes?: boolean; - onRobotShapesToggle?: () => void; + robotDrawing?: RobotDrawing; + onRobotDrawing?: (next: RobotDrawing) => void; // The cameras the controller warps. None registered, no Camera heading. cameras?: RegisteredCamera[]; // What each camera's detector last made of its own area, keyed by area. @@ -370,13 +449,7 @@ export const RightPane: React.FC = (props) => { zIndex: 11, }} > -
props.setCollapsed(false)} - title="Open the right pane" - style={{ ...ico, background: "transparent", border: "none", fontSize: 15, color: "var(--muted)" }} - > - ‹ -
+ props.setCollapsed(false)} />
{tabs.map((tab) => (
= (props) => { ))}
- props.setCollapsed(true)} - title="Collapse the right pane" - style={{ cursor: "pointer", color: "var(--muted)", fontSize: 15, lineHeight: 1 }} - > - › - +
+ props.setCollapsed(true)} /> +
@@ -467,12 +536,10 @@ export const RightPane: React.FC = (props) => { onToggle={() => props.onLayerToggle(row.key)} /> ))} - {props.onRobotShapesToggle && ( - )} diff --git a/dotbot/console-web/src/TestbedRail.tsx b/dotbot/console-web/src/TestbedRail.tsx index 5226c510..0539b12f 100644 --- a/dotbot/console-web/src/TestbedRail.tsx +++ b/dotbot/console-web/src/TestbedRail.tsx @@ -10,6 +10,7 @@ import { UnifiedBot, } from "./types"; import { FirmwareSection } from "./FirmwareSection"; +import { PanelToggle } from "./PanelToggle"; import { FirmwareFile } from "./firmwareFile"; import { FlashJob, LogRow } from "./useOrchestration"; @@ -36,6 +37,8 @@ interface Mission { } interface TestbedRailProps { + collapsed: boolean; + setCollapsed: (collapsed: boolean) => void; bots: UnifiedBot[]; selection: Set; planned: PlannedMission[]; @@ -157,12 +160,10 @@ export function deriveMissions(bots: UnifiedBot[], planned: PlannedMission[]): M } export const TestbedRail: React.FC = (props) => { - // The panel is open by default; ?rail=collapsed starts it as the icon strip, - // and ?rail=testbed|missions picks which tab is on top. + // ?rail=testbed|missions|localization picks which tab is on top. const railParam = new URLSearchParams(window.location.search).get("rail"); - const [mode, setMode] = useState<"collapsed" | "panel">( - railParam === "collapsed" ? "collapsed" : "panel", - ); + const mode = props.collapsed ? "collapsed" : "panel"; + const expand = () => props.setCollapsed(false); const [top, setTop] = useState<"testbed" | "missions" | "localization">( railParam === "missions" ? "missions" @@ -204,9 +205,7 @@ export const TestbedRail: React.FC = (props) => { {/* collapsed icon strip */} {mode === "collapsed" && (
-
setMode("panel")} title="Open testbed" style={{ ...ico, cursor: "pointer" }}> - ▤ -
+
{[ { g: "▶", t: "Start", fn: props.onStart }, @@ -219,7 +218,7 @@ export const TestbedRail: React.FC = (props) => {
{ - setMode("panel"); + expand(); setTop("testbed"); setTab("console"); }} @@ -230,7 +229,7 @@ export const TestbedRail: React.FC = (props) => {
{ - setMode("panel"); + expand(); setTop("missions"); }} title={`Missions (${missions.length})`} @@ -262,7 +261,7 @@ export const TestbedRail: React.FC = (props) => {
{ - setMode("panel"); + expand(); setTop("localization"); }} title="Localization" @@ -314,13 +313,9 @@ export const TestbedRail: React.FC = (props) => {
- setMode("collapsed")} - title="Collapse" - style={{ cursor: "pointer", color: "var(--muted)", fontSize: 15, lineHeight: 1 }} - > - ‹ - +
+ props.setCollapsed(true)} /> +
{/* TESTBED tab */} diff --git a/dotbot/console-web/src/api.ts b/dotbot/console-web/src/api.ts index 5c06b5c9..7140ca94 100644 --- a/dotbot/console-web/src/api.ts +++ b/dotbot/console-web/src/api.ts @@ -1,4 +1,5 @@ import { + BotPose, CalibrationPreview, CalibrationPushed, CalibrationSaved, @@ -29,6 +30,18 @@ export async function fetchSite(): Promise { return res.json(); } +// The headingless pose of each swarmit device type, photodiode at the origin. +// A controller too old to know the route has none to give. +export async function fetchDevicePoses(): Promise> { + try { + const res = await fetch(`${CONTROLLER}/device_poses`); + if (!res.ok) return {}; + return await res.json(); + } catch { + return {}; + } +} + // A controller with no registered camera answers with an empty list, and one // too old to know the route answers 404; both mean the same thing to the // console - no camera layer - so neither is an error. diff --git a/dotbot/console-web/src/botChrome.test.tsx b/dotbot/console-web/src/botChrome.test.tsx index 6b8490b1..019e89e5 100644 --- a/dotbot/console-web/src/botChrome.test.tsx +++ b/dotbot/console-web/src/botChrome.test.tsx @@ -4,9 +4,10 @@ import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; import { botFootprintPx } from "./BotGlyph"; import { pxPerMm } from "./grid"; -import { MapView } from "./MapView"; -import type { Area, LH2Position, Site, UnifiedBot } from "./types"; -import { Camera, SITE_CAMERA, viewGeom } from "./zoom"; +import { MapView, WAYPOINT_MAX_PX, WAYPOINT_MIN_PX, WAYPOINT_OF_BODY } from "./MapView"; +import type { RobotDrawing } from "./robotDrawing"; +import type { Area, BotPose, LH2Position, Site, UnifiedBot } from "./types"; +import { Camera, FRAME_CAMERA, viewGeom } from "./zoom"; const ARENA: Area = { x: 0, y: 0, w: 2000, h: 2000, name: "arena" }; const C405: Site = { @@ -19,17 +20,63 @@ const VIEWPORT: Area = { x: -2000, y: -2000, w: 6000, h: 8000 }; const CANVAS = { w: 900, h: 600 }; const GEOM = viewGeom(CANVAS.w, CANVAS.h, VIEWPORT); +// A v3 body as the controller ships it: the board path around a fix, already +// rotated, in frame millimetres. 95 mm long and 94 wide, so at heading 45 it +// is the awkward case the chrome has to keep hugging. +const V3_AT_ORIGIN: LH2Position[] = [ + { x: -43, y: 10.5 }, + { x: -47, y: 10.5 }, + { x: -47, y: -27.5 }, + { x: -28.5, y: -27.5 }, + { x: -28.5, y: -76.5 }, + { x: 28.5, y: -76.5 }, + { x: 28.5, y: -27.5 }, + { x: 47, y: -27.5 }, + { x: 47, y: 10.5 }, + { x: 43, y: 10.5 }, + { x: 42, y: 11.5 }, + { x: 42, y: 18.5 }, + { x: -42, y: 18.5 }, + { x: -42, y: 11.5 }, +]; +const V3_SPAN_MM = 95; + +// The body of a bot standing at `at` facing `heading`, rotated the way the +// controller rotates it. +const bodyPose = (at: LH2Position, heading = 45): BotPose => { + const theta = (heading * Math.PI) / 180; + const place = (p: LH2Position): LH2Position => ({ + x: at.x + p.x * Math.cos(theta) - p.y * Math.sin(theta), + y: at.y + p.x * Math.sin(theta) + p.y * Math.cos(theta), + }); + return { + heading_deg: heading, + heading_source: "travel", + photodiode: at, + axle: place({ x: 0, y: -53.5 }), + centre: place({ x: 0, y: -29 }), + nose: place({ x: 0, y: 18.5 }), + led: place({ x: 0, y: 5.5 }), + outline: V3_AT_ORIGIN.map(place), + wheels: [], + // The radii the controller ships with every pose, whatever its heading. + reach_mm: 89.33, + core_mm: 18.5, + envelope_mm: 95, + }; +}; + const bot = (id: string, position: LH2Position, extra: Partial = {}): UnifiedBot => ({ id, state: "Running", link: "active", position, heading: 45, + pose: bodyPose(position), battery: 2.9, led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, drivable: true, nav: "drive", waypoints: [], @@ -48,15 +95,15 @@ interface HarnessProps { selection?: Set; from?: Camera; planned?: { ids: string[]; waypoints: LH2Position[]; led: string | null }[]; - robotShapes?: boolean; + robotDrawing?: RobotDrawing; } const Harness: React.FC = ({ bots, selection = new Set(), - from = SITE_CAMERA, + from = FRAME_CAMERA, planned = [], - robotShapes, + robotDrawing, }) => { const [cam, setCam] = useState(from); return ( @@ -75,7 +122,7 @@ const Harness: React.FC = ({ trails: false, crashedOnly: false, }} - robotShapes={robotShapes} + robotDrawing={robotDrawing} plannedMissions={planned} cam={cam} setCam={setCam} @@ -148,9 +195,9 @@ describe("what is drawn around a robot", () => { const fleet = () => [bot("a", { x: 500, y: 500 })]; it("hugs the selected robot's footprint with the ring, at any zoom", () => { - for (const cam of [SITE_CAMERA, near]) { + for (const cam of [FRAME_CAMERA, near]) { render(); - const footprint = botFootprintPx(pxPerMm("x", VIEWPORT, GEOM, cam)); + const footprint = botFootprintPx(pxPerMm("x", VIEWPORT, GEOM, cam), V3_SPAN_MM); const ring = screen.getByTestId("selection-a"); expect(parseFloat(ring.style.width)).toBeCloseTo(footprint + 6, 3); expect(parseFloat(ring.style.height)).toBeCloseTo(footprint + 6, 3); @@ -163,14 +210,12 @@ describe("what is drawn around a robot", () => { expect(screen.getByTestId("selection-a").style.transform).toContain("rotate(225deg)"); }); - it("draws the battery bar and drive dot on a board, not on a dot", () => { + it("draws the battery bar on a board, not on a dot", () => { render(); expect(screen.getByTestId("battery-a")).toBeInTheDocument(); - expect(screen.getByTestId("drive-a")).toBeInTheDocument(); cleanup(); render(); expect(screen.queryByTestId("battery-a")).not.toBeInTheDocument(); - expect(screen.queryByTestId("drive-a")).not.toBeInTheDocument(); }); it("keeps the reset badge on a dot, where a crash still has to be found", () => { @@ -178,35 +223,214 @@ describe("what is drawn around a robot", () => { expect(screen.getByTitle("Last reset: hard fault")).toBeInTheDocument(); }); - it("sizes a waypoint diamond to the robot it belongs to", () => { + it("sizes a waypoint diamond to the robot's body, between a floor and a cap", () => { const fleet = [bot("a", { x: 500, y: 500 }, { waypoints: [{ x: 600, y: 600 }] })]; + const width = () => parseFloat(screen.getByTestId("waypoint-a-0").style.width); render(); - expect(parseFloat(screen.getByTestId("waypoint-a-0").style.width)).toBe(5); + expect(width()).toBe(WAYPOINT_MIN_PX); + cleanup(); + // A scale where the body is between the two bounds. + const mid: Camera = { scale: 5, tx: 0, ty: 0 }; + render(); + const body = botFootprintPx(pxPerMm("x", VIEWPORT, GEOM, mid), V3_SPAN_MM); + expect(width()).toBeCloseTo(body * WAYPOINT_OF_BODY, 3); cleanup(); render(); - const footprint = botFootprintPx(pxPerMm("x", VIEWPORT, GEOM, near)); - expect(parseFloat(screen.getByTestId("waypoint-a-0").style.width)).toBeCloseTo(footprint * 0.35, 3); + expect(width()).toBe(WAYPOINT_MAX_PX); + }); + + it("sizes a waypoint from the body, not the ring, for a robot drawn as its sensor", () => { + const fleet = [bot("a", { x: 500, y: 500 }, { waypoints: [{ x: 600, y: 600 }] })]; + const width = () => parseFloat(screen.getByTestId("waypoint-a-0").style.width); + const mid: Camera = { scale: 5, tx: 0, ty: 0 }; + render(); + const asBody = width(); + cleanup(); + render( + , + ); + expect(width()).toBeCloseTo(asBody, 3); + expect(width()).toBeLessThan(WAYPOINT_MAX_PX); }); }); -describe("the robot shapes toggle", () => { +describe("what the map draws a robot from", () => { const glyph = (id: string) => screen.getByTestId(`glyph-${id}`); + const near: Camera = { scale: 20, tx: 0, ty: 0 }; + + it("is the body the controller shipped, with its sensor mark", () => { + render(); + const svg = glyph("a").querySelector("svg")!; + expect(svg.querySelectorAll('[data-layer="board"]')).toHaveLength(1); + expect(svg.querySelectorAll("line")).toHaveLength(0); + expect(svg.querySelectorAll('[data-layer="sensor-mark"]')).toHaveLength(1); + }); + + const headingless = (id: string, at: LH2Position) => + bot(id, at, { + heading: null, + pose: { ...bodyPose(at, 0), heading_source: "none" }, + }); + const shape = (id: string) => glyph(id).getAttribute("data-shape"); + const layer = (id: string, name: string) => + glyph(id).querySelector(`[data-layer="${name}"]`); + const noFootprint: RobotDrawing = { mode: "body", footprint: false }; - it("draws a DotBot as the robot by default, heading or not", () => { - render(); - expect(glyph("a").querySelector("circle")).toBeNull(); + it("is the sensor point alone for a bot whose heading the robot never reported", () => { + render( + , + ); + const svg = glyph("a").querySelector("svg")!; + expect(svg.querySelectorAll("polygon")).toHaveLength(0); + expect(svg.querySelectorAll("line")).toHaveLength(0); + expect(layer("a", "sensor")).not.toBeNull(); + expect(layer("a", "reach")).toBeNull(); + }); + + it("is the sensor point for a bot with no body at all", () => { + render(); + expect(glyph("a").querySelector("polygon")).toBeNull(); + expect(shape("a")).toBe("sensor"); + }); + + it("marks the LED colour the controller holds, hollow when it holds none", () => { + render( + , + ); + expect(layer("a", "sensor-mark")!.getAttribute("data-led")).toBe("rgb(255,0,200)"); + expect(layer("b", "sensor-mark")!.getAttribute("data-led")).toBe("unknown"); + expect(screen.queryByTestId("drive-a")).not.toBeInTheDocument(); }); - it("draws every bot as a plain mark when turned off", () => { + it("falls back to the sensor point per robot, not per fleet", () => { render( , + ); + expect(shape("a")).toBe("board"); + expect(shape("b")).toBe("sensor"); + }); + + it("is the sensor point for every bot in Sensor mode", () => { + render( + , ); for (const id of ["a", "b"]) { - expect(glyph(id).querySelector("circle")).not.toBeNull(); - expect(glyph(id).querySelector("svg")!.style.transform).toBe(""); + expect(glyph(id).querySelector("polygon")).toBeNull(); + expect(shape(id)).toBe("sensor"); } }); + + it("keeps the battery bar on a sensor point", () => { + render(); + expect(screen.getByTestId("battery-a")).toBeInTheDocument(); + }); +}); + +describe("the fallback for a board that cannot be drawn", () => { + const glyph = (id: string) => screen.getByTestId(`glyph-${id}`); + const shape = (id: string) => glyph(id).getAttribute("data-shape"); + const near: Camera = { scale: 20, tx: 0, ty: 0 }; + const crowd = () => + Array.from({ length: 201 }, (_, i) => bot(`c${i}`, { x: 100 + i * 5, y: 500 })); + + it("is the disc with a heading bar where the board is too small", () => { + render(); + expect(shape("a")).toBe("mark"); + expect(glyph("a").querySelector("rect")).toBeNull(); + expect(glyph("a").querySelector('circle[data-layer="mark"]')).not.toBeNull(); + expect(glyph("a").querySelector('[data-layer="heading"]')).not.toBeNull(); + }); + + it("is the real-size disc with its heading where a crowd hides a readable board", () => { + render(); + expect(shape("c0")).toBe("disc"); + const disc = glyph("c0").querySelector('[data-layer="disc"]')!; + const perMm = pxPerMm("x", VIEWPORT, GEOM, near); + expect(parseFloat(disc.getAttribute("r")!)).toBeCloseTo((95 * perMm) / 2, 3); + expect(glyph("c0").querySelector('[data-layer="heading"]')).not.toBeNull(); + expect(glyph("c0").querySelector('[data-layer="sensor-mark"]')).not.toBeNull(); + }); + + it("is still the mark in a crowd where the board would be too small anyway", () => { + render(); + expect(shape("c0")).toBe("mark"); + }); +}); + +describe("the possible footprint", () => { + const glyph = (id: string) => screen.getByTestId(`glyph-${id}`); + const layer = (id: string, name: string) => + glyph(id).querySelector(`[data-layer="${name}"]`); + const near: Camera = { scale: 20, tx: 0, ty: 0 }; + const headingless = (id: string, at: LH2Position) => + bot(id, at, { + heading: null, + pose: { ...bodyPose(at, 0), heading_source: "none" }, + }); + + it("rings a robot drawn as its sensor with the reach and the core the host sent", () => { + render(); + const perMm = pxPerMm("x", VIEWPORT, GEOM, near); + expect(parseFloat(layer("a", "reach")!.getAttribute("r")!)).toBeCloseTo(89.33 * perMm, 3); + expect(parseFloat(layer("a", "core")!.getAttribute("r")!)).toBeCloseTo(18.5 * perMm, 3); + }); + + it("is not drawn around a robot drawn as its body", () => { + render( + , + ); + expect(layer("a", "reach")).toBeNull(); + expect(layer("b", "reach")).not.toBeNull(); + }); + + it("is drawn around every robot in Sensor mode", () => { + render( + , + ); + expect(layer("a", "reach")).not.toBeNull(); + }); + + it("is hidden where the ring would be too small to read", () => { + // At the whole-site zoom the ring is about 12 px across. + render(); + expect(layer("a", "reach")).toBeNull(); + expect(layer("a", "core")).toBeNull(); + expect(layer("a", "sensor")).not.toBeNull(); + }); + + it("loses its fill in a crowd", () => { + const crowd = Array.from({ length: 201 }, (_, i) => + headingless(`c${i}`, { x: 100 + i * 5, y: 500 }), + ); + render(); + expect(layer("c0", "reach")!.getAttribute("fill")).toBe("none"); + cleanup(); + render(); + expect(layer("a", "reach")!.getAttribute("fill")).not.toBe("none"); + }); }); diff --git a/dotbot/console-web/src/botGlyph.test.tsx b/dotbot/console-web/src/botGlyph.test.tsx index 1b702c94..1eee255c 100644 --- a/dotbot/console-web/src/botGlyph.test.tsx +++ b/dotbot/console-web/src/botGlyph.test.tsx @@ -3,44 +3,154 @@ import { cleanup, render } from "@testing-library/react"; import { afterEach, describe, expect, it } from "vitest"; import { - BOT_FOOTPRINT_MM, - BOT_GLYPH_BOX, - BOT_GLYPH_SPAN, BOT_MIN_PX, BotGlyph, + FOOTPRINT_MIN_PX, + LED_OFF, GLYPH_CROWD_BOTS, GLYPH_DETAIL_PX, + SENSOR_POINT_PX, + botBody, botFootprintPx, - glyphBoxPx, glyphLevel, + ledFill, + robotDraw, } from "./BotGlyph"; +import type { RobotDrawing } from "./robotDrawing"; +import type { BotPose, LH2Position } from "./types"; afterEach(cleanup); +// A v3 body as the controller ships it: the board path, the centre, the nose, +// all in frame millimetres and already rotated. This is a payload, not a +// geometry the console may compute - the point of the fixture is that the +// numbers arrive from outside. +const V3_AT_ORIGIN: LH2Position[] = [ + { x: -43, y: 10.5 }, + { x: -47, y: 10.5 }, + { x: -47, y: -27.5 }, + { x: -28.5, y: -27.5 }, + { x: -28.5, y: -76.5 }, + { x: 28.5, y: -76.5 }, + { x: 28.5, y: -27.5 }, + { x: 47, y: -27.5 }, + { x: 47, y: 10.5 }, + { x: 43, y: 10.5 }, + { x: 42, y: 11.5 }, + { x: 42, y: 18.5 }, + { x: -42, y: 18.5 }, + { x: -42, y: 11.5 }, +]; + +// The two tyres in the same payload frame: the track between their centres, +// one tyre's width across the robot and the wheel's diameter along it. +const V3_WHEELS_AT_ORIGIN: LH2Position[][] = [ + [ + { x: 30.25, y: -31.5 }, + { x: 47.75, y: -31.5 }, + { x: 47.75, y: -75.5 }, + { x: 30.25, y: -75.5 }, + ], + [ + { x: -47.75, y: -31.5 }, + { x: -30.25, y: -31.5 }, + { x: -30.25, y: -75.5 }, + { x: -47.75, y: -75.5 }, + ], +]; + +const SENSOR: LH2Position = { x: 1000, y: 1000 }; + +// The pose of a bot standing on SENSOR facing `heading`. The board turns with +// the robot, which is what the controller sends: the heading is a label on an +// already-rotated path, never an instruction to rotate one. +const pose = (heading = 0, over: Partial = {}): BotPose => { + const theta = (heading * Math.PI) / 180; + const place = (p: LH2Position): LH2Position => ({ + x: SENSOR.x + p.x * Math.cos(theta) - p.y * Math.sin(theta), + y: SENSOR.y + p.x * Math.sin(theta) + p.y * Math.cos(theta), + }); + return { + heading_deg: heading, + heading_source: "travel", + photodiode: SENSOR, + axle: place({ x: 0, y: -53.5 }), + centre: place({ x: 0, y: -29 }), + nose: place({ x: 0, y: 18.5 }), + led: place({ x: 0, y: 5.5 }), + outline: V3_AT_ORIGIN.map(place), + wheels: V3_WHEELS_AT_ORIGIN.map((wheel) => wheel.map(place)), + reach_mm: 89.33, + core_mm: 18.5, + envelope_mm: 95, + ...over, + }; +}; + +describe("the body a pose describes", () => { + it("is the payload's own path, put back on the fix it belongs to", () => { + const body = botBody(pose())!; + expect(body.outline).toEqual(V3_AT_ORIGIN); + expect(body.centre).toEqual({ x: 0, y: -29 }); + expect(body.nose).toEqual({ x: 0, y: 18.5 }); + }); + + it("is the robot's own size, not the box it happens to occupy", () => { + // 95 mm long by 94 wide, whichever way it faces. Measured in the arena + // frame instead, a body turned 45 degrees spans 134 mm and the chrome + // around it would breathe as the robot turned. + for (const heading of [0, 37, 90, 180, -135]) { + const turned = botBody(pose(heading))!; + expect(turned.spanMm).toBeCloseTo(95, 6); + } + }); + + it("is nothing at all without a heading, which is the whole rule", () => { + expect(botBody(pose(0, { heading_source: "none" }))).toBeNull(); + }); + + it("is hung off the pose's photodiode, not the LH2 fix", () => { + // A pose placed 10 mm off SENSOR, the bot's lh2_position: read against + // the fix the board would come out 10 mm askew. + const shift = (p: LH2Position): LH2Position => ({ x: p.x + 10, y: p.y }); + const p = pose(); + const body = botBody({ + ...p, + photodiode: shift(p.photodiode), + centre: shift(p.centre), + nose: shift(p.nose), + outline: p.outline.map(shift), + })!; + expect(body.outline).toEqual(V3_AT_ORIGIN); + expect(body.centre).toEqual({ x: 0, y: -29 }); + expect(body.nose).toEqual({ x: 0, y: 18.5 }); + }); + + it("is nothing without a pose or an outline", () => { + expect(botBody(undefined)).toBeNull(); + expect(botBody(null)).toBeNull(); + expect(botBody(pose(0, { outline: [] }))).toBeNull(); + }); +}); + describe("how big a bot is drawn", () => { it("is its true footprint wherever that is big enough to see", () => { // 1 m across 200 px: a 95 mm robot is 19 px of it. - expect(botFootprintPx(200 / 1000)).toBeCloseTo(19, 6); - expect(botFootprintPx(1000 / 1000)).toBeCloseTo(95, 6); + expect(botFootprintPx(200 / 1000, 95)).toBeCloseTo(19, 6); + expect(botFootprintPx(1000 / 1000, 95)).toBeCloseTo(95, 6); }); it("grows with the camera rather than staying a fixed size", () => { - const near = botFootprintPx(0.6); - const far = botFootprintPx(0.3); - expect(near / far).toBeCloseTo(2, 6); + expect(botFootprintPx(0.6, 95) / botFootprintPx(0.3, 95)).toBeCloseTo(2, 6); }); it("floors at a size that can still be seen and clicked", () => { - expect(botFootprintPx(0.001)).toBe(BOT_MIN_PX); - expect(botFootprintPx(0)).toBe(BOT_MIN_PX); + expect(botFootprintPx(0.001, 95)).toBe(BOT_MIN_PX); + expect(botFootprintPx(0, 95)).toBe(BOT_MIN_PX); }); - it("sizes the glyph box so the drawn robot is that footprint", () => { - const box = glyphBoxPx(BOT_FOOTPRINT_MM); - expect((box * BOT_GLYPH_SPAN) / BOT_GLYPH_BOX).toBeCloseTo( - BOT_FOOTPRINT_MM, - 6, - ); + it("is that floor for a bot with no body, which is a point", () => { + expect(botFootprintPx(1, 0)).toBe(BOT_MIN_PX); }); }); @@ -58,7 +168,7 @@ describe("how much of a bot is drawn", () => { expect(glyphLevel(11, 1)).toBe("dot"); }); - it("drops the board to a square in a crowd, where detail is lost anyway", () => { + it("drops the board to a mark in a crowd, where detail is lost anyway", () => { const many = GLYPH_CROWD_BOTS + 1; expect(glyphLevel(GLYPH_DETAIL_PX, many)).toBe("dot"); expect(glyphLevel(200, many)).toBe("dot"); @@ -73,40 +183,399 @@ describe("how much of a bot is drawn", () => { }); }); -describe("the glyph a level draws", () => { +describe("what a robot is drawn as", () => { + const BODY: RobotDrawing = { mode: "body", footprint: true }; + const SENSOR_MODE: RobotDrawing = { mode: "sensor", footprint: true }; + // Scales at which the 95 mm board is 95 px, and 9.5 px. + const NEAR = 1; + const FAR = 0.1; + + it("is the board for a robot with a heading, big enough and not crowded", () => { + const d = robotDraw(pose(), BODY, NEAR, 1); + expect(d.shape.kind).toBe("board"); + expect(d.turned).toBe(true); + expect(d.battery).toBe(true); + }); + + it("is drawn fainter as an estimate only when built on the travel bearing", () => { + expect(robotDraw(pose(), BODY, NEAR, 1).estimate).toBe(true); + expect(robotDraw(pose(0, { heading_source: "ekf" }), BODY, NEAR, 1).estimate).toBe(false); + }); + + it("is the mark below the size the board reads at", () => { + const d = robotDraw(pose(), BODY, FAR, 1); + expect(d.shape.kind).toBe("mark"); + expect(d.footprintPx).toBeLessThan(GLYPH_DETAIL_PX); + expect(d.battery).toBe(false); + }); + + it("is the envelope disc where a crowd hides a board big enough to read", () => { + const d = robotDraw(pose(), BODY, NEAR, GLYPH_CROWD_BOTS + 1); + expect(d.shape.kind).toBe("disc"); + expect(d.shape.kind === "disc" && d.shape.radiusPx).toBeCloseTo(47.5, 6); + expect(d.centre).toEqual({ x: 0, y: -29 }); + }); + + it("is the mark in a crowd too, once the board is too small anyway", () => { + expect(robotDraw(pose(), BODY, FAR, GLYPH_CROWD_BOTS + 1).shape.kind).toBe("mark"); + }); + + it("is the sensor point for a robot with no heading, whatever the zoom", () => { + for (const scale of [NEAR, FAR]) { + const d = robotDraw(pose(0, { heading_source: "none" }), BODY, scale, 1); + expect(d.shape.kind).toBe("sensor"); + expect(d.centre).toEqual({ x: 0, y: 0 }); + } + }); + + it("is the sensor point for every robot in Sensor mode", () => { + expect(robotDraw(pose(), SENSOR_MODE, NEAR, 1).shape.kind).toBe("sensor"); + }); + + it("rings the sensor point with the host's reach and core", () => { + const { shape, footprintPx } = robotDraw(pose(), SENSOR_MODE, NEAR, 1); + expect(shape).toMatchObject({ kind: "sensor", ringPx: 89.33, corePx: 18.5, crowded: false }); + expect(footprintPx).toBeCloseTo(2 * 89.33, 6); + }); + + it("hides the ring below the size it reads at, and the core with it", () => { + const scale = (FOOTPRINT_MIN_PX - 1) / (2 * 89.33); + const { shape, footprintPx } = robotDraw(pose(), SENSOR_MODE, scale, 1); + expect(shape).toMatchObject({ ringPx: null, corePx: null }); + expect(footprintPx).toBe(SENSOR_POINT_PX); + }); + + it("draws the core only when it is visibly larger than the point", () => { + const scale = SENSOR_POINT_PX / (2 * 18.5); + const { shape } = robotDraw(pose(), SENSOR_MODE, scale, 1); + expect(shape).toMatchObject({ corePx: null }); + expect(shape.kind === "sensor" && shape.ringPx).toBeGreaterThan(0); + }); + + it("draws no footprint with the checkbox off", () => { + const { shape } = robotDraw(pose(), { mode: "sensor", footprint: false }, NEAR, 1); + expect(shape).toMatchObject({ ringPx: null, corePx: null }); + }); + + it("gives a known heading a bar out to the ring, at the heading's angle", () => { + for (const [heading, dx, dy] of [ + [0, 0, 1], + [90, -1, 0], + [-135, Math.SQRT1_2, -Math.SQRT1_2], + ]) { + const { shape } = robotDraw(pose(heading), SENSOR_MODE, NEAR, 1); + const bar = shape.kind === "sensor" ? shape.bar : null; + expect(bar).not.toBeNull(); + expect(bar!.dir.x).toBeCloseTo(dx, 6); + expect(bar!.dir.y).toBeCloseTo(dy, 6); + expect(bar!.lengthPx).toBeCloseTo(89.33, 6); + } + }); + + it("gives no bar to a robot whose heading is unknown, in either mode", () => { + for (const mode of [BODY, SENSOR_MODE]) { + const { shape } = robotDraw(pose(0, { heading_source: "none" }), mode, NEAR, 1); + expect(shape).toMatchObject({ kind: "sensor", bar: null }); + } + expect(robotDraw(null, SENSOR_MODE, NEAR, 1).shape).toMatchObject({ bar: null }); + }); + + it("keeps the bar past the point when the ring is too small to draw", () => { + const { shape } = robotDraw(pose(), SENSOR_MODE, 0.01, 1); + expect(shape).toMatchObject({ ringPx: null }); + expect(shape.kind === "sensor" && shape.bar!.lengthPx).toBe(SENSOR_POINT_PX); + }); + + it("marks the ring as crowded past the crowd size", () => { + const { shape } = robotDraw(pose(), SENSOR_MODE, NEAR, GLYPH_CROWD_BOTS + 1); + expect(shape).toMatchObject({ crowded: true }); + }); +}); + +describe("the glyph a shape draws", () => { const svg = (props: Parameters[0]) => render().container.querySelector("svg")!; - it("draws the board outline and its tyres at full detail", () => { - const el = svg({ color: "red", heading: 90, level: "detail" }); - expect(el.querySelectorAll("path")).toHaveLength(1); - expect(el.querySelectorAll("rect").length).toBeGreaterThan(2); + const body = botBody(pose())!; + const board = { kind: "board", body } as const; + + it("draws the layers of one robot at full detail", () => { + const el = svg({ state: "red", led: null, shape: board, pxPerMm: 1, footprintPx: 95 }); + // The tyres, the board over them and the sensor mark: no heading line, + // since the board's own shape carries it. + expect(el.querySelectorAll('[data-layer="wheel"]')).toHaveLength(2); + expect(el.querySelectorAll('[data-layer="board"]')).toHaveLength(1); + expect(el.querySelectorAll("line")).toHaveLength(0); + expect(el.querySelectorAll('[data-layer="sensor-mark"]')).toHaveLength(1); + }); + + it("draws each tyre where the pose puts it, at the size it was sent", () => { + const el = svg({ state: "red", led: null, shape: board, pxPerMm: 1, footprintPx: 95 }); + const wheels = [...el.querySelectorAll('[data-layer="wheel"]')].map((w) => + w.getAttribute("points"), + ); + expect(wheels[0]).toBe("30.25,-31.5 47.75,-31.5 47.75,-75.5 30.25,-75.5"); + expect(wheels[1]).toBe("-47.75,-31.5 -30.25,-31.5 -30.25,-75.5 -47.75,-75.5"); + }); + + it("puts the board where the pose puts it, not on the fix", () => { + const el = svg({ state: "red", led: null, shape: board, pxPerMm: 1, footprintPx: 95 }); + const points = el + .querySelector('[data-layer="board"]')! + .getAttribute("points")!; + // The rear edge of a bot facing heading 0 is 76.5 mm back from its fix. + expect(points).toContain("-76.5"); + // The photodiode is the origin, so the mark needs no placing at all. + for (const dot of el.querySelectorAll('[data-layer="sensor-mark"] circle')) { + expect(dot.getAttribute("cx")).toBeNull(); + expect(dot.getAttribute("cy")).toBeNull(); + } + }); + + it("draws the small fallback as a rimless disc at the board's centre", () => { + const el = svg({ state: "red", led: null, shape: { kind: "mark", body }, pxPerMm: 0.1, footprintPx: 9.5 }); + expect(el.querySelectorAll("polygon")).toHaveLength(0); + expect(el.querySelector("rect")).toBeNull(); + const mark = el.querySelector('[data-layer="mark"]')!; + expect(mark.tagName).toBe("circle"); + expect(parseFloat(mark.getAttribute("r")!)).toBeCloseTo(9.5 / 2, 6); + // Centred 29 mm behind the fix, so a mark stands where the robot does. + expect(parseFloat(mark.getAttribute("cy")!)).toBeCloseTo(-2.9, 6); + // No rim: the white rim is what marks out a sensor point. + expect(mark.getAttribute("stroke")).toBeNull(); + }); + + it("turns the small fallback's heading bar with the robot", () => { + for (const [heading, dx, dy] of [ + [0, 0, 1], + [90, -1, 0], + [-90, 1, 0], + [180, 0, -1], + ]) { + const turned = botBody(pose(heading))!; + const el = svg({ state: "red", led: null, shape: { kind: "mark", body: turned }, pxPerMm: 0.1, footprintPx: 12 }); + const bar = el.querySelector('[data-layer="heading"]')!; + const vx = parseFloat(bar.getAttribute("x2")!) - parseFloat(bar.getAttribute("x1")!); + const vy = parseFloat(bar.getAttribute("y2")!) - parseFloat(bar.getAttribute("y1")!); + const n = Math.hypot(vx, vy); + expect(vx / n).toBeCloseTo(dx, 6); + expect(vy / n).toBeCloseTo(dy, 6); + // From the disc's centre to its edge. + expect(n).toBeGreaterThan(4); + expect(n).toBeLessThanOrEqual(6); + cleanup(); + } + }); + + it("draws the disc about the board's centre, with its heading and sensor mark", () => { + const el = svg({ state: "red", led: null, shape: { kind: "disc", body, radiusPx: 20 }, pxPerMm: 0.4, footprintPx: 40 }); + const disc = el.querySelector('[data-layer="disc"]')!; + expect(parseFloat(disc.getAttribute("cy")!)).toBeCloseTo(-29 * 0.4, 6); + expect(el.querySelector('[data-layer="heading"]')).not.toBeNull(); + expect(el.querySelector('[data-layer="sensor-mark"]')).not.toBeNull(); + }); + + it("draws the sensor point at a fixed size", () => { + for (const pxPerMm of [0.05, 2]) { + const el = svg({ + state: "red", led: null, + shape: { kind: "sensor", ringPx: null, corePx: null, crowded: false, bar: null }, + pxPerMm, + footprintPx: BOT_MIN_PX, + }); + expect(el.querySelectorAll("polygon")).toHaveLength(0); + const point = el.querySelector('[data-layer="sensor"]')!; + expect(2 * parseFloat(point.getAttribute("r")!)).toBe(SENSOR_POINT_PX); + cleanup(); + } + }); + + it("draws the ring dashed and the core solid around the point", () => { + const el = svg({ + state: "red", led: null, + shape: { kind: "sensor", ringPx: 60, corePx: 12, crowded: false, bar: null }, + pxPerMm: 1, + footprintPx: 120, + }); + expect(el.querySelector('[data-layer="reach"]')!.getAttribute("stroke-dasharray")).toBeTruthy(); + expect(el.querySelector('[data-layer="core"]')!.getAttribute("stroke-dasharray")).toBeNull(); + expect(el.querySelector('[data-layer="sensor"]')).not.toBeNull(); }); - it("draws a square with no front where a front would not read", () => { - const el = svg({ color: "red", heading: 90, level: "dot" }); - expect(el.querySelectorAll("path")).toHaveLength(0); - expect(el.querySelectorAll("rect")).toHaveLength(1); - expect(el.style.transform).toBe(""); + it("draws a known heading as a white bar from the point, under it", () => { + const el = svg({ + state: "red", led: null, + shape: { + kind: "sensor", + ringPx: 60, + corePx: null, + crowded: false, + bar: { dir: { x: -1, y: 0 }, lengthPx: 60 }, + }, + pxPerMm: 1, + footprintPx: 120, + }); + const bar = el.querySelector('[data-layer="sensor-heading"]')!; + expect(bar.getAttribute("x1")).toBe("0"); + expect(parseFloat(bar.getAttribute("x2")!)).toBeCloseTo(-60, 6); + expect(parseFloat(bar.getAttribute("y2")!)).toBeCloseTo(0, 6); + const all = [...el.querySelectorAll("[data-layer]")].map((n) => n.getAttribute("data-layer")); + expect(all.indexOf("sensor-heading")).toBeLessThan(all.indexOf("sensor")); }); - it("keeps the headingless body at every level for a bot that is not a DotBot", () => { - (["detail", "dot"] as const).forEach((level) => { - const el = svg({ color: "red", heading: null, level }); - expect(el.querySelectorAll("circle")).toHaveLength(1); + it("draws no bar on a point whose heading is unknown", () => { + const el = svg({ + state: "red", led: null, + shape: { kind: "sensor", ringPx: 60, corePx: null, crowded: false, bar: null }, + pxPerMm: 1, + footprintPx: 120, }); + expect(el.querySelectorAll("line")).toHaveLength(0); }); - it("draws a headingless DotBot as the board, nose-up", () => { - const el = svg({ color: "red", heading: null, dotBot: true, level: "detail" }); - expect(el.querySelectorAll("circle")).toHaveLength(0); - expect(el.querySelectorAll("path")).toHaveLength(1); - expect(el.style.transform).toBe(""); + it("lays the dashed ring over a solid casing of the same radius", () => { + const el = svg({ + state: "red", led: null, + shape: { kind: "sensor", ringPx: 60, corePx: null, crowded: false, bar: null }, + pxPerMm: 1, + footprintPx: 120, + }); + const circles = [...el.querySelectorAll("circle")]; + const casing = el.querySelector('[data-layer="reach-casing"]')!; + const ring = el.querySelector('[data-layer="reach"]')!; + expect(circles.indexOf(casing as SVGCircleElement)).toBeLessThan( + circles.indexOf(ring as SVGCircleElement), + ); + expect(casing.getAttribute("r")).toBe(ring.getAttribute("r")); + expect(casing.getAttribute("stroke")).toBe("var(--footprint-casing)"); + expect(casing.getAttribute("stroke-dasharray")).toBeNull(); + expect(parseFloat(casing.getAttribute("stroke-width")!)).toBeGreaterThan( + parseFloat(ring.getAttribute("stroke-width")!), + ); }); +}); + +describe("the colour rule, the same at every level", () => { + const STATE = "#22c55e"; + const RED = { red: 255, green: 0, blue: 0 }; + const svg = (props: Parameters[0]) => + render().container.querySelector("svg")!; + const body = botBody(pose())!; + const sensor = (bar: boolean) => + ({ + kind: "sensor", + ringPx: 60, + corePx: null, + crowded: false, + bar: bar ? { dir: { x: 0, y: 1 }, lengthPx: 60 } : null, + }) as const; + // Every level, as the map draws it: the board, the small circle at 11 and + // 16 px, the crowd disc, and the sensor point with and without a heading. + const levels = [ + ["board", { shape: { kind: "board", body } as const, pxPerMm: 1, footprintPx: 95 }], + ["circle at 11 px", { shape: { kind: "mark", body } as const, pxPerMm: 11 / 95, footprintPx: 11 }], + ["circle at 16 px", { shape: { kind: "mark", body } as const, pxPerMm: 16 / 95, footprintPx: 15.9 }], + ["disc", { shape: { kind: "disc", body, radiusPx: 20 } as const, pxPerMm: 0.4, footprintPx: 40 }], + ["sensor point", { shape: sensor(false), pxPerMm: 1, footprintPx: 120 }], + ["sensor point with a heading", { shape: sensor(true), pxPerMm: 1, footprintPx: 120 }], + ] as const; + const FILL_LAYER = { board: "board", mark: "mark", disc: "disc", sensor: "sensor" } as const; + + for (const [name, level] of levels) { + it(`fills the ${name} in the state colour`, () => { + const el = svg({ state: STATE, led: RED, ...level }); + const fill = el.querySelector(`[data-layer="${FILL_LAYER[level.shape.kind]}"]`)!; + expect(fill.getAttribute("fill")).toBe(STATE); + }); + + it(`carries exactly one LED mark on the ${name}, at the photodiode`, () => { + const el = svg({ state: STATE, led: RED, ...level }); + const marks = el.querySelectorAll('[data-layer="sensor-mark"]'); + expect(marks).toHaveLength(1); + expect(marks[0].getAttribute("data-led")).toBe("rgb(255,0,0)"); + for (const c of marks[0].querySelectorAll("circle")) { + expect(c.getAttribute("cx")).toBeNull(); + expect(c.getAttribute("cy")).toBeNull(); + } + // A white rim inside a dark one: it reads on a fill of its own colour. + const [halo, dot] = [...marks[0].querySelectorAll("circle")]; + expect(dot.getAttribute("stroke")).toBe("rgba(255,255,255,.95)"); + expect(parseFloat(halo.getAttribute("r")!)).toBeGreaterThan(parseFloat(dot.getAttribute("r")!)); + }); + + it(`draws the ${name}'s mark hollow when the LED colour is unknown`, () => { + const el = svg({ state: STATE, led: null, ...level }); + const mark = el.querySelector('[data-layer="sensor-mark"]')!; + expect(mark.getAttribute("data-led")).toBe("unknown"); + for (const c of mark.querySelectorAll("circle")) expect(c.getAttribute("fill")).toBe("none"); + }); + + it(`draws the ${name}'s mark near-black for an LED commanded off`, () => { + const el = svg({ state: STATE, led: { red: 0, green: 0, blue: 0 }, ...level }); + expect(el.querySelector('[data-layer="sensor-mark"]')!.getAttribute("data-led")).toBe(LED_OFF); + }); + + it(`draws no centre dot on the ${name}`, () => { + const el = svg({ state: STATE, led: RED, ...level }); + expect(el.querySelector('[data-layer="photodiode"]')).toBeNull(); + }); + } + + it("carries a known heading as a white bar, except on the board, whose shape does", () => { + for (const [name, level] of levels) { + const el = svg({ state: STATE, led: RED, ...level }); + const bars = [...el.querySelectorAll("line")].filter( + (l) => !l.getAttribute("data-layer")!.endsWith("casing"), + ); + const expected = name === "board" || name === "sensor point" ? 0 : 1; + expect(bars, name).toHaveLength(expected); + for (const bar of bars) expect(bar.getAttribute("stroke")).toBe("rgba(255,255,255,.95)"); + cleanup(); + } + }); + + it("puts the circle's and the disc's mark on the bar", () => { + for (const [, level] of levels.filter(([, l]) => l.shape.kind === "mark" || l.shape.kind === "disc")) { + const el = svg({ state: STATE, led: RED, ...level }); + const bar = el.querySelector('[data-layer="heading"]')!; + const [x1, y1, x2, y2] = ["x1", "y1", "x2", "y2"].map((k) => parseFloat(bar.getAttribute(k)!)); + // The photodiode is the origin: it lies on the segment from the centre. + const cross = x1 * (y2 - y1) - y1 * (x2 - x1); + const along = -(x1 * (x2 - x1) + y1 * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2); + expect(Math.abs(cross)).toBeLessThan(1e-6); + expect(along).toBeGreaterThan(0); + expect(along).toBeLessThan(1); + cleanup(); + } + }); + + it("keeps the small circle's mark small, so the fill and the bar still show", () => { + for (const footprintPx of [11, 16]) { + const el = svg({ state: STATE, led: RED, shape: { kind: "mark", body }, pxPerMm: footprintPx / 95, footprintPx }); + const halo = el.querySelector('[data-layer="sensor-mark"] circle')!; + expect(2 * parseFloat(halo.getAttribute("r")!)).toBeLessThan(footprintPx * 0.4); + cleanup(); + } + }); + + it("rims the sensor point in the state colour around the LED mark", () => { + const el = svg({ state: STATE, led: RED, shape: sensor(false), pxPerMm: 1, footprintPx: 120 }); + const point = el.querySelector('[data-layer="sensor"]')!; + const halo = el.querySelector('[data-layer="sensor-mark"] circle')!; + expect(parseFloat(point.getAttribute("r")!)).toBeGreaterThan(parseFloat(halo.getAttribute("r")!) + 1); + }); + + it("colours the ring and the core by state, not by LED", () => { + const el = svg({ state: STATE, led: RED, shape: { ...sensor(false), corePx: 20 }, pxPerMm: 1, footprintPx: 120 }); + expect(el.querySelector('[data-layer="reach"]')!.getAttribute("stroke")).toBe(STATE); + expect(el.querySelector('[data-layer="core"]')!.getAttribute("fill")).toBe(STATE); + }); +}); - it("draws a headingless DotBot as the square where a front would not read", () => { - const el = svg({ color: "red", heading: null, dotBot: true, level: "dot" }); - expect(el.querySelectorAll("circle")).toHaveLength(0); - expect(el.querySelectorAll("rect")).toHaveLength(1); +describe("the LED colour a mark is filled with", () => { + it("is the commanded colour, near-black when off, and nothing when unknown", () => { + expect(ledFill({ red: 1, green: 2, blue: 3 })).toBe("rgb(1,2,3)"); + expect(ledFill({ red: 0, green: 0, blue: 0 })).toBe(LED_OFF); + expect(ledFill(null)).toBeNull(); }); }); diff --git a/dotbot/console-web/src/calibrationTab.test.tsx b/dotbot/console-web/src/calibrationTab.test.tsx index 24b50e58..358a1d31 100644 --- a/dotbot/console-web/src/calibrationTab.test.tsx +++ b/dotbot/console-web/src/calibrationTab.test.tsx @@ -7,6 +7,7 @@ import type { CalibrationSession, Site } from "./types"; // The fleet hook is the one seam calibration mode turns on: the controller // owns the session, so App only ever sees it appear and disappear. let session: CalibrationSession | null = null; +const VIEWPORT = { x: -2000, y: -2000, w: 6000, h: 8000 }; const site: Site = { name: "c405-arena", anchor: "the arena's top-left corner", @@ -25,7 +26,7 @@ vi.mock("./useFleet", () => ({ setSession: (next: CalibrationSession | null) => { session = next; }, - viewport: { x: -2000, y: -2000, w: 6000, h: 8000 }, + viewport: VIEWPORT, wsUp: true, }), })); @@ -62,6 +63,17 @@ vi.mock("./api", () => ({ import { abandonCalibration, startCalibration } from "./api"; import { App } from "./App"; +import { sessionRect } from "./calibration"; +import { loadPanels } from "./panels"; +import { saveSavedViews, withView } from "./savedView"; +import { + Camera, + cameraForArea, + centreOfView, + padArea, + viewGeom, + zoomMax, +} from "./zoom"; const SESSION: CalibrationSession = { at: "arena:corners", @@ -257,3 +269,81 @@ describe("the step card following the robot's button", () => { expect(screen.queryByText("Capture")).not.toBeInTheDocument(); }); }); + +describe("the camera across a session started with the right pane collapsed", () => { + // The map with the right pane collapsed, then with it open. + const WIDE = { w: 760, h: 860 }; + const NARROW = { w: 488, h: 860 }; + const rect = (c: { w: number; h: number }) => + ({ width: c.w, height: c.h, top: 0, left: 0, right: c.w, bottom: c.h, x: 0, y: 0, toJSON: () => ({}) }) as DOMRect; + + let rectSpy: ReturnType; + const observers: (() => void)[] = []; + const Real = globalThis.ResizeObserver; + beforeEach(() => { + rectSpy = vi.spyOn(HTMLElement.prototype, "getBoundingClientRect").mockReturnValue(rect(WIDE)); + globalThis.ResizeObserver = class { + constructor(cb: () => void) { + observers.push(cb); + } + observe() {} + unobserve() {} + disconnect() {} + } as unknown as typeof ResizeObserver; + }); + afterEach(() => { + rectSpy.mockRestore(); + globalThis.ResizeObserver = Real; + observers.length = 0; + }); + + const resize = (c: { w: number; h: number }) => { + rectSpy.mockReturnValue(rect(c)); + act(() => observers.forEach((cb) => cb())); + }; + const camOf = (): Camera => { + const t = screen.getByTestId("camera-layer").style.transform; + const [, tx, ty, scale] = + /translate\(([-\d.e+]+)px, ([-\d.e+]+)px\) scale\(([-\d.e+]+)\)/.exec(t) ?? []; + return { scale: Number(scale), tx: Number(tx), ty: Number(ty) }; + }; + const paneOpen = () => screen.queryByLabelText("Collapse the right panel") !== null; + + it("fits the session to the opened canvas and gives the view and the pane back on Done", () => { + window.localStorage.setItem("dotbot.console.panels", JSON.stringify({ left: false, right: true })); + saveSavedViews(withView({}, site.name, { x: 1200, y: 3100, pxPerMm: 0.4 })); + const { rerender } = render(); + const wide = viewGeom(WIDE.w, WIDE.h, VIEWPORT); + const before = centreOfView(camOf(), VIEWPORT, wide); + + session = SESSION; + act(() => { + rerender(); + }); + expect(paneOpen()).toBe(true); + expect(loadPanels().right).toBe(true); + resize(NARROW); + const narrow = viewGeom(NARROW.w, NARROW.h, VIEWPORT); + const fit = cameraForArea( + padArea(sessionRect(SESSION)!), + VIEWPORT, + narrow, + zoomMax(site, VIEWPORT, narrow), + ); + expect(camOf().scale).toBeCloseTo(fit.scale, 6); + expect(camOf().tx).toBeCloseTo(fit.tx, 4); + expect(camOf().ty).toBeCloseTo(fit.ty, 4); + + session = null; + act(() => { + rerender(); + }); + expect(paneOpen()).toBe(false); + expect(loadPanels().right).toBe(true); + resize(WIDE); + const after = centreOfView(camOf(), VIEWPORT, wide); + expect(after.pxPerMm).toBeCloseTo(before.pxPerMm, 9); + expect(after.x).toBeCloseTo(before.x, 4); + expect(after.y).toBeCloseTo(before.y, 4); + }); +}); diff --git a/dotbot/console-web/src/cameraLayer.test.tsx b/dotbot/console-web/src/cameraLayer.test.tsx index 35738daa..9b26c89c 100644 --- a/dotbot/console-web/src/cameraLayer.test.tsx +++ b/dotbot/console-web/src/cameraLayer.test.tsx @@ -125,7 +125,7 @@ const bot = (id: string, position: LH2Position): UnifiedBot => ({ led: null, deviceType: "DotBot", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], @@ -780,6 +780,22 @@ const OUTLINE: number[][] = [ [1471.2, 507.3], ]; +// The two tyres of the same robot, as the detector publishes them. +const WHEELS: number[][][] = [ + [ + [1478.9, 559.7], + [1496.4, 559.7], + [1496.4, 603.7], + [1478.9, 603.7], + ], + [ + [1531.0, 559.7], + [1548.5, 559.7], + [1548.5, 603.7], + [1531.0, 603.7], + ], +]; + const detection = ( status: CameraDetection["status"], withPose = status !== "none", @@ -797,6 +813,7 @@ const detection = ( photodiode_mm: [1540.2, 511.7], nose_mm: [1551.0, 526.2], outline_mm: OUTLINE, + wheels_mm: WHEELS, heading_deg: -37.5, heading_atan2_deg: 52.5, green_flare: 0.83, @@ -840,6 +857,21 @@ describe("the detection on the map", () => { expect(outline.getAttribute("stroke")).toBe("var(--accent)"); expect(outline.getAttribute("stroke-dasharray")).toBeNull(); + // The tyres, under the board line, so the camera robot reads like the + // glyph the lighthouse draws beside it. + WHEELS.forEach((wheel, i) => { + const tyre = within(map).getByTestId( + `camera-detection-wheel-dev-corner-${i}`, + ); + expect(tyre.getAttribute("points")).toBe( + polygonPoints(wheel, DEV_CORNER), + ); + expect(tyre.getAttribute("stroke")).toBe("var(--accent)"); + expect(tyre.compareDocumentPosition(outline)).toBe( + Node.DOCUMENT_POSITION_FOLLOWING, + ); + }); + const nose = within(map).getByTestId("camera-detection-nose-dev-corner"); expect(nose.getAttribute("x1")).toBe(String(1523.4 - DEV_CORNER.x)); expect(nose.getAttribute("y1")).toBe(String(488.1 - DEV_CORNER.y)); @@ -855,6 +887,19 @@ describe("the detection on the map", () => { expect(layer.contains(group)).toBe(true); }); + it("draws the board alone for a pose that carries no tyres", () => { + const found = detection("found"); + const older = { ...found, pose: { ...found.pose!, wheels_mm: undefined } }; + render(); + const map = screen.getByTestId("map"); + expect( + within(map).queryByTestId("camera-detection-wheel-dev-corner-0"), + ).toBeNull(); + expect( + within(map).getByTestId("camera-detection-outline-dev-corner"), + ).toBeTruthy(); + }); + it("dashes a pose the estimator would not vouch for", () => { render( , diff --git a/dotbot/console-web/src/frame.test.ts b/dotbot/console-web/src/frame.test.ts index 6aaebbb4..39350873 100644 --- a/dotbot/console-web/src/frame.test.ts +++ b/dotbot/console-web/src/frame.test.ts @@ -1,10 +1,12 @@ import { describe, expect, it } from "vitest"; import { - VIEWPORT_MARGIN_MM, + PAN_MARGIN_MM, + SITE_VIEW_MARGIN_MIN_MM, areaToFraction, fractionToArea, headingToGlyphRotation, + siteView, siteViewport, } from "./frame"; import type { Area, Site } from "./types"; @@ -50,16 +52,16 @@ describe("the site frame", () => { }); }); -describe("the default viewport", () => { - const site = (extent: [number, number] | null): Site => ({ - name: "c405-arena", - anchor: "the arena top-left corner", - extent_mm: extent, - areas: [ARENA, ANNEX], - }); +const site = (extent: [number, number] | null): Site => ({ + name: "c405-arena", + anchor: "the arena top-left corner", + extent_mm: extent, + areas: [ARENA, ANNEX], +}); - it("surrounds the site extent by the margin on every side", () => { - expect(VIEWPORT_MARGIN_MM).toBe(2000); +describe("the drawn viewport", () => { + it("runs 2 m past the site extent on every side", () => { + expect(PAN_MARGIN_MM).toBe(2000); expect(siteViewport(site([2000, 4000]), ARENA)).toMatchObject({ x: -2000, y: -2000, @@ -68,10 +70,14 @@ describe("the default viewport", () => { }); }); - it("keeps the site's zero at the same fraction of the box on both axes", () => { + it("puts the site at the same place in the box on both axes", () => { const vp = siteViewport(site([2000, 4000]), ARENA); - expect(areaToFraction({ x: 0, y: 0 }, vp)).toEqual({ fx: 1 / 3, fy: 0.25 }); - expect(areaToFraction({ x: 2000, y: 4000 }, vp)).toEqual({ fx: 2 / 3, fy: 0.75 }); + const zero = areaToFraction({ x: 0, y: 0 }, vp); + const far = areaToFraction({ x: 2000, y: 4000 }, vp); + expect(zero.fx).toBeCloseTo(2 / 6, 12); + expect(zero.fy).toBeCloseTo(2 / 8, 12); + expect(far.fx).toBeCloseTo(4 / 6, 12); + expect(far.fy).toBeCloseTo(6 / 8, 12); }); it("falls back when the site has no measured extent", () => { @@ -92,3 +98,31 @@ describe("the default viewport", () => { }); }); }); + +describe("the site view", () => { + it("surrounds the site extent by a tenth of its longer side on every side", () => { + expect(siteView(siteViewport(site([2000, 4000]), ARENA))).toMatchObject({ + x: -400, + y: -400, + w: 2800, + h: 4800, + }); + }); + + it("keeps a small site's margin at the floor", () => { + expect(siteView(siteViewport(site([1000, 1000]), ARENA))).toMatchObject({ + x: -SITE_VIEW_MARGIN_MIN_MM, + y: -SITE_VIEW_MARGIN_MIN_MM, + w: 1000 + 2 * SITE_VIEW_MARGIN_MIN_MM, + }); + }); + + it("frames the fallback when the site has no measured extent", () => { + expect(siteView(siteViewport(null, ARENA))).toMatchObject({ + x: -250, + y: -250, + w: 1500, + h: 1300, + }); + }); +}); diff --git a/dotbot/console-web/src/frame.ts b/dotbot/console-web/src/frame.ts index 098ed518..26109ac3 100644 --- a/dotbot/console-web/src/frame.ts +++ b/dotbot/console-web/src/frame.ts @@ -6,8 +6,26 @@ import type { Area, LH2Position, Site } from "./types"; // console receives is in frame millimetres; the viewport says which part of // the frame is drawn, so the box origin is subtracted before scaling. -/** How much frame lies outside the site on every side of the default view. */ -export const VIEWPORT_MARGIN_MM = 2000; +/** + * How far past the site, on every side, the map draws and can be panned, in + * millimetres. A robot that drives off the site stays reachable until it is + * this far out. + */ +export const PAN_MARGIN_MM = 2000; + +/** + * How much floor the site view leaves outside the site on every side, as a + * fraction of the site's longer side, floored at a minimum in millimetres. It + * is kept modest so the whole-site view is mostly site: a margin as wide as + * the site shrinks every robot on it to a mark. + */ +export const SITE_VIEW_MARGIN_FRAC = 0.1; +export const SITE_VIEW_MARGIN_MIN_MM = 250; + +/** The margin the site view leaves around `a`. */ +export function siteViewMarginMm(a: Area): number { + return Math.max(SITE_VIEW_MARGIN_MIN_MM, SITE_VIEW_MARGIN_FRAC * Math.max(a.w, a.h)); +} /** What a renderer draws when the site has no measured extent. */ export const AREA_FALLBACK: Area = { x: 0, y: 0, w: 2000, h: 2000 }; @@ -30,17 +48,26 @@ export function fractionToArea(fx: number, fy: number, a: Area): LH2Position { } /** A rectangle grown by the same margin on every side. */ -export function withMargin(a: Area, m = VIEWPORT_MARGIN_MM): Area { +export function withMargin(a: Area, m: number): Area { return { x: a.x - m, y: a.y - m, w: a.w + 2 * m, h: a.h + 2 * m, name: a.name }; } /** - * The box the map draws: the whole site with a margin on every side, so a bot - * that drives out of the site is still drawn rather than clipped at the wall. - * A site with no measured extent falls back to a 2 x 2 m square at the origin. + * The box the map draws, and the bounds its pan is held to: the whole site + * with `PAN_MARGIN_MM` on every side. A site with no measured extent falls + * back to `fallback`. */ export function siteViewport(site: Site | null, fallback: Area): Area { - return withMargin(siteExtentArea(site) ?? fallback); + return withMargin(siteExtentArea(site) ?? fallback, PAN_MARGIN_MM); +} + +/** + * What the site view frames inside a viewport built by `siteViewport`: the + * site again, with the modest site-view margin in place of the pan margin. + */ +export function siteView(viewport: Area): Area { + const site = withMargin(viewport, -PAN_MARGIN_MM); + return withMargin(site, siteViewMarginMm(site)); } /** diff --git a/dotbot/console-web/src/goKey.test.tsx b/dotbot/console-web/src/goKey.test.tsx index eb054412..6ea9aa24 100644 --- a/dotbot/console-web/src/goKey.test.tsx +++ b/dotbot/console-web/src/goKey.test.tsx @@ -24,7 +24,7 @@ const bot = (id: string, extra: Partial = {}): UnifiedBot => ({ led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/inspector.test.ts b/dotbot/console-web/src/inspector.test.ts index e557bae4..22af131e 100644 --- a/dotbot/console-web/src/inspector.test.ts +++ b/dotbot/console-web/src/inspector.test.ts @@ -13,7 +13,7 @@ const bot = (over: Partial = {}): UnifiedBot => ({ led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/localization.test.ts b/dotbot/console-web/src/localization.test.ts index f7663c23..8fc43a9f 100644 --- a/dotbot/console-web/src/localization.test.ts +++ b/dotbot/console-web/src/localization.test.ts @@ -42,7 +42,7 @@ const bot = (id: string, calibrationId?: string): UnifiedBot => led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/mapChrome.css b/dotbot/console-web/src/mapChrome.css index 5e70ee1c..aeae2868 100644 --- a/dotbot/console-web/src/mapChrome.css +++ b/dotbot/console-web/src/mapChrome.css @@ -195,3 +195,34 @@ background: var(--text); box-shadow: 0 1px 3px rgba(0, 0, 0, 0.3); } + +/* A side panel's collapse / expand button: a 32 px target round a small + chevron. */ +.db-panel-toggle { + flex: none; + display: flex; + align-items: center; + justify-content: center; + width: 32px; + height: 32px; + padding: 0; + border: 1px solid transparent; + border-radius: 7px; + background: transparent; + color: var(--muted); + font: 18px/1 var(--font-ui); + cursor: pointer; +} +.db-panel-toggle:hover { + background: var(--elevated); + border-color: var(--hairline); + color: var(--text); +} +.db-panel-toggle:active { + background: var(--grid); +} +.db-panel-toggle:focus-visible { + outline: 2px solid var(--accent); + outline-offset: 1px; + color: var(--text); +} diff --git a/dotbot/console-web/src/mapGestures.test.tsx b/dotbot/console-web/src/mapGestures.test.tsx index 99d86738..b9b8b119 100644 --- a/dotbot/console-web/src/mapGestures.test.tsx +++ b/dotbot/console-web/src/mapGestures.test.tsx @@ -1,14 +1,15 @@ import React, { useState } from "react"; -import { cleanup, fireEvent, render, screen } from "@testing-library/react"; +import { act, cleanup, fireEvent, render, screen } from "@testing-library/react"; import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; +import { areaToFraction } from "./frame"; import { frameMm } from "./grid"; import { MapView } from "./MapView"; import { MAP_MODIFIER, Modifier } from "./shortcuts"; import type { Area, LH2Position, Site, UnifiedBot } from "./types"; import { Camera, - SITE_CAMERA, + FRAME_CAMERA, ZOOM_STEP, cameraForArea, viewGeom, @@ -37,7 +38,7 @@ const bot = (id: string, position: LH2Position, extra: Partial = {}) led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], @@ -70,7 +71,7 @@ interface HarnessProps { const Harness: React.FC = ({ bots = [], selection = new Set(), - from = SITE_CAMERA, + from = FRAME_CAMERA, onSelect = () => {}, }) => { const [cam, setCam] = useState(from); @@ -186,9 +187,9 @@ describe("the wheel", () => { it("does nothing with no modifier, or with another one", () => { render(); fireEvent.wheel(canvas(), { deltaY: -100, clientX: 300, clientY: 200 }); - expectCamera(camera(), SITE_CAMERA); + expectCamera(camera(), FRAME_CAMERA); fireEvent.wheel(canvas(), { deltaY: -100, clientX: 300, clientY: 200, ...held(MAP_MODIFIER.select) }); - expectCamera(camera(), SITE_CAMERA); + expectCamera(camera(), FRAME_CAMERA); }); }); @@ -219,8 +220,8 @@ describe("a drag with the zoom modifier", () => { it("frames the rectangle it drew", () => { const onSelect = vi.fn(); render(); - const tl = under(300, 200, SITE_CAMERA); - const br = under(500, 400, SITE_CAMERA); + const tl = under(300, 200, FRAME_CAMERA); + const br = under(500, 400, FRAME_CAMERA); drag(MAP_MODIFIER.zoom, [300, 200], [500, 400]); @@ -286,7 +287,7 @@ describe("a drag with the select modifier", () => { expect(onSelect).toHaveBeenLastCalledWith(["a", "b"], "add"); drag(MAP_MODIFIER.select, [10, 10], [60, 60]); expect(onSelect).toHaveBeenLastCalledWith([], "add"); - expectCamera(camera(), SITE_CAMERA); + expectCamera(camera(), FRAME_CAMERA); }); it("leaves the selection alone when it does not move", () => { @@ -316,3 +317,92 @@ describe("a plain press", () => { expect(onSelect).toHaveBeenCalledWith([], "replace"); }); }); + +describe("a panel toggle", () => { + // The map between both panes, then with the rail or the right pane + // collapsed: the canvas grows into the space the pane gave up, and its left + // edge moves with the rail. + const RAIL_PX = 288; + const PANE_PX = 272; + interface Rect { + left: number; + w: number; + h: number; + } + const BOTH: Rect = { left: RAIL_PX, w: 420, h: 860 }; + const NO_RAIL: Rect = { left: 0, w: BOTH.w + RAIL_PX, h: BOTH.h }; + const NO_PANE: Rect = { left: RAIL_PX, w: BOTH.w + PANE_PX, h: BOTH.h }; + const size = (c: Rect) => + ({ + width: c.w, + height: c.h, + top: 0, + left: c.left, + right: c.left + c.w, + bottom: c.h, + x: c.left, + y: 0, + toJSON: () => ({}), + }) as DOMRect; + // Where a floor point lands in client pixels, for this canvas and camera. + const onScreen = (p: LH2Position, c: Rect, cam: Camera) => { + const g = viewGeom(c.w, c.h, VIEWPORT); + const { fx, fy } = areaToFraction(p, VIEWPORT); + return { + x: c.left + c.w / 2 + cam.tx + cam.scale * g.boxW * (fx - 0.5), + y: c.h / 2 + cam.ty + cam.scale * g.boxH * (fy - 0.5), + }; + }; + const FLOOR: LH2Position[] = [ + { x: 1000, y: 1000 }, + { x: 200, y: 1700 }, + { x: 1900, y: 150 }, + ]; + + let fire: (() => void) | null = null; + const Real = globalThis.ResizeObserver; + beforeEach(() => { + globalThis.ResizeObserver = class { + constructor(cb: () => void) { + fire = cb; + } + observe() {} + unobserve() {} + disconnect() {} + } as unknown as typeof ResizeObserver; + }); + afterEach(() => { + globalThis.ResizeObserver = Real; + fire = null; + }); + + for (const [name, open] of [ + ["rail", NO_RAIL], + ["right pane", NO_PANE], + ] as const) { + it(`leaves every floor point where it was on screen when the ${name} toggles`, () => { + rectSpy.mockReturnValue(size(BOTH)); + const g = viewGeom(BOTH.w, BOTH.h, VIEWPORT); + const from = cameraForArea(ARENA, VIEWPORT, g, zoomMax(C405, VIEWPORT, g)); + render(); + const before = camera(); + + for (const [was, now] of [ + [BOTH, open], + [open, BOTH], + ]) { + const c0 = camera(); + rectSpy.mockReturnValue(size(now)); + act(() => fire!()); + const c1 = camera(); + for (const p of FLOOR) { + const a = onScreen(p, was, c0); + const b = onScreen(p, now, c1); + expect(b.x - a.x).toBeCloseTo(0, 2); + expect(b.y - a.y).toBeCloseTo(0, 2); + } + } + expectCamera(camera(), before); + }); + } +}); diff --git a/dotbot/console-web/src/minimap.test.tsx b/dotbot/console-web/src/minimap.test.tsx new file mode 100644 index 00000000..295a4d99 --- /dev/null +++ b/dotbot/console-web/src/minimap.test.tsx @@ -0,0 +1,72 @@ +import React from "react"; +import { fireEvent, render, screen } from "@testing-library/react"; +import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; + +import { siteViewport } from "./frame"; +import { frameMm } from "./grid"; +import { Minimap } from "./Minimap"; +import type { Area, Site } from "./types"; +import { Camera, viewGeom } from "./zoom"; + +const ARENA: Area = { x: 0, y: 0, w: 2000, h: 2000, name: "arena" }; +const SITE: Site = { + name: "c405-arena", + anchor: "the arena's top-left corner", + extent_mm: [2000, 2000], + areas: [ARENA], +}; +const VIEWPORT = siteViewport(SITE, ARENA); +const GEOM = viewGeom(900, 600, VIEWPORT); +const MINIMAP_PX = 190; + +let rectSpy: ReturnType; + +beforeEach(() => { + rectSpy = vi + .spyOn(HTMLElement.prototype, "getBoundingClientRect") + .mockReturnValue({ + width: MINIMAP_PX, + height: MINIMAP_PX, + top: 0, + left: 0, + right: MINIMAP_PX, + bottom: MINIMAP_PX, + x: 0, + y: 0, + toJSON: () => ({}), + } as DOMRect); +}); + +afterEach(() => rectSpy.mockRestore()); + +describe("the minimap", () => { + it("centres the map on the point of the site that was pressed", () => { + let cam: Camera = { scale: 4, tx: 0, ty: 0 }; + const setCam = (next: React.SetStateAction) => { + cam = typeof next === "function" ? next(cam) : next; + }; + render( + , + ); + + // A quarter of the way into the site on both axes: frame (500, 500). + fireEvent.pointerDown(screen.getByTitle("Drag to move the map view"), { + button: 0, + clientX: MINIMAP_PX / 4, + clientY: MINIMAP_PX / 4, + pointerId: 1, + }); + + expect(frameMm("x", GEOM.w / 2, VIEWPORT, GEOM, cam)).toBeCloseTo(500, 6); + expect(frameMm("y", GEOM.h / 2, VIEWPORT, GEOM, cam)).toBeCloseTo(500, 6); + }); +}); diff --git a/dotbot/console-web/src/missions.test.ts b/dotbot/console-web/src/missions.test.ts index d144aa71..ee31efac 100644 --- a/dotbot/console-web/src/missions.test.ts +++ b/dotbot/console-web/src/missions.test.ts @@ -18,7 +18,7 @@ const bot = (id: string, over: Partial = {}): UnifiedBot => ({ led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/panelKeys.test.tsx b/dotbot/console-web/src/panelKeys.test.tsx new file mode 100644 index 00000000..21b9697e --- /dev/null +++ b/dotbot/console-web/src/panelKeys.test.tsx @@ -0,0 +1,222 @@ +import React from "react"; +import { cleanup, fireEvent, render, screen } from "@testing-library/react"; +import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; + +import { ACTION_KEY } from "./shortcuts"; +import type { Site, UnifiedBot } from "./types"; + +const site: Site = { + name: "c405-arena", + anchor: "the arena's top-left corner", + extent_mm: [2000, 4000], + areas: [{ x: 0, y: 0, w: 2000, h: 2000, name: "arena" }], +}; +const VIEWPORT = { x: -2000, y: -2000, w: 6000, h: 8000 }; + +const bot = (id: string, extra: Partial = {}): UnifiedBot => ({ + id, + state: "Running", + link: "active", + position: { x: 500, y: 500 }, + heading: 0, + battery: 2.9, + led: null, + deviceType: "DotBotV3", + application: 0, + pose: null, + drivable: true, + nav: "drive", + waypoints: [], + trail: [], + image: null, + resetCause: null, + severity: "normal", + batteryPct: 80, + batteryLevel: "ok", + swarmit: null, + ...extra, +}); + +vi.mock("./useFleet", () => ({ + useFleet: () => ({ + bots: [bot("BADCAFE111111111")], + site, + session: null, + setSession: () => {}, + viewport: VIEWPORT, + wsUp: true, + }), +})); + +vi.mock("./useOrchestration", () => ({ + useOrchestration: () => ({ + logs: [], + jobs: [], + queue: {}, + fleetPct: 0, + flashing: false, + clearLogs: vi.fn(), + flash: vi.fn(), + act: vi.fn(), + }), +})); + +vi.mock("./useMrta", () => ({ + useMrta: () => ({ status: { available: false, on: false }, toggle: vi.fn() }), +})); + +vi.mock("./api", () => ({ + fetchConnection: vi.fn(async () => null), + fetchBuild: vi.fn(async () => null), + putWaypoints: vi.fn(async () => {}), + abandonCalibration: vi.fn(async () => {}), + captureCalibrationPoint: vi.fn(), + previewCalibrationPoints: vi.fn(async () => ({ points: [], reads: 25 })), + pushCalibration: vi.fn(), + redoCalibrationPoint: vi.fn(), + saveCalibration: vi.fn(), + startCalibration: vi.fn(), +})); + +import { App } from "./App"; + +let rectSpy: ReturnType; +beforeEach(() => { + rectSpy = vi + .spyOn(HTMLElement.prototype, "getBoundingClientRect") + .mockReturnValue({ + width: 900, + height: 600, + top: 0, + left: 0, + right: 900, + bottom: 600, + x: 0, + y: 0, + toJSON: () => ({}), + } as DOMRect); + window.history.replaceState({}, "", "/"); +}); +afterEach(() => { + rectSpy.mockRestore(); + cleanup(); + window.history.replaceState({}, "", "/"); + window.localStorage.clear(); +}); + +const press = (key: string, target: Element = document.body) => + fireEvent.keyDown(target, { key }); +const toggle = (verb: "Collapse" | "Expand", side: "left" | "right") => + screen.queryByRole("button", { name: `${verb} the ${side} panel` }); + +describe("the side panel keys", () => { + it("collapse and expand the left panel", () => { + render(); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + press(ACTION_KEY.leftPanel); + expect(toggle("Expand", "left")).toBeInTheDocument(); + expect(toggle("Collapse", "right")).toBeInTheDocument(); + press(ACTION_KEY.leftPanel); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + }); + + it("collapse and expand the right panel", () => { + render(); + press(ACTION_KEY.rightPanel); + expect(toggle("Expand", "right")).toBeInTheDocument(); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + press(ACTION_KEY.rightPanel); + expect(toggle("Collapse", "right")).toBeInTheDocument(); + }); + + it("toggle once for a key held down, not once per auto-repeat", () => { + render(); + press(ACTION_KEY.leftPanel); + fireEvent.keyDown(document.body, { key: ACTION_KEY.leftPanel, repeat: true }); + expect(toggle("Expand", "left")).toBeInTheDocument(); + }); + + it("are left alone while a field has focus", () => { + render(); + const slider = screen.getByLabelText("Zoom"); + slider.focus(); + press(ACTION_KEY.leftPanel, slider); + press(ACTION_KEY.rightPanel, slider); + const input = document.createElement("input"); + document.body.appendChild(input); + press(ACTION_KEY.leftPanel, input); + press(ACTION_KEY.rightPanel, input); + input.remove(); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + expect(toggle("Collapse", "right")).toBeInTheDocument(); + }); + + it("do what the buttons do, which name their key", () => { + render(); + const left = toggle("Collapse", "left")!; + expect(left).toHaveAttribute("title", `Collapse the left panel (${ACTION_KEY.leftPanel})`); + fireEvent.click(left); + fireEvent.click(toggle("Collapse", "right")!); + expect(toggle("Expand", "left")).toBeInTheDocument(); + expect(toggle("Expand", "right")).toBeInTheDocument(); + press(ACTION_KEY.leftPanel); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + }); + + it("start the left panel collapsed from ?rail=collapsed", () => { + window.history.replaceState({}, "", "/?rail=collapsed"); + render(); + expect(toggle("Expand", "left")).toBeInTheDocument(); + }); +}); + +describe("the side panels, remembered by this browser", () => { + const KEY = "dotbot.console.panels"; + + it("keep a toggle across a reload", () => { + const { unmount } = render(); + press(ACTION_KEY.leftPanel); + fireEvent.click(toggle("Collapse", "right")!); + expect(JSON.parse(window.localStorage.getItem(KEY)!)).toEqual({ left: true, right: true }); + unmount(); + render(); + expect(toggle("Expand", "left")).toBeInTheDocument(); + expect(toggle("Expand", "right")).toBeInTheDocument(); + }); + + it("open both panels from a corrupt record", () => { + window.localStorage.setItem(KEY, "{not json"); + render(); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + expect(toggle("Collapse", "right")).toBeInTheDocument(); + }); + + it("open both panels when storage refuses them, and still toggle", () => { + const getItem = Storage.prototype.getItem; + const setItem = Storage.prototype.setItem; + const get = vi.spyOn(Storage.prototype, "getItem").mockImplementation(function (this: Storage, k: string) { + if (k === KEY) throw new Error("blocked"); + return getItem.call(this, k); + }); + const set = vi.spyOn(Storage.prototype, "setItem").mockImplementation(function (this: Storage, k: string, v: string) { + if (k === KEY) throw new Error("blocked"); + return setItem.call(this, k, v); + }); + render(); + expect(toggle("Collapse", "left")).toBeInTheDocument(); + press(ACTION_KEY.rightPanel); + expect(toggle("Expand", "right")).toBeInTheDocument(); + get.mockRestore(); + set.mockRestore(); + }); + + it("let ?rail=collapsed win without overwriting what is stored", () => { + window.localStorage.setItem(KEY, JSON.stringify({ left: false, right: true })); + window.history.replaceState({}, "", "/?rail=collapsed"); + render(); + expect(toggle("Expand", "left")).toBeInTheDocument(); + expect(toggle("Expand", "right")).toBeInTheDocument(); + press(ACTION_KEY.rightPanel); + expect(JSON.parse(window.localStorage.getItem(KEY)!)).toEqual({ left: false, right: false }); + }); +}); diff --git a/dotbot/console-web/src/panels.test.ts b/dotbot/console-web/src/panels.test.ts new file mode 100644 index 00000000..cd837fb5 --- /dev/null +++ b/dotbot/console-web/src/panels.test.ts @@ -0,0 +1,45 @@ +import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; + +import { DEFAULT_PANELS, loadPanels, savePanel } from "./panels"; + +const KEY = "dotbot.console.panels"; + +beforeEach(() => window.localStorage.clear()); +afterEach(() => vi.restoreAllMocks()); + +describe("the side panels, per browser", () => { + it("are open with nothing stored", () => { + expect(loadPanels()).toEqual({ left: false, right: false }); + }); + + it("read back each side as saved, leaving the other alone", () => { + savePanel("right", true); + expect(loadPanels()).toEqual({ left: false, right: true }); + savePanel("left", true); + savePanel("right", false); + expect(loadPanels()).toEqual({ left: true, right: false }); + }); + + it("keep a valid side and open the other", () => { + window.localStorage.setItem(KEY, JSON.stringify({ left: true, right: "yes" })); + expect(loadPanels()).toEqual({ left: true, right: false }); + }); + + it("read anything but a stored record as open", () => { + for (const raw of ["not json", "true", "[]", "null", "{"]) { + window.localStorage.setItem(KEY, raw); + expect(loadPanels()).toEqual(DEFAULT_PANELS); + } + }); + + it("are open when storage refuses, and forget quietly on a save", () => { + vi.spyOn(Storage.prototype, "getItem").mockImplementation(() => { + throw new Error("blocked"); + }); + vi.spyOn(Storage.prototype, "setItem").mockImplementation(() => { + throw new Error("blocked"); + }); + expect(loadPanels()).toEqual(DEFAULT_PANELS); + expect(() => savePanel("left", true)).not.toThrow(); + }); +}); diff --git a/dotbot/console-web/src/panels.ts b/dotbot/console-web/src/panels.ts new file mode 100644 index 00000000..6ffcae42 --- /dev/null +++ b/dotbot/console-web/src/panels.ts @@ -0,0 +1,54 @@ +import { useCallback, useState } from "react"; + +import { store } from "./persisted"; + +// Whether each side panel is collapsed, as this browser last left it. + +export type PanelSide = "left" | "right"; + +export type PanelState = Record; + +/** Both panels open: what a browser with nothing stored sees. */ +export const DEFAULT_PANELS: PanelState = { left: false, right: false }; + +const KEY = "dotbot.console.panels"; + +/** This browser's panels, side by side; anything unreadable is open. */ +export function loadPanels(): PanelState { + try { + const raw = window.localStorage.getItem(KEY); + const value: unknown = raw === null ? null : JSON.parse(raw); + if (!value || typeof value !== "object" || Array.isArray(value)) return DEFAULT_PANELS; + const { left, right } = value as Record; + return { + left: typeof left === "boolean" ? left : DEFAULT_PANELS.left, + right: typeof right === "boolean" ? right : DEFAULT_PANELS.right, + }; + } catch { + return DEFAULT_PANELS; + } +} + +/** Remember one side, leaving the other as stored. */ +export function savePanel(side: PanelSide, collapsed: boolean): void { + store(KEY, { ...loadPanels(), [side]: collapsed }); +} + +/** + * One panel's collapsed state, starting from `initial` when given and from + * storage otherwise. The second setter writes it back to storage; the third + * changes it for this page only. + */ +export function usePanel(side: PanelSide, initial?: boolean) { + const [collapsed, setCollapsed] = useState(() => initial ?? loadPanels()[side]); + const update = useCallback( + (next: boolean | ((prev: boolean) => boolean)) => + setCollapsed((prev) => { + const value = typeof next === "function" ? next(prev) : next; + savePanel(side, value); + return value; + }), + [side], + ); + return [collapsed, update, setCollapsed] as const; +} diff --git a/dotbot/console-web/src/redoMission.test.tsx b/dotbot/console-web/src/redoMission.test.tsx index 78698930..8953dccf 100644 --- a/dotbot/console-web/src/redoMission.test.tsx +++ b/dotbot/console-web/src/redoMission.test.tsx @@ -23,7 +23,7 @@ const bot = (id: string, extra: Partial = {}): UnifiedBot => ({ led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/rightPaneCollapsed.test.tsx b/dotbot/console-web/src/rightPaneCollapsed.test.tsx index 549bb836..8cf525fd 100644 --- a/dotbot/console-web/src/rightPaneCollapsed.test.tsx +++ b/dotbot/console-web/src/rightPaneCollapsed.test.tsx @@ -123,7 +123,7 @@ describe("the collapsed right pane", () => { it("still collapses back from the open pane", () => { render(); fireEvent.click(screen.getByTitle("Layers")); - fireEvent.click(screen.getByTitle("Collapse the right pane")); + fireEvent.click(screen.getByRole("button", { name: "Collapse the right panel" })); expect(state()).toBe("collapsed:layers"); expect(screen.getByTitle("Layers")).toBeInTheDocument(); }); diff --git a/dotbot/console-web/src/robotDrawing.test.tsx b/dotbot/console-web/src/robotDrawing.test.tsx new file mode 100644 index 00000000..cd597b7d --- /dev/null +++ b/dotbot/console-web/src/robotDrawing.test.tsx @@ -0,0 +1,128 @@ +import React, { useState } from "react"; +import { cleanup, fireEvent, render, screen } from "@testing-library/react"; +import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; + +import { + DEFAULT_ROBOT_DRAWING, + RobotDrawing, + loadRobotDrawing, + saveRobotDrawing, +} from "./robotDrawing"; +import { RightPane } from "./RightPane"; +import type { Calibration } from "./useCalibration"; + +const KEY = "dotbot.console.robotDrawing"; + +beforeEach(() => window.localStorage.clear()); +afterEach(() => { + cleanup(); + vi.restoreAllMocks(); +}); + +describe("the robot drawing choice, per browser", () => { + it("defaults to Body with the possible footprint on", () => { + expect(loadRobotDrawing()).toEqual({ mode: "body", footprint: true }); + }); + + it("reads back what was saved", () => { + saveRobotDrawing({ mode: "sensor", footprint: false }); + expect(loadRobotDrawing()).toEqual({ mode: "sensor", footprint: false }); + }); + + it("keeps each field that is valid and defaults the other", () => { + window.localStorage.setItem(KEY, JSON.stringify({ mode: "sensor", footprint: "yes" })); + expect(loadRobotDrawing()).toEqual({ mode: "sensor", footprint: true }); + window.localStorage.setItem(KEY, JSON.stringify({ mode: "robot", footprint: false })); + expect(loadRobotDrawing()).toEqual({ mode: "body", footprint: false }); + }); + + it("reads anything but a stored record as the default", () => { + for (const raw of ["not json", "true", "[]", "null"]) { + window.localStorage.setItem(KEY, raw); + expect(loadRobotDrawing()).toEqual(DEFAULT_ROBOT_DRAWING); + } + }); + + it("falls back to the default when storage refuses", () => { + vi.spyOn(Storage.prototype, "getItem").mockImplementation(() => { + throw new Error("blocked"); + }); + expect(loadRobotDrawing()).toEqual(DEFAULT_ROBOT_DRAWING); + }); + + it("forgets quietly when storage refuses a save", () => { + vi.spyOn(Storage.prototype, "setItem").mockImplementation(() => { + throw new Error("blocked"); + }); + expect(() => saveRobotDrawing({ mode: "sensor", footprint: true })).not.toThrow(); + }); +}); + +const Harness: React.FC = () => { + const [drawing, setDrawing] = useState(DEFAULT_ROBOT_DRAWING); + return ( + <> +
{JSON.stringify(drawing)}
+ {}} + collapsed={false} + setCollapsed={() => {}} + bots={[]} + site={null} + hiddenAreas={new Set()} + onAreaToggle={() => {}} + layers={{ + batteryBars: true, + waypoints: true, + hotSpots: false, + dotBots: true, + trails: false, + crashedOnly: false, + }} + layerRows={[]} + onLayerToggle={() => {}} + robotDrawing={drawing} + onRobotDrawing={setDrawing} + session={null} + calibration={{} as Calibration} + device="" + onDeviceChange={() => {}} + onCalibrationDone={() => {}} + /> + + ); +}; + +describe("the robot drawing control on the Layers tab", () => { + const state = () => JSON.parse(screen.getByTestId("drawing").textContent!); + const hint = () => screen.getByTestId("robot-drawing-hint").textContent; + + it("switches between Body and Sensor", () => { + render(); + expect(screen.getByRole("radio", { name: "Body" })).toHaveAttribute("aria-checked", "true"); + fireEvent.click(screen.getByRole("radio", { name: "Sensor" })); + expect(state()).toEqual({ mode: "sensor", footprint: true }); + expect(screen.getByRole("radio", { name: "Sensor" })).toHaveAttribute("aria-checked", "true"); + fireEvent.click(screen.getByRole("radio", { name: "Body" })); + expect(state().mode).toBe("body"); + }); + + it("flips the possible footprint", () => { + render(); + fireEvent.click(screen.getByText("Possible footprint")); + expect(state()).toEqual({ mode: "body", footprint: false }); + }); + + it("gives a different hint for each mode and checkbox", () => { + render(); + const seen = new Set([hint()]); + fireEvent.click(screen.getByText("Possible footprint")); + seen.add(hint()); + fireEvent.click(screen.getByRole("radio", { name: "Sensor" })); + seen.add(hint()); + fireEvent.click(screen.getByText("Possible footprint")); + seen.add(hint()); + expect(seen.size).toBe(4); + }); +}); diff --git a/dotbot/console-web/src/robotDrawing.ts b/dotbot/console-web/src/robotDrawing.ts new file mode 100644 index 00000000..68c9bc00 --- /dev/null +++ b/dotbot/console-web/src/robotDrawing.ts @@ -0,0 +1,38 @@ +import { store } from "./persisted"; + +/** What each robot on the map is drawn as: its body, or its sensor point. */ +export type DrawMode = "body" | "sensor"; + +export interface RobotDrawing { + mode: DrawMode; + /** Ring each robot drawn as its sensor point with where its body could be. */ + footprint: boolean; +} + +export const DEFAULT_ROBOT_DRAWING: RobotDrawing = { mode: "body", footprint: true }; + +const KEY = "dotbot.console.robotDrawing"; + +/** This browser's choice, field by field; anything unreadable is the default. */ +export function loadRobotDrawing(): RobotDrawing { + try { + const raw = window.localStorage.getItem(KEY); + const value: unknown = raw === null ? null : JSON.parse(raw); + if (!value || typeof value !== "object" || Array.isArray(value)) { + return DEFAULT_ROBOT_DRAWING; + } + const { mode, footprint } = value as Record; + return { + mode: mode === "body" || mode === "sensor" ? mode : DEFAULT_ROBOT_DRAWING.mode, + footprint: + typeof footprint === "boolean" ? footprint : DEFAULT_ROBOT_DRAWING.footprint, + }; + } catch { + return DEFAULT_ROBOT_DRAWING; + } +} + +/** Remember the choice; a browser that refuses storage just forgets it. */ +export function saveRobotDrawing(drawing: RobotDrawing): void { + store(KEY, drawing); +} diff --git a/dotbot/console-web/src/robotShapes.test.tsx b/dotbot/console-web/src/robotShapes.test.tsx deleted file mode 100644 index c33b9193..00000000 --- a/dotbot/console-web/src/robotShapes.test.tsx +++ /dev/null @@ -1,86 +0,0 @@ -import React, { useState } from "react"; -import { cleanup, fireEvent, render, screen } from "@testing-library/react"; -import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; - -import { loadRobotShapes, saveRobotShapes } from "./robotShapes"; -import { RightPane } from "./RightPane"; -import type { Calibration } from "./useCalibration"; - -beforeEach(() => window.localStorage.clear()); -afterEach(() => { - cleanup(); - vi.restoreAllMocks(); -}); - -describe("the robot shapes choice, per browser", () => { - it("defaults to the robot shape", () => { - expect(loadRobotShapes()).toBe(true); - }); - - it("reads back what was saved", () => { - saveRobotShapes(false); - expect(loadRobotShapes()).toBe(false); - saveRobotShapes(true); - expect(loadRobotShapes()).toBe(true); - }); - - it("reads anything but a stored boolean as the robot shape", () => { - window.localStorage.setItem("dotbot.console.robotShapes", "0"); - expect(loadRobotShapes()).toBe(true); - window.localStorage.setItem("dotbot.console.robotShapes", "not json"); - expect(loadRobotShapes()).toBe(true); - }); - - it("falls back to the robot shape when storage refuses", () => { - vi.spyOn(Storage.prototype, "getItem").mockImplementation(() => { - throw new Error("blocked"); - }); - expect(loadRobotShapes()).toBe(true); - }); -}); - -const Harness: React.FC = () => { - const [on, setOn] = useState(true); - return ( - <> -
{String(on)}
- {}} - collapsed={false} - setCollapsed={() => {}} - bots={[]} - site={null} - hiddenAreas={new Set()} - onAreaToggle={() => {}} - layers={{ - batteryBars: true, - waypoints: true, - hotSpots: false, - dotBots: true, - trails: false, - crashedOnly: false, - }} - layerRows={[]} - onLayerToggle={() => {}} - robotShapes={on} - onRobotShapesToggle={() => setOn((prev) => !prev)} - session={null} - calibration={{} as Calibration} - device="" - onDeviceChange={() => {}} - onCalibrationDone={() => {}} - /> - - ); -}; - -describe("the Robot shapes row on the Layers tab", () => { - it("flips the choice when clicked", () => { - render(); - fireEvent.click(screen.getByText("Robot shapes")); - expect(screen.getByTestId("shapes").textContent).toBe("false"); - fireEvent.click(screen.getByText("Robot shapes")); - expect(screen.getByTestId("shapes").textContent).toBe("true"); - }); -}); diff --git a/dotbot/console-web/src/robotShapes.ts b/dotbot/console-web/src/robotShapes.ts deleted file mode 100644 index f6367bfd..00000000 --- a/dotbot/console-web/src/robotShapes.ts +++ /dev/null @@ -1,19 +0,0 @@ -import { store } from "./persisted"; - -const KEY = "dotbot.console.robotShapes"; - -/** Whether this browser draws DotBots as the robot; true unless it said otherwise. */ -export function loadRobotShapes(): boolean { - try { - const raw = window.localStorage.getItem(KEY); - const value: unknown = raw === null ? true : JSON.parse(raw); - return typeof value === "boolean" ? value : true; - } catch { - return true; - } -} - -/** Remember the choice; a browser that refuses storage just forgets it. */ -export function saveRobotShapes(on: boolean): void { - store(KEY, on); -} diff --git a/dotbot/console-web/src/savedView.test.tsx b/dotbot/console-web/src/savedView.test.tsx index 3dc40c12..02f05507 100644 --- a/dotbot/console-web/src/savedView.test.tsx +++ b/dotbot/console-web/src/savedView.test.tsx @@ -19,9 +19,11 @@ import { import type { Area, Site } from "./types"; import { Camera, - SITE_CAMERA, + FRAME_CAMERA, cameraForArea, + SITE_ZOOM, cameraForZoom, + centreOfView, padArea, viewGeom, visibleArea, @@ -120,6 +122,12 @@ afterEach(() => { const transform = () => screen.getByTestId("camera-layer").style.transform; +// The camera the site view lands on in a 900 x 600 canvas. +const siteTransform = () => { + const c = cameraForZoom(SITE_ZOOM, site, VIEWPORT, viewGeom(900, 600, VIEWPORT))!; + return `translate(${c.tx}px, ${c.ty}px) scale(${c.scale})`; +}; + const camOf = (): Camera => { const moved = /translate\(([-\d.e+]+)px, ([-\d.e+]+)px\)/.exec(transform())!; const scaled = /scale\(([-\d.e+]+)\)/.exec(transform())!; @@ -130,10 +138,6 @@ const camOf = (): Camera => { }; }; -/** The floor the rendered map is showing, in a canvas of this size. */ -const shown = (width: number, height: number) => - visibleArea(camOf(), VIEWPORT, viewGeom(width, height, VIEWPORT)); - const centre = (a: Area) => ({ x: a.x + a.w / 2, y: a.y + a.h / 2 }); describe("a view stated as floor rather than as a camera", () => { @@ -150,7 +154,7 @@ describe("a view stated as floor rather than as a camera", () => { }); it("reaches past the drawn frame at the site camera, as the canvas does", () => { - const rect = visibleArea(SITE_CAMERA, VIEWPORT, geom); + const rect = visibleArea(FRAME_CAMERA, VIEWPORT, geom); // The box carries the viewport's aspect and the canvas has slack on one // axis, so the view is the whole frame and then some on both. @@ -162,16 +166,16 @@ describe("a view stated as floor rather than as a camera", () => { }); describe("the view this browser remembers", () => { - const dock: Area = { x: 2400, y: 3200, w: 900, h: 700 }; + const dock = { x: 2850, y: 3550, pxPerMm: 0.4 }; it("round-trips one view per site through storage", () => { let views: SavedViews = withView({}, "c405-arena", dock); - views = withView(views, "limerick", { x: 0, y: 0, w: 100, h: 100 }); + views = withView(views, "limerick", { x: 50, y: 50, pxPerMm: 2 }); saveSavedViews(views); const back = loadSavedViews(); - expect(back["c405-arena"]).toEqual({ x: 2400, y: 3200, w: 900, h: 700 }); - expect(back.limerick).toEqual({ x: 0, y: 0, w: 100, h: 100 }); + expect(back["c405-arena"]).toEqual(dock); + expect(back.limerick).toEqual({ x: 50, y: 50, pxPerMm: 2 }); expect(viewFor(back, "c405-arena", VIEWPORT)).toEqual(dock); }); @@ -187,85 +191,77 @@ describe("the view this browser remembers", () => { window.localStorage.setItem(KEY, '["a list"]'); expect(loadSavedViews()).toEqual({}); - // A rectangle with no area, or one carrying nothing measurable, is not a - // view: it would fit to a scale of nothing. + // No scale, a scale of nothing, or a rectangle stored by an older console. window.localStorage.setItem( KEY, - '{"c405-arena":{"x":0,"y":0,"w":0,"h":10},"other":{"x":null,"y":1,"w":1,"h":1}}', + '{"a":{"x":0,"y":0,"pxPerMm":0},"b":{"x":null,"y":1,"pxPerMm":1},"c":{"x":0,"y":0,"w":10,"h":10}}', ); expect(loadSavedViews()).toEqual({}); }); - it("drops a view the floor no longer meets", () => { - const away = withView({}, "c405-arena", { x: 90000, y: 90000, w: 900, h: 700 }); + it("drops a view whose centre is off the floor", () => { + const away = withView({}, "c405-arena", { x: 90000, y: 90000, pxPerMm: 0.4 }); expect(viewFor(away, "c405-arena", VIEWPORT)).toBeNull(); - - // Touching the frame is meeting it: the clamps take it from there. - const edge = withView({}, "c405-arena", { x: -2500, y: 0, w: 900, h: 700 }); + const edge = withView({}, "c405-arena", { x: VIEWPORT.x, y: 0, pxPerMm: 0.4 }); expect(viewFor(edge, "c405-arena", VIEWPORT)).not.toBeNull(); }); }); describe("the view the map opens on", () => { - it("writes the floor it is looking at once the camera settles", async () => { + // The floor point at the canvas centre and the floor's px per mm, as drawn. + const viewShown = (width: number, height: number) => + centreOfView(camOf(), VIEWPORT, viewGeom(width, height, VIEWPORT)); + const expectSameView = (got: { x: number; y: number; pxPerMm: number }, want: typeof got) => { + expect(got.pxPerMm).toBeCloseTo(want.pxPerMm, 9); + expect(got.x).toBeCloseTo(want.x, 4); + expect(got.y).toBeCloseTo(want.y, 4); + }; + const leaveOnDock = async (width: number, height: number) => { + sizeCanvas(width, height); render(); fireEvent.click(screen.getByTitle("Zoom to dock")); - const floor = shown(900, 600); - + const view = viewShown(width, height); await waitFor( () => expect(loadSavedViews()[site.name]).toBeDefined(), { timeout: VIEW_SETTLE_MS * 8 }, ); + return view; + }; - const stored = loadSavedViews()[site.name]; - expect(stored.x).toBeCloseTo(floor.x, 6); - expect(stored.y).toBeCloseTo(floor.y, 6); - expect(stored.w).toBeCloseTo(floor.w, 6); - expect(stored.h).toBeCloseTo(floor.h, 6); + it("writes the centre and the scale it is looking at once the camera settles", async () => { + const view = await leaveOnDock(900, 600); + expectSameView(loadSavedViews()[site.name], view); }); - it("comes back to the same floor in a canvas of another shape", async () => { - render(); - fireEvent.click(screen.getByTitle("Zoom to dock")); - const left = camOf(); - const floor = shown(900, 600); - await waitFor( - () => expect(loadSavedViews()[site.name]).toBeDefined(), - { timeout: VIEW_SETTLE_MS * 8 }, - ); + // Landscape, portrait, and a map squeezed between both panes that is + // narrower than it is tall: every pair crosses the frame's aspect ratio + // one way or the other. + for (const [from, to] of [ + [[900, 600], [600, 900]], + [[900, 600], [420, 860]], + [[420, 860], [900, 600]], + ] as const) { + it(`comes back to the same centre and scale from ${from.join("x")} to ${to.join("x")}`, async () => { + const left = await leaveOnDock(from[0], from[1]); + cleanup(); + sizeCanvas(to[0], to[1]); + render(); + expectSameView(viewShown(to[0], to[1]), left); + }); + } + + it("comes back to the same centre and scale with a panel collapsed", async () => { + const left = await leaveOnDock(420, 860); cleanup(); - - // The same console opened on a portrait window. - sizeCanvas(600, 900); + // Reloaded with the right pane remembered collapsed: a wider canvas. + window.localStorage.setItem("dotbot.console.panels", JSON.stringify({ left: false, right: true })); + sizeCanvas(760, 860); render(); - const back = shown(600, 900); - - // The floor that was in the middle is in the middle again ... - expect(centre(back).x).toBeCloseTo(centre(floor).x, 6); - expect(centre(back).y).toBeCloseTo(centre(floor).y, 6); - // ... all of it is on screen, the new canvas letterboxing whichever axis - // it has to spare ... - expect(back.x).toBeLessThanOrEqual(floor.x + 1e-6); - expect(back.y).toBeLessThanOrEqual(floor.y + 1e-6); - expect(back.x + back.w).toBeGreaterThanOrEqual(floor.x + floor.w - 1e-6); - expect(back.y + back.h).toBeGreaterThanOrEqual(floor.y + floor.h - 1e-6); - // ... and one axis fits it exactly, so the view is the rectangle fitted - // rather than the rectangle plus a margin. - expect( - Math.abs(back.w - floor.w) < 1e-6 || Math.abs(back.h - floor.h) < 1e-6, - ).toBe(true); - - // Which is the whole reason the rectangle travels rather than the camera: - // the camera that framed this floor frames somewhere else here. - const raw = visibleArea(left, VIEWPORT, viewGeom(600, 900, VIEWPORT)); - expect(Math.hypot( - centre(raw).x - centre(floor).x, - centre(raw).y - centre(floor).y, - )).toBeGreaterThan(100); + expectSameView(viewShown(760, 860), left); }); it("lets ?zoom= outrank what was stored", () => { - saveSavedViews(withView({}, site.name, { x: 0, y: 2000, w: 3330, h: 2000 })); + saveSavedViews(withView({}, site.name, { x: 1665, y: 3000, pxPerMm: 0.1 })); window.history.replaceState({}, "", "/?zoom=dock"); render(); @@ -279,20 +275,20 @@ describe("the view the map opens on", () => { it("opens on the whole site when what was stored is unusable", () => { window.localStorage.setItem(KEY, "{ not json"); render(); - expect(transform()).toBe("translate(0px, 0px) scale(1)"); + expect(transform()).toBe(siteTransform()); cleanup(); // A view of floor this site no longer has: off the map altogether. - saveSavedViews({ [site.name]: { x: 90000, y: 90000, w: 900, h: 700 } }); + saveSavedViews({ [site.name]: { x: 90000, y: 90000, pxPerMm: 0.4 } }); render(); - expect(transform()).toBe("translate(0px, 0px) scale(1)"); + expect(transform()).toBe(siteTransform()); }); it("opens on the whole site when the view belongs to another one", () => { - saveSavedViews(withView({}, "limerick", { x: 2400, y: 3200, w: 900, h: 700 })); + saveSavedViews(withView({}, "limerick", { x: 2850, y: 3550, pxPerMm: 0.4 })); render(); - expect(transform()).toBe("translate(0px, 0px) scale(1)"); + expect(transform()).toBe(siteTransform()); }); }); diff --git a/dotbot/console-web/src/savedView.ts b/dotbot/console-web/src/savedView.ts index f5c2ad50..55ba3f30 100644 --- a/dotbot/console-web/src/savedView.ts +++ b/dotbot/console-web/src/savedView.ts @@ -1,53 +1,44 @@ // Where this browser was last looking, per site. // -// A camera is not portable. The same `{scale, tx, ty}` frames a different -// patch of floor in a canvas of another size, so a view stored as the camera -// itself lands somewhere else on a laptop screen than on the monitor it was -// left on, and anywhere at all once the rail or the right pane has been -// opened. What is stored is the floor the canvas was showing - a rectangle in -// frame millimetres - and the camera is derived from it again on arrival, -// through the same fit a named zoom goes through, so the clamps that keep the -// map reachable apply to a restored view as well. +// A camera is not portable: the same `{scale, tx, ty}` frames a different +// patch of floor in a canvas of another size, and the canvas changes size +// whenever a side panel opens or closes. What is stored is the floor point at +// the canvas centre and the floor's pixels per millimetre, and the camera is +// derived from them again on arrival, so the map opens on the same floor at +// the same size whatever the canvas, held only by the pan clamp. // -// Per site, because a rectangle of one floor means nothing on another, and -// this browser's own like the other map layers: it reaches no controller. +// Per site, because a point of one floor means nothing on another, and this +// browser's own like the other map layers: it reaches no controller. // `?zoom=` outranks it, being an instruction where this is only a default. import { loadRecord, store } from "./persisted"; import type { Area } from "./types"; +import type { ViewCentre } from "./zoom"; const KEY = "dotbot.console.mapView"; /** How long the camera has to sit still before the view is written. */ export const VIEW_SETTLE_MS = 250; -/** The floor one canvas was showing, in frame millimetres. */ -export interface ViewRect { - x: number; - y: number; - w: number; - h: number; -} - /** The last view by site name. */ -export type SavedViews = Record; +export type SavedViews = Record; -const usable = (v: unknown): v is ViewRect => { - const r = v as ViewRect; +const usable = (v: unknown): v is ViewCentre => { + const r = v as ViewCentre; return ( !!r && typeof r === "object" && !Array.isArray(r) && Number.isFinite(r.x) && Number.isFinite(r.y) && - r.w > 0 && - r.h > 0 + Number.isFinite(r.pxPerMm) && + r.pxPerMm > 0 ); }; /** What this browser last looked at, empty when storage says nothing usable. */ export function loadSavedViews(): SavedViews { - return loadRecord(KEY, usable, (rect) => ({ ...rect })); + return loadRecord(KEY, usable, (view) => ({ x: view.x, y: view.y, pxPerMm: view.pxPerMm })); } /** Remember them; a browser that refuses storage just forgets. */ @@ -56,36 +47,29 @@ export function saveSavedViews(views: SavedViews): void { } /** The map with one site's view replaced. */ -export function withView( - views: SavedViews, - site: string, - rect: Area, -): SavedViews { - return { - ...views, - [site]: { x: rect.x, y: rect.y, w: rect.w, h: rect.h }, - }; +export function withView(views: SavedViews, site: string, view: ViewCentre): SavedViews { + return { ...views, [site]: { x: view.x, y: view.y, pxPerMm: view.pxPerMm } }; } /** - * The rectangle to open a site on, or null when there is nothing usable to - * open on: no site yet, nothing stored for it, or a view that no longer meets - * the floor. A site remeasured or re-anchored under a stored view leaves one - * of those, and restoring it would open the map on nothing; the whole site is - * the better answer. + * The view to open a site on, or null when there is nothing usable to open + * on: no site yet, nothing stored for it, or a centre off the floor. A site + * remeasured or re-anchored under a stored view leaves one of those, and + * restoring it would open the map on nothing; the whole site is the better + * answer. */ export function viewFor( views: SavedViews, site: string | null | undefined, viewport: Area, -): Area | null { +): ViewCentre | null { if (!site) return null; - const rect = views[site]; - if (!usable(rect)) return null; - const meetsFloor = - rect.x < viewport.x + viewport.w && - rect.x + rect.w > viewport.x && - rect.y < viewport.y + viewport.h && - rect.y + rect.h > viewport.y; - return meetsFloor ? { ...rect } : null; + const view = views[site]; + if (!usable(view)) return null; + const onFloor = + view.x >= viewport.x && + view.x <= viewport.x + viewport.w && + view.y >= viewport.y && + view.y <= viewport.y + viewport.h; + return onFloor ? { ...view } : null; } diff --git a/dotbot/console-web/src/shortcuts.test.ts b/dotbot/console-web/src/shortcuts.test.ts index 3a049cfe..f9916f20 100644 --- a/dotbot/console-web/src/shortcuts.test.ts +++ b/dotbot/console-web/src/shortcuts.test.ts @@ -90,6 +90,19 @@ describe("the action keys", () => { expect(pressed(press("h"), ACTION_KEY.go)).toBe(false); }); + it("take a symbol typed with Option or AltGr, but not under Ctrl or Cmd", () => { + const altGraph = { getModifierState: (k: string) => k === "AltGraph" }; + expect(pressed(press("["), ACTION_KEY.leftPanel)).toBe(true); + expect(pressed(press("]"), ACTION_KEY.rightPanel)).toBe(true); + expect(pressed(press("[", { altKey: true }), ACTION_KEY.leftPanel)).toBe(true); + expect( + pressed(press("[", { ctrlKey: true, altKey: true, ...altGraph }), ACTION_KEY.leftPanel), + ).toBe(true); + expect(pressed(press("[", { ctrlKey: true }), ACTION_KEY.leftPanel)).toBe(false); + expect(pressed(press("[", { metaKey: true }), ACTION_KEY.leftPanel)).toBe(false); + expect(pressed(press("]"), ACTION_KEY.leftPanel)).toBe(false); + }); + it("are keys to the panel, not gestures or modifiers, and every one has a row", () => { const named = new Set( SHORTCUT_GROUPS.flatMap((g) => g.rows.flatMap((r) => r.keys.filter(isKey))), diff --git a/dotbot/console-web/src/shortcuts.ts b/dotbot/console-web/src/shortcuts.ts index b5da10f4..6bc5551f 100644 --- a/dotbot/console-web/src/shortcuts.ts +++ b/dotbot/console-web/src/shortcuts.ts @@ -48,16 +48,31 @@ export function roleOf(e: ModifierKeys): MapRole | null { /** The keys that fire an action on their own: one key, one action. */ export const ACTION_KEY = { go: "G", + leftPanel: "[", + rightPanel: "]", } as const; export type ActionKey = (typeof ACTION_KEY)[keyof typeof ACTION_KEY]; -/** Whether a key press is `key` on its own: either case, no modifier held. */ +/** + * Whether a key press is `key` on its own: a letter in either case with no + * modifier held. A symbol may need Option or AltGr to type on some layouts + * (`[` on AZERTY), so for a symbol only Ctrl or Cmd without AltGr rule it out. + */ export function pressed( - e: { key: string; ctrlKey: boolean; metaKey: boolean; altKey: boolean }, + e: { + key: string; + ctrlKey: boolean; + metaKey: boolean; + altKey: boolean; + getModifierState?: (key: string) => boolean; + }, key: ActionKey, ): boolean { - return !e.ctrlKey && !e.metaKey && !e.altKey && e.key.toUpperCase() === key; + if (e.key.toUpperCase() !== key) return false; + if (/^[A-Z]$/.test(key)) return !e.ctrlKey && !e.metaKey && !e.altKey; + const altGraph = e.getModifierState?.("AltGraph") ?? false; + return !e.metaKey && (!e.ctrlKey || altGraph); } /** The key that opens and closes the shortcuts panel. */ @@ -119,6 +134,13 @@ export const SHORTCUT_GROUPS: ShortcutGroup[] = [ }, ], }, + { + surface: "Panels", + rows: [ + { keys: [ACTION_KEY.leftPanel], does: "Collapse or expand the left panel" }, + { keys: [ACTION_KEY.rightPanel], does: "Collapse or expand the right panel" }, + ], + }, ]; /** The modifier's name on this platform's keyboard. */ diff --git a/dotbot/console-web/src/shortcutsPanel.test.tsx b/dotbot/console-web/src/shortcutsPanel.test.tsx index 5c57dd59..c269a64c 100644 --- a/dotbot/console-web/src/shortcutsPanel.test.tsx +++ b/dotbot/console-web/src/shortcutsPanel.test.tsx @@ -23,7 +23,7 @@ const bot: UnifiedBot = { led: null, deviceType: "DotBotV3", application: 0, - isDotBot: true, + pose: null, drivable: true, nav: "drive", waypoints: [], diff --git a/dotbot/console-web/src/tokens.css b/dotbot/console-web/src/tokens.css index 7d8896c0..7f079a20 100644 --- a/dotbot/console-web/src/tokens.css +++ b/dotbot/console-web/src/tokens.css @@ -12,6 +12,8 @@ --accent: #e4032e; --hairline: rgba(255, 255, 255, 0.07); --tyre: #4a5160; + /* Under a robot's dashed footprint ring, whatever the floor or picture. */ + --footprint-casing: rgba(0, 0, 0, 0.55); /* State colors are shared across themes: a color always means the same thing. */ --s-Running: #22c55e; @@ -45,6 +47,7 @@ --muted: #5b6577; --hairline: rgba(0, 0, 0, 0.07); --tyre: #5a6270; + --footprint-casing: rgba(255, 255, 255, 0.9); } * { diff --git a/dotbot/console-web/src/types.ts b/dotbot/console-web/src/types.ts index b9aaff27..77600517 100644 --- a/dotbot/console-web/src/types.ts +++ b/dotbot/console-web/src/types.ts @@ -44,6 +44,36 @@ export interface RgbLed { blue: number; } +// How the heading a body pose was built from was made: "travel" is the +// bearing between two fixes, "ekf" the robot's own estimate, and "none" means +// the robot reported no heading at all, so the pose's own heading is a +// placeholder and its body must not be drawn. +export type HeadingSource = "none" | "travel" | "ekf"; + +// The robot's body, as the controller expands one photodiode fix into it. Every +// point is frame millimetres, the same frame as an LH2 position: `centre` is +// the board outline's centre, `nose` the middle of its front edge, and +// `outline` the board path itself, already rotated to the heading. +export interface BotPose { + heading_deg: number; + heading_source: HeadingSource; + /** Where the pose places the LH2 photodiode. */ + photodiode: LH2Position; + axle: LH2Position; + centre: LH2Position; + nose: LH2Position; + led: LH2Position; + outline: LH2Position[]; + /** Each driven wheel in plan view, as a rectangle. */ + wheels: LH2Position[][]; + /** Radius about the photodiode holding the whole body, tyres included, in any heading. */ + reach_mm: number; + /** Radius about the photodiode the board covers in any heading. */ + core_mm: number; + /** The robot's plan-view size. */ + envelope_mm: number; +} + export interface PyDotBot { address: string; application: number; // ApplicationType: 0 = DotBot @@ -51,6 +81,7 @@ export interface PyDotBot { mode?: number; // ControlModeType: 0 MANUAL, 1 AUTO (navigating waypoints) direction?: number; lh2_position?: LH2Position; + pose?: BotPose; position_history?: LH2Position[]; waypoints?: LH2Position[]; waypoints_threshold?: number; @@ -92,6 +123,8 @@ export interface CameraPose { photodiode_mm: [number, number]; nose_mm: [number, number]; outline_mm: number[][]; + /** Each tyre in plan view, as a rectangle; empty on an older host. */ + wheels_mm?: number[][][]; heading_deg: number; heading_atan2_deg: number; green_flare: number; @@ -244,13 +277,16 @@ export interface UnifiedBot { id: string; // hex address, the join key state: BotState | null; // null: swarmit does not know this bot (no sandbox) link: LinkState; - position: LH2Position | null; // arena mm + position: LH2Position | null; // the LH2 photodiode, arena mm heading: number | null; // degrees + // The body around that photodiode fix, as the controller expanded it. Null + // for a bot with no fix, and for one whose position comes from swarmit, + // which reports a point and no heading. + pose: BotPose | null; battery: number; // volts led: RgbLed | null; deviceType: string; application: number; - isDotBot: boolean; // drawn as the robot outline drivable: boolean; // a DBP-speaking image is running (= known to PyDotBot and active) nav: "drive" | "auto"; // auto = navigating waypoints (firmware AUTO mode) waypoints: LH2Position[]; // active mission (as reported by the controller) diff --git a/dotbot/console-web/src/useFleet.test.ts b/dotbot/console-web/src/useFleet.test.ts index e4d17a9f..eb08da87 100644 --- a/dotbot/console-web/src/useFleet.test.ts +++ b/dotbot/console-web/src/useFleet.test.ts @@ -1,16 +1,31 @@ import { describe, expect, it } from "vitest"; -import { CameraDetection, PyDotBot, SwarmitNode } from "./types"; +import { BotPose, CameraDetection, PyDotBot, SwarmitNode } from "./types"; import { deriveLink, derivePose, deriveState, - isDotBot, merge, severityOf, withDetection, } from "./useFleet"; +const pose = (over: Partial = {}): BotPose => ({ + heading_deg: 90, + heading_source: "travel", + photodiode: { x: 0, y: 0 }, + axle: { x: 0, y: 0 }, + centre: { x: 0, y: 0 }, + nose: { x: 0, y: 0 }, + led: { x: 0, y: 0 }, + outline: [], + wheels: [], + reach_mm: 0, + core_mm: 0, + envelope_mm: 0, + ...over, +}); + const py = (over: Partial = {}): PyDotBot => ({ address: "badcafe111111111", application: 0, @@ -136,6 +151,20 @@ describe("merge", () => { expect(c.drivable).toBe(false); }); + it("knows the LED colour only while the app runs", () => { + const red = { red: 255, green: 0, blue: 0 }; + const led = (status: string | null) => + merge( + { aaaa: py({ address: "aaaa", rgb_led: red }) }, + status === null ? {} : { aaaa: sw({ status }) }, + )[0].led; + expect(led("Running")).toEqual(red); + expect(led(null)).toEqual(red); + for (const status of ["Bootloader", "Stopping", "Programming", "Resetting"]) { + expect(led(status)).toBeNull(); + } + }); + it("maps firmware AUTO mode to nav=auto", () => { const [a] = merge({ a: py({ address: "a", mode: 1 }) }, {}); expect(a.nav).toBe("auto"); @@ -205,29 +234,86 @@ describe("withDetection (one camera's latest view of its own area)", () => { }); }); -describe("isDotBot (drawn as the robot)", () => { - it("takes swarmit's device type alone, as in the bootloader", () => { - expect(isDotBot(undefined, sw({ device: "DotBotV3", status: "Bootloader" }))).toBe(true); - expect(isDotBot(undefined, sw({ device: "DotBotV2" }))).toBe(true); +describe("the body the controller expanded the fix into", () => { + const at = { x: 1500, y: 300 }; + + it("rides with the controller's position", () => { + const p = pose(); + expect(derivePose(py({ lh2_position: at, pose: p }), sw(), "active").pose).toBe(p); + }); + + // The host's pose for a device type, photodiode at the origin. + const V3_AT_ORIGIN = pose({ + heading_source: "none", + heading_deg: 0, + centre: { x: 0, y: -29 }, + reach_mm: 89, + core_mm: 18, + envelope_mm: 95, + }); + + it("is the device type's pose, moved onto swarmit's position", () => { + const heard = py({ lh2_position: at, pose: pose({ photodiode: at }) }); + const placed = derivePose(heard, sw(), "lost", { DotBotV3: V3_AT_ORIGIN }).pose!; + expect(placed.heading_source).toBe("none"); + expect(placed.photodiode).toEqual({ x: 100, y: 200 }); + expect(placed.centre).toEqual({ x: 100, y: 171 }); + expect(placed).toMatchObject({ reach_mm: 89, core_mm: 18, envelope_mm: 95 }); + }); + + it("is sized even for a bot the controller has never heard", () => { + const placed = derivePose(undefined, sw({ status: "Bootloader" }), "unknown", { + DotBotV3: V3_AT_ORIGIN, + }).pose; + expect(placed).toMatchObject({ heading_source: "none", photodiode: { x: 100, y: 200 } }); + }); + + it("is a bare point for a device type the host has no record of", () => { + const poses = { DotBotV3: V3_AT_ORIGIN }; + expect(derivePose(py({ pose: pose() }), sw({ device: "SailBot" }), "lost", poses).pose).toBeNull(); + expect(derivePose(py({ pose: pose() }), sw(), "lost").pose).toBeNull(); + }); + + it("never keeps the controller's heading for a bot out of its app", () => { + // The link lingers for up to a minute after the app stops; the heading + // it last reported is stale by then. + const heard = py({ lh2_position: at, direction: 90, pose: pose() }); + for (const status of ["Bootloader", "Stopping", "Programming"]) { + const got = derivePose(heard, sw({ status }), "active", { DotBotV3: V3_AT_ORIGIN }); + expect(got.heading).toBeNull(); + expect(got.pose?.heading_source).toBe("none"); + expect(got.position).toEqual({ x: 100, y: 200 }); + const unplaced = derivePose(heard, sw({ status, pos_x: 0, pos_y: 0 }), "active"); + expect(unplaced.position).toEqual(at); + expect(unplaced.heading).toBeNull(); + expect(unplaced.pose?.heading_source).toBe("none"); + } }); - it("takes the controller's application alone, whatever image advertises it", () => { - expect(isDotBot(py({ application: 0 }), undefined)).toBe(true); - expect(isDotBot(py({ application: 0 }), sw({ device: "Unknown" }))).toBe(true); + it("is dropped with the position it belongs to", () => { + expect(derivePose(py({ pose: pose() }), sw({ pos_x: 0, pos_y: 0 }), "active").pose).toBeNull(); }); - it("is not a DotBot when neither signal says so", () => { - expect(isDotBot(py({ application: 1 }), sw({ device: "nRF5340DK" }))).toBe(false); - expect(isDotBot(undefined, sw({ device: "Unknown" }))).toBe(false); + it("reaches the merged bot", () => { + const p = pose(); + const [b] = merge( + { aaaa: py({ address: "aaaa", lh2_position: at, pose: p }) }, + { aaaa: sw({ status: "Running" }) }, + ); + expect(b.pose).toBe(p); }); - it("keeps a headingless DotBot a robot on the merged bot", () => { + it("is carried for a bot that reported no heading, flagged as such", () => { + // The controller always expands a valid fix; the flag is what says the + // orientation is unknown, and the console keys on that rather than on the + // field being absent. + const p = pose({ heading_source: "none", heading_deg: 0 }); const [b] = merge( - { aaaa: py({ address: "aaaa", direction: -1000 }) }, + { aaaa: py({ address: "aaaa", direction: -1000, lh2_position: at, pose: p }) }, { aaaa: sw({ status: "Running" }) }, ); expect(b.heading).toBeNull(); - expect(b.isDotBot).toBe(true); + expect(b.pose?.heading_source).toBe("none"); }); }); @@ -250,6 +336,16 @@ describe("derivePose (whose pose is live)", () => { { aaaa: sw({ status: "Bootloader", pos_x: 700, pos_y: 800 }) }, ); expect(b.position).toEqual({ x: 700, y: 800 }); + expect(b.pose).toBeNull(); + }); + + it("sizes a bootloader bot from its device type", () => { + const [b] = merge( + { aaaa: py({ address: "aaaa", status: 1, lh2_position: stale, pose: pose({ photodiode: stale }) }) }, + { aaaa: sw({ status: "Bootloader", pos_x: 700, pos_y: 800 }) }, + { DotBotV3: pose({ heading_source: "none", reach_mm: 89 }) }, + ); + expect(b.pose).toMatchObject({ heading_source: "none", photodiode: { x: 700, y: 800 }, reach_mm: 89 }); }); it("keeps the controller's stale position when swarmit has never located the bot", () => { @@ -259,10 +355,18 @@ describe("derivePose (whose pose is live)", () => { it("takes the controller's heading only with its position", () => { const heard = py({ lh2_position: stale, direction: 90 }); expect(derivePose(heard, sw(), "active").heading).toBe(90); - expect(derivePose(heard, sw(), "lost")).toEqual({ position: { x: 100, y: 200 }, heading: null }); + expect(derivePose(heard, sw(), "lost")).toEqual({ + position: { x: 100, y: 200 }, + heading: null, + pose: null, + }); expect(derivePose(heard, sw({ pos_x: 0, pos_y: 0 }), "lost").heading).toBe(90); const unplaced = py({ direction: 90 }); - expect(derivePose(unplaced, sw({ pos_x: 0, pos_y: 0 }), "active")).toEqual({ position: null, heading: null }); + expect(derivePose(unplaced, sw({ pos_x: 0, pos_y: 0 }), "active")).toEqual({ + position: null, + heading: null, + pose: null, + }); }); it("does not draw swarmit's unlocated origin", () => { diff --git a/dotbot/console-web/src/useFleet.ts b/dotbot/console-web/src/useFleet.ts index 055d7b11..32d2ffea 100644 --- a/dotbot/console-web/src/useFleet.ts +++ b/dotbot/console-web/src/useFleet.ts @@ -4,6 +4,7 @@ import { controllerWsUrl, fetchCalibrationSession, fetchCameras, + fetchDevicePoses, fetchDotBots, fetchSite, fetchSwarmitStatus, @@ -11,6 +12,7 @@ import { import { AREA_FALLBACK, siteViewport } from "./frame"; import { Area, + BotPose, BotState, CalibrationSession, LH2Position, @@ -50,38 +52,65 @@ export function deriveLink(py: PyDotBot | undefined): LinkState { return py.status === 2 ? "lost" : "inactive"; } -// The controller's pose while the link is active, else swarmit's position if -// it has located the bot, else the controller's last pose. swarmit reports -// (0, 0) for a bot it has never located, and no heading at all. +// A device type's pose moved onto `at`, which is where its photodiode goes. +function poseAt(pose: BotPose, at: LH2Position): BotPose { + const dx = at.x - pose.photodiode.x; + const dy = at.y - pose.photodiode.y; + const move = (p: LH2Position): LH2Position => ({ x: p.x + dx, y: p.y + dy }); + return { + ...pose, + photodiode: move(pose.photodiode), + axle: move(pose.axle), + centre: move(pose.centre), + nose: move(pose.nose), + led: move(pose.led), + outline: pose.outline.map(move), + wheels: pose.wheels.map((wheel) => wheel.map(move)), + }; +} + +// The controller's pose while it hears the app running, else swarmit's +// position if it has located the bot, else the controller's last pose. Out of +// its app a robot computes no heading, so it is placed headingless: from +// swarmit, sized by the pose the host gave its device type, or a bare point +// for a type the host has no record of. swarmit reports (0, 0) for a bot it +// has never located. export function derivePose( py: PyDotBot | undefined, sw: SwarmitNode | undefined, link: LinkState, -): { position: LH2Position | null; heading: number | null } { + devicePoses: Record = {}, +): { + position: LH2Position | null; + heading: number | null; + pose: BotPose | null; +} { + const inApp = !sw || sw.status === "Running"; const pyHeading = py?.direction !== undefined && py.direction !== -1000 ? py.direction : null; - if (link === "active" && py?.lh2_position) { - return { position: py.lh2_position, heading: pyHeading }; + const pyPose = py?.pose ?? null; + if (inApp && link === "active" && py?.lh2_position) { + return { position: py.lh2_position, heading: pyHeading, pose: pyPose }; } if (sw && (sw.pos_x !== 0 || sw.pos_y !== 0)) { - return { position: { x: sw.pos_x, y: sw.pos_y }, heading: null }; + const at = { x: sw.pos_x, y: sw.pos_y }; + const devicePose = devicePoses[sw.device]; + return { position: at, heading: null, pose: devicePose ? poseAt(devicePose, at) : null }; } const position = py?.lh2_position ?? null; - return { position, heading: position ? pyHeading : null }; -} - -// Either signal is enough: swarmit knows the board even in its bootloader, -// and the controller knows the firmware even on a board swarmit cannot name. -export function isDotBot( - py: PyDotBot | undefined, - sw: SwarmitNode | undefined, -): boolean { - return (sw?.device.startsWith("DotBot") ?? false) || py?.application === 0; + if (!position) return { position: null, heading: null, pose: null }; + if (inApp) return { position, heading: pyHeading, pose: pyPose }; + return { + position, + heading: null, + pose: pyPose ? { ...pyPose, heading_source: "none" } : null, + }; } export function merge( pyBots: Record, swNodes: Record, + devicePoses: Record = {}, ): UnifiedBot[] { const ids = new Set([...Object.keys(pyBots), ...Object.keys(swNodes)]); const out: UnifiedBot[] = []; @@ -90,18 +119,20 @@ export function merge( const sw = swNodes[id]; const state = deriveState(sw); const link = deriveLink(py); - const { position, heading } = derivePose(py, sw, link); + const { position, heading, pose } = derivePose(py, sw, link, devicePoses); out.push({ id, state, link, position, heading, + pose, battery: py?.battery ?? (sw ? sw.battery / 1000 : 0), - led: py?.rgb_led ?? null, + // The colour the controller last commanded, which the LED shows only + // while the app runs: out of it, the bootloader drives the LED itself. + led: state === null || state === "Running" ? py?.rgb_led ?? null : null, deviceType: sw?.device ?? "DotBot", application: py?.application ?? 0, - isDotBot: isDotBot(py, sw), // Drivable = a DBP-speaking image is running. The control plane must be // hearing the bot, and either its sandbox is Running or it has no // sandbox at all (a bare-mode bot swarmit does not manage). @@ -140,6 +171,7 @@ export function useFleet(): { } { const pyRef = useRef>({}); const swRef = useRef>({}); + const devicePosesRef = useRef>({}); const [bots, setBots] = useState([]); const [site, setSite] = useState(null); const [cameras, setCameras] = useState([]); @@ -150,7 +182,7 @@ export function useFleet(): { const [wsUp, setWsUp] = useState(false); const rebuild = useCallback(() => { - setBots(merge(pyRef.current, swRef.current)); + setBots(merge(pyRef.current, swRef.current, devicePosesRef.current)); }, []); const reloadDotBots = useCallback(async () => { @@ -166,6 +198,10 @@ export function useFleet(): { // Initial data, and the site the map is drawn over. useEffect(() => { reloadDotBots(); + fetchDevicePoses().then((poses) => { + devicePosesRef.current = poses; + rebuild(); + }); fetchSite() .then(setSite) .catch(() => {}); @@ -178,7 +214,7 @@ export function useFleet(): { fetchCalibrationSession() .then(setSession) .catch(() => {}); - }, [reloadDotBots]); + }, [reloadDotBots, rebuild]); // Live updates over the controller WebSocket. useEffect(() => { @@ -219,6 +255,7 @@ export function useFleet(): { } const d = msg.data; if (d.direction !== undefined) bot.direction = d.direction; + if (d.pose !== undefined) bot.pose = d.pose; if (d.battery !== undefined) bot.battery = d.battery; if (d.rgb_led !== undefined) bot.rgb_led = d.rgb_led; if (d.lh2_position !== undefined) { diff --git a/dotbot/console-web/src/zoom.test.ts b/dotbot/console-web/src/zoom.test.ts index 91eb5b4f..fcd4decd 100644 --- a/dotbot/console-web/src/zoom.test.ts +++ b/dotbot/console-web/src/zoom.test.ts @@ -1,16 +1,11 @@ import { describe, expect, it } from "vitest"; -import { - BOT_FOOTPRINT_MM, - GLYPH_DETAIL_PX, - botFootprintPx, - glyphLevel, -} from "./BotGlyph"; -import { areaToFraction } from "./frame"; +import { GLYPH_DETAIL_PX, botFootprintPx, glyphLevel } from "./BotGlyph"; +import { PAN_MARGIN_MM, areaToFraction, siteView, siteViewport } from "./frame"; import { frameMm, pxPerMm } from "./grid"; import { CANVAS_INSET_PX, - SITE_CAMERA, + FRAME_CAMERA, SITE_ZOOM, ZOOM_MAX_FLOOR, ZOOM_MAX_PX_PER_MM, @@ -19,9 +14,13 @@ import { cameraForArea, cameraForZoom, clampCam, + visibleArea, clampScale, fitScale, padArea, + refitCam, + cameraAtCentre, + centreOfView, scaleForFraction, steppedScale, viewCentre, @@ -45,6 +44,10 @@ const C405: Site = { }; // The site extent plus the 2 m margin, which is what the map draws. const VIEWPORT: Area = { x: -2000, y: -2000, w: 6000, h: 8000 }; +// The span a v3 body reports, for the questions the zoom ladder asks about how +// big a robot lands on screen. The console never computes this; the controller +// ships it inside each body pose. +const V3_SPAN_MM = 95; const GEOM = viewGeom(900, 600, VIEWPORT); const MAX = zoomMax(C405, VIEWPORT, GEOM); @@ -115,10 +118,10 @@ describe("panning", () => { }); describe("the zoom range", () => { - it("starts at the whole site, which is the map's own camera", () => { + it("starts at the whole viewport, which is everywhere the pan reaches", () => { expect(ZOOM_MIN).toBe(1); - expect(SITE_CAMERA.scale).toBe(ZOOM_MIN); - expect(zoomFraction(SITE_CAMERA.scale, 11.4)).toBe(0); + expect(FRAME_CAMERA.scale).toBe(ZOOM_MIN); + expect(zoomFraction(FRAME_CAMERA.scale, 11.4)).toBe(0); }); it("holds a scale between the site and the ceiling", () => { @@ -144,6 +147,12 @@ describe("the zoom range", () => { expect(steppedScale(11.4, 1, 11.4)).toBe(11.4); }); + it("never zooms out on a press in from above the ceiling", () => { + expect(steppedScale(20, 1, 11.4)).toBe(20); + expect(steppedScale(20, 0, 11.4)).toBe(20); + expect(steppedScale(20, -1, 11.4)).toBeLessThan(20); + }); + it("comes back to the scale it left, in and out again", () => { let scale = 2; for (let i = 0; i < 3; i += 1) scale = steppedScale(scale, 1, 40); @@ -260,8 +269,15 @@ describe("the named zooms", () => { }); describe("zooming to the site", () => { - it("is the map's own default view", () => { - expect(cameraForZoom(SITE_ZOOM, C405, VIEWPORT, GEOM)).toEqual(SITE_CAMERA); + it("fits the site with its modest margin, not the whole viewport", () => { + const cam = cameraForZoom(SITE_ZOOM, C405, VIEWPORT, GEOM)!; + const seen = visibleArea(cam, VIEWPORT, GEOM); + const framed = siteView(VIEWPORT); + // The tight axis shows exactly the framed rectangle; the other shows more. + expect(seen.h).toBeCloseTo(framed.h, 6); + expect(seen.y).toBeCloseTo(framed.y, 6); + expect(seen.w).toBeGreaterThanOrEqual(framed.w - 1e-6); + expect(seen.x + seen.w / 2).toBeCloseTo(framed.x + framed.w / 2, 6); }); }); @@ -353,8 +369,8 @@ describe("the zoom ceiling", () => { ); // Which is a 95 mm robot at very nearly its own size in pixels. expect( - BOT_FOOTPRINT_MM * pxPerMm("x", VIEWPORT, GEOM, { scale: max, tx: 0, ty: 0 }), - ).toBeGreaterThanOrEqual(BOT_FOOTPRINT_MM - 1e-6); + V3_SPAN_MM * pxPerMm("x", VIEWPORT, GEOM, { scale: max, tx: 0, ty: 0 }), + ).toBeGreaterThanOrEqual(V3_SPAN_MM - 1e-6); }); it("still clears an area that asks for more than that", () => { @@ -401,6 +417,7 @@ describe("how far in the glyph ladder reaches", () => { tx: 0, ty: 0, }), + V3_SPAN_MM, ), ); }; @@ -447,3 +464,159 @@ describe("the ?zoom= preset", () => { expect(zoomFromSearch("?view=map", C405)).toBeNull(); }); }); + +describe("the whole-site view", () => { + // The map canvas a 1600 x 950 window leaves once both panes are open. + const arena: Site = { ...C405, extent_mm: [2000, 2000], areas: [ARENA] }; + const viewport = siteViewport(arena, ARENA); + const geom = viewGeom(936, 740, viewport); + const siteCam = cameraForZoom(SITE_ZOOM, arena, viewport, geom)!; + const sitePerMm = pxPerMm("x", viewport, geom, siteCam); + + it("is mostly site: the arena fills most of the canvas", () => { + expect((ARENA.h * sitePerMm) / geom.h).toBeGreaterThan(0.75); + }); + + it("shows a 2 x 2 m arena's robots as full outlines on a normal screen", () => { + expect(glyphLevel(botFootprintPx(sitePerMm, V3_SPAN_MM), 12)).toBe("detail"); + }); +}); + +describe("panning past the site", () => { + const arena: Site = { ...C405, extent_mm: [2000, 2000], areas: [ARENA] }; + const viewport = siteViewport(arena, ARENA); + const geom = viewGeom(936, 740, viewport); + const siteCam = cameraForZoom(SITE_ZOOM, arena, viewport, geom)!; + const panned = (tx: number, ty: number) => + visibleArea(clampCam({ ...siteCam, tx, ty }, geom), viewport, geom); + + it("reaches 2 m past the site on every side and no further", () => { + const tl = panned(1e9, 1e9); + const br = panned(-1e9, -1e9); + expect(tl.x).toBeCloseTo(-PAN_MARGIN_MM, 6); + expect(tl.y).toBeCloseTo(-PAN_MARGIN_MM, 6); + expect(br.x + br.w).toBeCloseTo(2000 + PAN_MARGIN_MM, 6); + expect(br.y + br.h).toBeCloseTo(2000 + PAN_MARGIN_MM, 6); + }); + + it("brings a robot 1.5 m off the site into view", () => { + const bot = { x: 2000 + 1500, y: 1000 }; + const view = panned(-1e9, 0); + expect(bot.x).toBeGreaterThan(view.x); + expect(bot.x).toBeLessThan(view.x + view.w); + expect(bot.y).toBeGreaterThan(view.y); + expect(bot.y).toBeLessThan(view.y + view.h); + }); + + it("shows the whole pannable area fully zoomed out", () => { + const all = visibleArea(FRAME_CAMERA, viewport, geom); + expect(all.x).toBeLessThanOrEqual(-PAN_MARGIN_MM); + expect(all.y).toBeLessThanOrEqual(-PAN_MARGIN_MM); + expect(all.x + all.w).toBeGreaterThanOrEqual(2000 + PAN_MARGIN_MM); + expect(all.y + all.h).toBeGreaterThanOrEqual(2000 + PAN_MARGIN_MM); + }); +}); + +describe("a panel toggle, which only changes the canvas width", () => { + const viewport = siteViewport(C405, ARENA); + // A map squeezed between both panes, narrower than it is tall, and the same + // map with one pane collapsed: the drawn box goes from width-limited to + // height-limited across the toggle, so it changes size. + const narrow = viewGeom(420, 860, viewport); + const wide = viewGeom(760, 860, viewport); + const centreOf = (cam: Camera, geom: ReturnType) => { + const v = visibleArea(cam, viewport, geom); + return { x: v.x + v.w / 2, y: v.y + v.h / 2 }; + }; + + it("crosses the frame's aspect ratio in this fixture", () => { + expect((narrow.w - CANVAS_INSET_PX) / narrow.boxW).toBeCloseTo(1, 6); + expect(wide.boxH).toBeCloseTo(wide.h - CANVAS_INSET_PX, 6); + expect(wide.boxW).toBeGreaterThan(narrow.boxW); + }); + + for (const [name, zoom] of [ + ["the site", SITE_ZOOM], + ["an area", "arena"], + ] as const) { + for (const [dir, from, to] of [ + ["opened", narrow, wide], + ["closed", wide, narrow], + ] as const) { + it(`keeps the scale, the height and the centre of ${name} when a pane is ${dir}`, () => { + const cam = cameraForZoom(zoom, C405, viewport, from)!; + const next = refitCam(cam, from, to); + expect(pxPerMm("x", viewport, to, next)).toBeCloseTo(pxPerMm("x", viewport, from, cam), 9); + expect(next.ty).toBeCloseTo(cam.ty, 9); + const c0 = centreOf(cam, from); + const c1 = centreOf(next, to); + expect(c1.x).toBeCloseTo(c0.x, 6); + expect(c1.y).toBeCloseTo(c0.y, 6); + }); + } + } + + it("keeps a panned view inside the new canvas's pan clamp", () => { + const cam = clampCam({ scale: 6, tx: 99999, ty: -99999 }, wide); + const next = refitCam(cam, wide, narrow); + expect(next).toEqual(clampCam(next, narrow)); + }); + + it("does not pull a view left past the far end back in on a step outward", () => { + const out = refitCam(FRAME_CAMERA, narrow, wide); + expect(out.scale).toBeLessThan(ZOOM_MIN); + expect(steppedScale(out.scale, -1, 10)).toBe(out.scale); + expect(steppedScale(out.scale, 1, 10)).toBeGreaterThan(out.scale); + }); +}); + +describe("a view stated as its centre and scale", () => { + const viewport = siteViewport(C405, ARENA); + const geoms = [viewGeom(900, 600, viewport), viewGeom(600, 900, viewport), viewGeom(420, 860, viewport)]; + const maxOf = (geom: ReturnType) => zoomMax(C405, viewport, geom); + + it("takes a camera to its centre and scale and back again in the same canvas", () => { + for (const geom of geoms) { + const cam = cameraForZoom("arena", C405, viewport, geom)!; + const back = cameraAtCentre(centreOfView(cam, viewport, geom), viewport, geom, maxOf(geom)); + expect(back.scale).toBeCloseTo(cam.scale, 9); + expect(back.tx).toBeCloseTo(cam.tx, 6); + expect(back.ty).toBeCloseTo(cam.ty, 6); + } + }); + + it("shows the same centre at the same scale in a canvas of any shape", () => { + const from = geoms[0]; + const view = centreOfView(cameraForZoom("arena", C405, viewport, from)!, viewport, from); + for (const to of geoms) { + const got = centreOfView(cameraAtCentre(view, viewport, to, maxOf(to)), viewport, to); + expect(got.pxPerMm).toBeCloseTo(view.pxPerMm, 9); + expect(got.x).toBeCloseTo(view.x, 6); + expect(got.y).toBeCloseTo(view.y, 6); + } + }); + + it("agrees with a panel toggle's refit", () => { + const [wide, , narrow] = geoms; + const cam = cameraForZoom("arena", C405, viewport, wide)!; + const refit = refitCam(cam, wide, narrow); + const placed = cameraAtCentre(centreOfView(cam, viewport, wide), viewport, narrow, maxOf(narrow)); + expect(placed.scale).toBeCloseTo(refit.scale, 9); + expect(placed.tx).toBeCloseTo(refit.tx, 6); + expect(placed.ty).toBeCloseTo(refit.ty, 6); + }); + + it("holds a view saved in a larger window under this canvas's ceiling", () => { + const small = viewGeom(240, 200, viewport); + const top = maxOf(small); + const pxPerMm = (top * 4 * small.boxW) / viewport.w; + const cam = cameraAtCentre({ x: 1000, y: 1000, pxPerMm }, viewport, small, top); + expect(cam.scale).toBeCloseTo(top, 9); + }); + + it("holds a centre near the edge inside the pan clamp", () => { + const geom = geoms[2]; + const cam = cameraAtCentre({ x: viewport.x, y: viewport.y, pxPerMm: 1 }, viewport, geom, maxOf(geom)); + expect(cam).toEqual(clampCam(cam, geom)); + }); +}); diff --git a/dotbot/console-web/src/zoom.ts b/dotbot/console-web/src/zoom.ts index 70bed8ae..c05f7f70 100644 --- a/dotbot/console-web/src/zoom.ts +++ b/dotbot/console-web/src/zoom.ts @@ -1,4 +1,4 @@ -import { areaToFraction, fractionToArea } from "./frame"; +import { areaToFraction, fractionToArea, siteView } from "./frame"; import type { Area, Site } from "./types"; // The viewport: which part of the drawn frame fills the canvas. @@ -66,14 +66,16 @@ export const ZOOM_MAX_PX_PER_MM = 1; /** The zoom that shows the whole site: the map's own default. */ export const SITE_ZOOM = "site"; -export const SITE_CAMERA: Camera = { scale: 1, tx: 0, ty: 0 }; + +/** The identity camera: the whole drawn viewport in the box. */ +export const FRAME_CAMERA: Camera = { scale: 1, tx: 0, ty: 0 }; /** How much of a rectangle's own size is left around it when zoomed to. */ export const ZOOM_PAD = 0.15; /** - * The far end of the range: the whole site in view, which is also the map's - * own opening camera. Zooming out past it would only add margin. + * The far end of the range: the whole viewport in view, which is everywhere + * the pan can reach. Zooming out past it would only add empty canvas. */ export const ZOOM_MIN = 1; @@ -110,17 +112,25 @@ export function clampScale(scale: number, max: number): number { return Math.min(top, Math.max(ZOOM_MIN, scale)); } -/** The scale `delta` presses away, held inside the range at both ends. */ +/** + * The scale `delta` presses away, held inside the range at both ends. A scale + * already past either end, which only a canvas resize leaves, is not pulled + * back in by a step further out of the range: a step in never zooms out, and + * a step out never zooms in. + */ export function steppedScale( scale: number, delta: number, max: number, ): number { + if (scale < ZOOM_MIN && delta <= 0) return scale; + const top = Number.isFinite(max) ? Math.max(ZOOM_MIN, max) : ZOOM_MIN; + if (scale > top && delta >= 0) return scale; return clampScale(scale * ZOOM_STEP ** delta, max); } /** - * Where a scale sits in the range: 0 at the whole site, 1 at the ceiling. + * Where a scale sits in the range: 0 fully zoomed out, 1 at the ceiling. * Measured in ratios rather than differences, so the same travel along a * slider is the same magnification wherever the handle already is. */ @@ -141,12 +151,18 @@ export function scaleForFraction(fraction: number, max: number): number { return ZOOM_MIN * (top / ZOOM_MIN) ** f; } -// v1 clampPan: keep the arena reachable, never fling it off-screen. +/** + * The camera held to the viewport: the pan reaches as far past the site as + * the viewport runs, and no further. + */ export function clampCam(cam: Camera, geom: ViewGeom): Camera { + // A box much wider than the canvas pans until its edge meets the canvas + // edge. Up to that point it keeps the slack the letterbox gave it at the + // far end of the range, so zooming about an edge never snaps it inward. const padX = Math.max(0, (geom.w - geom.boxW) / 2); const padY = Math.max(0, (geom.h - geom.boxH) / 2); - const mx = Math.max(0, (geom.boxW * cam.scale - geom.w) / 2) + padX; - const my = Math.max(0, (geom.boxH * cam.scale - geom.h) / 2) + padY; + const mx = Math.max(padX, (geom.boxW * cam.scale - geom.w) / 2); + const my = Math.max(padY, (geom.boxH * cam.scale - geom.h) / 2); return { ...cam, tx: Math.max(-mx, Math.min(mx, cam.tx)), @@ -154,6 +170,25 @@ export function clampCam(cam: Camera, geom: ViewGeom): Camera { }; } +/** + * The camera carried across a canvas resize from `from` to `to`, in the same + * viewport, with the canvas centre moved by `shift` client pixels: the floor + * keeps its pixels per millimetre and every floor point keeps its place on + * screen. The camera is relative to the drawn box, which refits to the + * canvas, so the scale is what compensates. Only the pan clamp of the new + * canvas is applied. + */ +export function refitCam( + cam: Camera, + from: ViewGeom, + to: ViewGeom, + shift: { x: number; y: number } = { x: 0, y: 0 }, +): Camera { + const moved = { ...cam, tx: cam.tx - shift.x, ty: cam.ty - shift.y }; + if (!(from.boxW > 0) || !(to.boxW > 0)) return clampCam(moved, to); + return clampCam({ ...moved, scale: (cam.scale * from.boxW) / to.boxW }, to); +} + /** * The camera at a new scale with one canvas point held where it is. That is * what a zoom is: the thing being looked at stays put and only the scale @@ -247,7 +282,7 @@ export function cameraForArea( ); const wPx = (br.fx - tl.fx) * geom.boxW; const hPx = (br.fy - tl.fy) * geom.boxH; - if (!(wPx > 0) || !(hPx > 0)) return SITE_CAMERA; + if (!(wPx > 0) || !(hPx > 0)) return FRAME_CAMERA; const scale = clampScale(Math.min(geom.w / wPx, geom.h / hPx), max); // Where the target's centre sits in canvas pixels before the camera runs. const cx = (geom.w - geom.boxW) / 2 + ((tl.fx + br.fx) / 2) * geom.boxW; @@ -266,11 +301,6 @@ export function cameraForArea( * The floor the canvas is showing, in frame millimetres: the rectangle * `cameraForArea` takes back to this camera, since a view that fills the * canvas is fitted to the canvas exactly. - * - * This is what a camera means, and the portable half of it. The same - * `{scale, tx, ty}` frames a different patch of floor in a canvas of another - * size, so a view that has to survive a resize - or a move to another screen - * - travels as the rectangle and is fitted again on arrival. */ export function visibleArea(cam: Camera, viewport: Area, geom: ViewGeom): Area { const scale = cam.scale > 0 ? cam.scale : ZOOM_MIN; @@ -290,10 +320,52 @@ export function visibleArea(cam: Camera, viewport: Area, geom: ViewGeom): Area { return { x: tl.x, y: tl.y, w: br.x - tl.x, h: br.y - tl.y }; } +/** The floor point at the canvas centre, and the floor's pixels per millimetre. */ +export interface ViewCentre { + x: number; + y: number; + pxPerMm: number; +} + +/** What a camera shows, stated without the canvas: `cameraAtCentre` back. */ +export function centreOfView(cam: Camera, viewport: Area, geom: ViewGeom): ViewCentre { + const v = visibleArea(cam, viewport, geom); + const scale = cam.scale > 0 ? cam.scale : ZOOM_MIN; + return { + x: v.x + v.w / 2, + y: v.y + v.h / 2, + pxPerMm: viewport.w > 0 ? (geom.boxW * scale) / viewport.w : 0, + }; +} + +/** + * The camera that puts `view`'s floor point at this canvas's centre at its + * pixels per millimetre, held by the pan clamp and by the ceiling `max`. + * Under the ceiling the scale is left as asked, so a canvas of another size + * shows the same floor at the same size. + */ +export function cameraAtCentre( + view: ViewCentre, + viewport: Area, + geom: ViewGeom, + max: number, +): Camera { + if (!(geom.boxW > 0) || !(view.pxPerMm > 0)) return FRAME_CAMERA; + const asked = (view.pxPerMm * viewport.w) / geom.boxW; + const scale = Math.min(asked, Math.max(ZOOM_MIN, max)); + const { fx, fy } = areaToFraction({ x: view.x, y: view.y }, viewport); + const cx = (geom.w - geom.boxW) / 2 + fx * geom.boxW; + const cy = (geom.h - geom.boxH) / 2 + fy * geom.boxH; + return clampCam( + { scale, tx: -(cx - geom.w / 2) * scale, ty: -(cy - geom.h / 2) * scale }, + geom, + ); +} + /** - * The camera one named zoom asks for. The site is the map's default view, so - * it is the identity camera rather than a fit; a name no area answers to - * leaves the camera alone, which is what an unknown `?zoom=` should do. + * The camera one named zoom asks for: the site with its modest margin, or an + * area with its pad. A name no area answers to leaves the camera alone, which + * is what an unknown `?zoom=` should do. */ export function cameraForZoom( name: string, @@ -301,15 +373,11 @@ export function cameraForZoom( viewport: Area, geom: ViewGeom, ): Camera | null { - if (name === SITE_ZOOM) return SITE_CAMERA; + const max = zoomMax(site, viewport, geom); + if (name === SITE_ZOOM) return cameraForArea(siteView(viewport), viewport, geom, max); const area = (site?.areas ?? []).find((a) => a.name === name); if (!area) return null; - return cameraForArea( - padArea(area), - viewport, - geom, - zoomMax(site, viewport, geom), - ); + return cameraForArea(padArea(area), viewport, geom, max); } /** The zoom `?zoom=` asks for, or null when it names nothing this site has. */ diff --git a/dotbot/console-web/src/zoomAgreement.test.tsx b/dotbot/console-web/src/zoomAgreement.test.tsx index 6f7aa57c..200d4fdb 100644 --- a/dotbot/console-web/src/zoomAgreement.test.tsx +++ b/dotbot/console-web/src/zoomAgreement.test.tsx @@ -5,9 +5,11 @@ import { afterEach, beforeEach, describe, expect, it, vi } from "vitest"; import { barLabel, pxPerMm, scaleBar } from "./grid"; import type { Site } from "./types"; import { + SITE_ZOOM, ZOOM_MAX_FLOOR, ZOOM_MIN, ZOOM_STEP, + cameraForZoom, fitScale, padArea, scaleForFraction, @@ -106,6 +108,14 @@ const camera = () => screen.getByTestId("camera-layer").style.transform; const scaleOf = (transform: string) => Number(/scale\(([-\d.]+)\)/.exec(transform)?.[1]); +// The camera the map opens on and the fit button returns to: the site view. +const siteCam = () => + cameraForZoom(SITE_ZOOM, site, VIEWPORT, viewGeom(CANVAS.width, CANVAS.height, VIEWPORT))!; +const siteTransform = () => { + const c = siteCam(); + return `translate(${c.tx}px, ${c.ty}px) scale(${c.scale})`; +}; + describe("zooming to an area", () => { it("lands on the same camera from the Layers row and from ?zoom=", () => { // The area's row under Layers > Areas, where its name lives. @@ -120,17 +130,17 @@ describe("zooming to an area", () => { const fromSearch = camera(); expect(fromSearch).toBe(fromRow); - expect(fromRow).not.toBe("translate(0px, 0px) scale(1)"); + expect(fromRow).not.toBe(siteTransform()); }); it("goes back to the whole site from the fit button", () => { render(); fireEvent.click(screen.getByTitle("Zoom to dock")); - expect(camera()).not.toBe("translate(0px, 0px) scale(1)"); + expect(camera()).not.toBe(siteTransform()); fireEvent.click(screen.getByTitle("Zoom to the whole site")); - expect(camera()).toBe("translate(0px, 0px) scale(1)"); + expect(camera()).toBe(siteTransform()); }); it("fills the canvas with the area rather than stopping short of it", () => { @@ -156,13 +166,14 @@ describe("zooming to an area", () => { scaleBar(pxPerMm("x", VIEWPORT, geom, { scale, tx: 0, ty: 0 })); const scaleText = () => screen.getByLabelText("Map scale").textContent ?? ""; - expect(scaleText()).toBe(barLabel(barAt(1).mm)); + const open = siteCam().scale; + expect(scaleText()).toBe(barLabel(barAt(open).mm)); // Zoomed in, the same length of canvas stands for less floor. for (let i = 0; i < 3; i += 1) fireEvent.click(screen.getByTitle("Zoom in")); - const closer = barAt(ZOOM_STEP ** 3); + const closer = barAt(open * ZOOM_STEP ** 3); expect(scaleText()).toBe(barLabel(closer.mm)); - expect(closer.mm).toBeLessThan(barAt(1).mm); + expect(closer.mm).toBeLessThan(barAt(open).mm); }); it("shows where in the range the map is, however it got there", () => { @@ -171,13 +182,15 @@ describe("zooming to an area", () => { const max = zoomMax(site, VIEWPORT, geom); const slider = () => screen.getByLabelText("Zoom") as HTMLInputElement; - // The whole site is the far end of the range. - expect(Number(slider().value)).toBeCloseTo(0, 9); + // The site view sits inside the range: the far end is the whole viewport. + const open = siteCam().scale; + expect(Number(slider().value)).toBeCloseTo(zoomFraction(open, max), 9); + expect(Number(slider().value)).toBeGreaterThan(0); // The buttons and the handle agree on where a press lands. fireEvent.click(screen.getByTitle("Zoom in")); expect(Number(slider().value)).toBeCloseTo( - zoomFraction(ZOOM_STEP, max), + zoomFraction(open * ZOOM_STEP, max), 6, ); @@ -210,14 +223,15 @@ describe("zooming to an area", () => { const geom = viewGeom(CANVAS.width, CANVAS.height, VIEWPORT); const max = zoomMax(site, VIEWPORT, geom); const slider = screen.getByLabelText("Zoom") as HTMLInputElement; + const open = siteCam().scale; fireEvent.keyDown(slider, { key: "ArrowRight" }); - expect(scaleOf(camera())).toBeCloseTo(ZOOM_STEP, 6); + expect(scaleOf(camera())).toBeCloseTo(open * ZOOM_STEP, 6); fireEvent.keyDown(slider, { key: "ArrowLeft" }); - expect(scaleOf(camera())).toBeCloseTo(ZOOM_MIN, 6); + expect(scaleOf(camera())).toBeCloseTo(open, 6); fireEvent.keyDown(slider, { key: "PageUp" }); - expect(scaleOf(camera())).toBeCloseTo(ZOOM_STEP ** 3, 6); + expect(scaleOf(camera())).toBeCloseTo(open * ZOOM_STEP ** 3, 6); fireEvent.keyDown(slider, { key: "End" }); expect(scaleOf(camera())).toBeCloseTo(max, 6); diff --git a/dotbot/console-web/src/zoomSurface.test.tsx b/dotbot/console-web/src/zoomSurface.test.tsx index d6be95d9..265887e0 100644 --- a/dotbot/console-web/src/zoomSurface.test.tsx +++ b/dotbot/console-web/src/zoomSurface.test.tsx @@ -7,7 +7,7 @@ import { MapView } from "./MapView"; import type { Area, Site } from "./types"; import { Camera, - SITE_CAMERA, + FRAME_CAMERA, cameraForZoom, viewGeom, zoomFromSearch, @@ -27,7 +27,7 @@ const GEOM = viewGeom(900, 600, VIEWPORT); // The map with a camera over it, zoomed the way App zooms it. const Harness: React.FC<{ onCam?: (c: Camera) => void; from?: Camera }> = ({ onCam, - from = SITE_CAMERA, + from = FRAME_CAMERA, }) => { const [cam, setCam] = useState(from); return ( @@ -71,7 +71,7 @@ describe("the fit button", () => { fireEvent.click(screen.getByTitle("Zoom to the whole site")); - expect(seen).toEqual([SITE_CAMERA]); + expect(seen).toEqual([cameraForZoom("site", C405, VIEWPORT, GEOM)]); }); it("offers no menu: an area is zoomed from its Layers row", () => { @@ -121,7 +121,7 @@ describe("the zoom buttons", () => { it("keeps what is in the middle of the map in the middle", () => { // Panned away from the frame's own centre: the case a zoom that only // rewrites the scale throws somewhere else. - render(); + render(); const was = centreOfFrame(camera()); // One frame millimetre is well under a pixel here, so a millimetre of // tolerance is stricter than the pixel the assertion is about. @@ -140,13 +140,13 @@ describe("the zoom buttons", () => { }); it("comes back to the camera it left, in and out again", () => { - render(); + render(); press("Zoom in", 3); press("Zoom out", 3); const back = camera(); expect(back.scale).toBeCloseTo(1, 6); - expect(back.tx).toBeCloseTo(100, 4); - expect(back.ty).toBeCloseTo(20, 4); + expect(back.tx).toBeCloseTo(40, 4); + expect(back.ty).toBeCloseTo(10, 4); cleanup(); }); }); diff --git a/dotbot/controller.py b/dotbot/controller.py index e51bce3e..acd8e17f 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -71,15 +71,23 @@ DotBotNotificationCommand, DotBotNotificationModel, DotBotNotificationUpdate, + DotBotPoseModel, DotBotQueryModel, DotBotStatus, ) from dotbot.protocol import ( + DIRECTION_NONE, ApplicationType, ControlModeType, PayloadLh2CalibrationHomography, PayloadType, ) +from dotbot.robots import ( + SWARMIT_DEVICE_MODELS, + HeadingSource, + Point, + robot_geometry, +) from dotbot.server import api, default_ui_path from dotbot.site import Site from dotbot.swarm_client import build_swarmit_client, conn_string @@ -92,6 +100,9 @@ # ) +# Stands in for the body heading until the control loop advertises one. +PLACEHOLDER_HEADING_DEG = 0 + INACTIVE_DELAY = 5 # seconds LOST_DELAY = 60 # seconds LH2_POSITION_DISTANCE_THRESHOLD = 20 # mm @@ -152,6 +163,32 @@ class ControllerSettings: mrta_url: str = MRTA_URL_DEFAULT +def body_pose( + model: str, position: DotBotLH2Position, direction: int +) -> DotBotPoseModel: + """The body around an LH2 photodiode fix, facing the advertised direction. + + With no advertised direction the pose faces `PLACEHOLDER_HEADING_DEG` + and says so in its `heading_source`. + """ + if direction != DIRECTION_NONE: + heading, source = direction, HeadingSource.TRAVEL + else: + heading, source = PLACEHOLDER_HEADING_DEG, HeadingSource.NONE + return DotBotPoseModel.from_body_pose( + robot_geometry(model).body_pose(Point(position.x, position.y), heading, source) + ) + + +def device_pose(device: str, position: DotBotLH2Position) -> Optional[DotBotPoseModel]: + """The headingless pose of a robot swarmit reports as `device` at + `position`, or None when the host has no geometry record for that type.""" + model = SWARMIT_DEVICE_MODELS.get(device) + if model is None: + return None + return body_pose(model, position, DIRECTION_NONE) + + def lh2_distance(last: DotBotLH2Position, new: DotBotLH2Position) -> float: """Helper function that computes the distance between 2 LH2 positions.""" return math.sqrt(((new.x - last.x) ** 2) + ((new.y - last.y) ** 2)) @@ -449,11 +486,15 @@ def _update_dotbot_twin( if twin is None: twin = DotBotSimulator( SimulatedDotBotSettings( - address=address, pos_x=init_pos_x, pos_y=init_pos_y + address=address, + pos_x=init_pos_x, + pos_y=init_pos_y, + direction=init_direction, ), queue.Queue(), ) - twin.direction = init_direction + twin._direction_origin_x = init_pos_x + twin._direction_origin_y = init_pos_y twin.encoder_left_acc = init_encoder_left twin.encoder_right_acc = init_encoder_right self._dotbot_twins[address] = twin @@ -562,7 +603,9 @@ def handle_received_frame( dotbot.rudder_angle = self.dotbots[source].rudder_angle dotbot.sail_angle = self.dotbots[source].sail_angle dotbot.rgb_led = self.dotbots[source].rgb_led + dotbot.model = self.dotbots[source].model dotbot.lh2_position = self.dotbots[source].lh2_position + dotbot.pose = self.dotbots[source].pose dotbot.gps_position = self.dotbots[source].gps_position dotbot.waypoints = self.dotbots[source].waypoints dotbot.waypoints_threshold = self.dotbots[source].waypoints_threshold @@ -613,14 +656,20 @@ def handle_received_frame( ) self.send_payload(int(source, 16), payload=payload) elif is_fully_calibrated is True: - if frame.packet.payload.direction != 0xFFFF: - dotbot.direction = frame.packet.payload.direction + dotbot.direction = ( + None + if frame.packet.payload.direction == DIRECTION_NONE + else frame.packet.payload.direction + ) new_position = DotBotLH2Position( x=frame.packet.payload.pos_x, y=frame.packet.payload.pos_y, ) if new_position.x != 0xFFFFFFFF and new_position.y != 0xFFFFFFFF: dotbot.lh2_position = new_position + dotbot.pose = body_pose( + dotbot.model, new_position, frame.packet.payload.direction + ) if ( dotbot.position_history and lh2_distance(dotbot.position_history[-1], new_position) @@ -646,7 +695,7 @@ def handle_received_frame( controller_mode=ControlModeType(frame.packet.payload.mode), init_pos_x=new_position.x, init_pos_y=new_position.y, - init_direction=dotbot.direction, + init_direction=frame.packet.payload.direction, init_encoder_left=frame.packet.payload.encoder_left, init_encoder_right=frame.packet.payload.encoder_right, ) @@ -654,7 +703,7 @@ def handle_received_frame( real_log = CSVLog( pos_x=dotbot.lh2_position.x, pos_y=dotbot.lh2_position.y, - direction=dotbot.direction, + direction=frame.packet.payload.direction, pwm_left=frame.packet.payload.pwm_left, pwm_right=frame.packet.payload.pwm_right, encoder_left=frame.packet.payload.encoder_left, @@ -681,6 +730,7 @@ def handle_received_frame( battery_level=dotbot.battery, sim_battery_voltage=twin.battery_voltage / 1000.0, address=dotbot.address, + pose=dotbot.pose, ) need_update = True @@ -696,7 +746,7 @@ def handle_received_frame( "Advertisement Data", direction=frame.packet.payload.direction, X=frame.packet.payload.pos_x, - Y=frame.packet.payload.pos_x, + Y=frame.packet.payload.pos_y, battery=frame.packet.payload.battery, ) if ( @@ -853,6 +903,12 @@ def send_payload(self, destination: int, payload: Payload): payload=payload, ) + def device_poses(self) -> Dict[str, DotBotPoseModel]: + """The headingless pose of each swarmit device type the host has a + geometry record for, with its photodiode at the origin.""" + origin = DotBotLH2Position(x=0, y=0) + return {device: device_pose(device, origin) for device in SWARMIT_DEVICE_MODELS} + def get_dotbots(self, query: DotBotQueryModel) -> List[DotBotModel]: """Returns the list of dotbots matching the query.""" dotbots: List[DotBotModel] = [] diff --git a/dotbot/csv_data_logger.py b/dotbot/csv_data_logger.py index 2b7618d2..4de4fbee 100644 --- a/dotbot/csv_data_logger.py +++ b/dotbot/csv_data_logger.py @@ -4,10 +4,13 @@ import time from dataclasses import dataclass from pathlib import Path -from typing import IO, Optional, Union +from typing import IO, TYPE_CHECKING, Optional, Union from dotbot.logger import LOGGER +if TYPE_CHECKING: + from dotbot.models import DotBotPoseModel + @dataclass class CSVLog: @@ -34,6 +37,10 @@ def __init__(self, file_path: Union[str, Path]) -> None: "real_pos_x", "real_pos_y", "real_direction", + "pose_centre_x", + "pose_centre_y", + "heading_deg", + "heading_source", "sim_pos_x", "sim_pos_y", "sim_direction", @@ -71,6 +78,7 @@ def log( battery_level: float, sim_battery_voltage: float, address: str, + pose: Optional["DotBotPoseModel"] = None, ) -> None: """Log a data entry to the CSV file.""" row = { @@ -78,6 +86,10 @@ def log( "real_pos_x": real_log.pos_x, "real_pos_y": real_log.pos_y, "real_direction": real_log.direction, + "pose_centre_x": pose.centre.x if pose else None, + "pose_centre_y": pose.centre.y if pose else None, + "heading_deg": pose.heading_deg if pose else None, + "heading_source": pose.heading_source if pose else None, "sim_pos_x": sim_log.pos_x, "sim_pos_y": sim_log.pos_y, "sim_direction": sim_log.direction, diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index 43f58409..c8ad6b99 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -29,18 +29,23 @@ ) from dotbot.area import Area from dotbot.logger import LOGGER -from dotbot.protocol import ControlModeType, PayloadDotBotAdvertisement, PayloadType +from dotbot.protocol import ( + DIRECTION_NONE, + ControlModeType, + PayloadDotBotAdvertisement, + PayloadType, +) +from dotbot.robots import robot_geometry from dotbot.site import Site -Kv = 700 # motor speed constant in RPM -R = 50 # motor reduction ratio -D = 44 # wheel diameter in mm -L = 78 # distance between the two wheels in mm +_GEOMETRY = robot_geometry() -# Encoder model: counts per mm of wheel travel (must match C-side DB_MM_PER_COUNT) -# mm_per_count = pi * D / (CPR * R) -ENCODER_CPR = 28 # counts per motor shaft revolution (7 PPR decoded x4) -MM_PER_COUNT = (pi * D) / (ENCODER_CPR * R) # ~0.0987 mm/count +Kv = 700 # motor speed constant in RPM +R = _GEOMETRY.gear_ratio # motor reduction ratio +D = _GEOMETRY.wheel_diameter_mm +L = _GEOMETRY.track_mm # distance between the two wheels in mm +ENCODER_CPR = _GEOMETRY.encoder_cpr # counts per motor shaft revolution +MM_PER_COUNT = _GEOMETRY.mm_per_count # Control parameters for the automatic mode MOTOR_SPEED = 60 @@ -48,6 +53,10 @@ REDUCE_SPEED_FACTOR = 0.8 REDUCE_SPEED_ANGLE = 25 +# Travel away from the last recorded point before a new heading is computed. +# Mirrors DB_DIRECTION_THRESHOLD in the firmware control loop. +DIRECTION_THRESHOLD_MM = 50 + SIMULATOR_STEP_DELTA_T = 0.01 # 10 ms # Battery model parameters @@ -138,7 +147,7 @@ class SimulatedDotBotSettings(BaseModel): address: str = Field(default_factory=_random_address) pos_x: Optional[int] = None pos_y: Optional[int] = None - direction: int = -1000 + direction: int = DIRECTION_NONE calibrated: int = 0xFF motor_left_error: float = 0 motor_right_error: float = 0 @@ -198,7 +207,9 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.address = settings.address.upper() self.pos_x = settings.pos_x or 0 self.pos_y = settings.pos_y or 0 - self.theta = settings.direction * -1 if settings.direction != -1000 else 0 + self.theta = ( + settings.direction * -1 if settings.direction != DIRECTION_NONE else 0 + ) self.motor_left_error = settings.motor_left_error self.motor_right_error = settings.motor_right_error self.custom_control_loop_library = settings.custom_control_loop_library @@ -208,6 +219,10 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.pwm_left = 0 self.pwm_right = 0 self.direction = settings.direction + # Point the next heading is measured from. The frame origin at boot, as + # on the real robot, so the first heading is an origin bearing. + self._direction_origin_x = 0.0 + self._direction_origin_y = 0.0 # Accumulated encoder deltas between control-loop calls (control runs at # SIMULATOR_UPDATE_INTERVAL_S, physics at SIMULATOR_STEP_DELTA_T — multiple @@ -344,12 +359,15 @@ def diff_drive_model_update(self, dt=SIMULATOR_STEP_DELTA_T): self.pos_y = pos_y_old + dy self.theta = (theta_old + w * dt * 180 / pi) % 360 - if sqrt(dx**2 + dy**2): - self.direction = int(-1 * atan2(dx, dy) * 180 / pi) % 360 + origin_dx = self.pos_x - self._direction_origin_x + origin_dy = self.pos_y - self._direction_origin_y + moved = dx != 0 or dy != 0 + if moved and sqrt(origin_dx**2 + origin_dy**2) > DIRECTION_THRESHOLD_MM: + self.direction = int(-1 * atan2(origin_dx, origin_dy) * 180 / pi) % 360 if self.direction > 180: self.direction -= 360 - elif self.direction < -180: - self.direction += 360 + self._direction_origin_x = self.pos_x + self._direction_origin_y = self.pos_y # Accumulate encoder counts for this physics step if self.controller_mode == ControlModeType.AUTO: @@ -541,7 +559,9 @@ def _control_loop_default(self): self.waypoint_y = int(self.waypoints[self.waypoint_index].pos_y) angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi - robot_angle = self.direction + # Steer on the true pose: the advertised direction lags travel, so a + # bot turning in place would never see its own heading change. + robot_angle = -self.theta if robot_angle >= 180: robot_angle -= 360 elif robot_angle < -180: @@ -566,6 +586,7 @@ def _control_loop_default(self): self.logger.info( "Loop update", robot_angle=int(robot_angle), + direction=int(self.direction), angle_to_target=int(angle_to_target), error_angle=int(error_angle), angular_speed=int(angular_speed), diff --git a/dotbot/models.py b/dotbot/models.py index e1a4519b..5dcf3d29 100644 --- a/dotbot/models.py +++ b/dotbot/models.py @@ -15,6 +15,7 @@ from pydantic import BaseModel from dotbot.protocol import ApplicationType, ControlModeType +from dotbot.robots import ROBOT_DEFAULT, BodyPose MAX_POSITION_HISTORY_SIZE = 1000 @@ -75,7 +76,12 @@ class DotBotGPSPosition(BaseModel): class DotBotWaypoints(BaseModel): - """Waypoints model.""" + """Waypoints model. + + An LH2 waypoint is a target for the robot's LH2 photodiode, not for its + body: the robot has arrived when its photodiode is within `threshold` mm + of it. + """ threshold: int waypoints: List[Union[DotBotLH2Position, DotBotGPSPosition]] @@ -144,6 +150,7 @@ class DotBotCameraPoseModel(BaseModel): photodiode_mm: List[float] nose_mm: List[float] outline_mm: List[List[float]] = [] + wheels_mm: List[List[List[float]]] = [] heading_deg: float heading_atan2_deg: float green_flare: float = 0.0 @@ -377,6 +384,50 @@ class DotBotReplyModel(BaseModel): data: Any +class DotBotPoseModel(BaseModel): + """A robot's body in the arena frame, expanded from its photodiode fix. + + `heading_source` says how `heading_deg` was made: "travel" is the bearing + between fixes, "ekf" the robot's own estimate, and "none" means there was + no heading and `heading_deg` is a placeholder. + """ + + heading_deg: float + heading_source: Literal["none", "travel", "ekf"] + photodiode: DotBotLH2Position # where the pose places the LH2 photodiode + axle: DotBotLH2Position + centre: DotBotLH2Position + nose: DotBotLH2Position + led: DotBotLH2Position + outline: List[DotBotLH2Position] + wheels: List[List[DotBotLH2Position]] = [] + # Radii about the photodiode, whatever the heading: `reach_mm` holds the + # whole body, tyres included, and `core_mm` is covered by the board. + reach_mm: float + core_mm: float + envelope_mm: float + + @classmethod + def from_body_pose(cls, pose: BodyPose) -> "DotBotPoseModel": + def point(p): + return DotBotLH2Position(x=p.x, y=p.y) + + return cls( + heading_deg=pose.heading_deg, + heading_source=pose.heading_source.name.lower(), + photodiode=point(pose.photodiode), + axle=point(pose.axle), + centre=point(pose.centre), + nose=point(pose.nose), + led=point(pose.led), + outline=[point(p) for p in pose.outline], + wheels=[[point(p) for p in wheel] for wheel in pose.wheels], + reach_mm=pose.reach_mm, + core_mm=pose.core_mm, + envelope_mm=pose.envelope_mm, + ) + + class DotBotModel(BaseModel): """Model class that defines a DotBot.""" @@ -392,7 +443,10 @@ class DotBotModel(BaseModel): sail_angle: Optional[int] = None move_raw: Optional[DotBotMoveRawCommandModel] = None rgb_led: Optional[DotBotRgbLedCommandModel] = None + model: str = ROBOT_DEFAULT # the geometry record's key + # The LH2 photodiode, not a body point; `pose` is the body. lh2_position: Optional[DotBotLH2Position] = None + pose: Optional[DotBotPoseModel] = None gps_position: Optional[DotBotGPSPosition] = None waypoints: List[Union[DotBotLH2Position, DotBotGPSPosition]] = [] waypoints_threshold: int = 100 # in mm diff --git a/dotbot/protocol.py b/dotbot/protocol.py index d907a218..6817fc83 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -14,6 +14,9 @@ from dotbot_utils.protocol import Payload, PayloadFieldMetadata, register_parser +# The advertised `direction` when the robot has no heading. +DIRECTION_NONE = -1000 + class PayloadType(IntEnum): """Types of DotBot payload types.""" diff --git a/dotbot/robots.py b/dotbot/robots.py index 7c2e6c71..f9378969 100644 --- a/dotbot/robots.py +++ b/dotbot/robots.py @@ -1,53 +1,277 @@ # SPDX-FileCopyrightText: 2026-present Inria # SPDX-License-Identifier: BSD-3-Clause -"""Physical geometry of the robot models the host talks to. +"""Physical geometry of the robot models the host talks to, one record per +board revision. -Anything that has to reason about where a robot's sensors sit relative to -its body reads this: the calibration point resolver offsets a corner mark -by the photodiode's distance to the body edges resting on that corner. +Every point in a record is a coordinate in that revision's KiCad board frame: +millimetres, x to the robot's right, y toward the rear, nose at low y, origin +off the robot. The C copy of the drivetrain constants and the lever arm lives +in DotBot-libs `drv/geometry.h` and is pinned to this one by +`dotbot/tests/test_control_loop_geometry.py`. """ from __future__ import annotations +import math from dataclasses import dataclass +from enum import IntEnum +from functools import cached_property +from typing import NamedTuple ROBOT_DEFAULT = "dotbot-v3" +class Point(NamedTuple): + x: float + y: float + + +class HeadingSource(IntEnum): + """How the heading a pose is built from was made.""" + + NONE = 0 # no heading; the pose's heading is a placeholder + TRAVEL = 1 # bearing of travel between two fixes + EKF = 2 # body heading from the on-bot estimator + + +@dataclass(frozen=True) +class BodyPose: + """A robot's body in the arena frame, in mm and degrees. + + `heading_deg` follows the robot `direction` convention: 0 = +y, and the + body-forward unit vector is (-sin, +cos). + """ + + heading_deg: float + heading_source: HeadingSource + photodiode: Point # where the pose places the LH2 photodiode + axle: Point + centre: Point + nose: Point + led: Point + outline: tuple[Point, ...] + wheels: tuple[tuple[Point, ...], ...] + # Radii about the photodiode, which hold under any heading: `reach_mm` + # encloses the whole body, tyres included; `core_mm` is covered by the + # board. `envelope_mm` is the record's plan-view square. + reach_mm: float + core_mm: float + envelope_mm: float + + +def _segment_distance(p: Point, a: Point, b: Point) -> float: + """Distance from `p` to the segment `a`-`b`.""" + dx, dy = b.x - a.x, b.y - a.y + length2 = dx * dx + dy * dy + t = 0.0 if length2 == 0 else ((p.x - a.x) * dx + (p.y - a.y) * dy) / length2 + t = max(0.0, min(1.0, t)) + return math.hypot(a.x + t * dx - p.x, a.y + t * dy - p.y) + + +def _rect(centre: Point, width_mm: float, length_mm: float) -> tuple[Point, ...]: + """An axis-aligned rectangle about `centre`, `width_mm` in x by `length_mm` + in y, wound from its low-x low-y corner.""" + x0, x1 = centre.x - width_mm / 2, centre.x + width_mm / 2 + y0, y1 = centre.y - length_mm / 2, centre.y + length_mm / 2 + return (Point(x0, y0), Point(x1, y0), Point(x1, y1), Point(x0, y1)) + + @dataclass(frozen=True) class RobotGeometry: - """One robot model's board footprint and sensor offsets, in millimetres. + """One board revision's outline, points and drivetrain, in the board frame. - Distances are measured from the photodiode to a board edge with the - robot's nose toward the frame's top edge, which is the low-y one since - y grows down. That orientation is this class's own reference, and is - not the robot `direction` convention, where 0 = +y. + The edge distances (`diode_to_front_mm` and the rest) are measured with + the robot's nose toward the frame's top edge, which is the low-y one since + y grows down. That orientation is the board frame's own, and is not the + robot `direction` convention, where 0 = +y. """ model: str - board_width_mm: float # side to side - board_length_mm: float # nose to tail - diode_to_front_mm: float - diode_to_rear_mm: float - diode_to_side_mm: float - led_to_front_mm: float # RGB LED, on the centreline + # Outer board path; each nose fillet (r 1.0) is its chord. + outline_path: tuple[Point, ...] + photodiode: Point + led: Point + caster: Point + axle_midpoint: Point + track_mm: float + wheel_diameter_mm: float + tyre_width_mm: float + # The motor connector pin block, whose red housing is the axle the camera + # detector fits: J5 to J6 centre to centre, then one housing's footprint. + connector_spacing_mm: float + connector_width_mm: float + connector_length_mm: float + encoder_cpr: int + gear_ratio: float + # The plan-view square for anything that needs a size rather than a shape. + envelope_mm: float + + def __post_init__(self): + if self.photodiode.x != self.axle_midpoint.x: + raise ValueError( + f"{self.model}: photodiode off the centreline; " + "a non-zero lever angle is not handled" + ) + + @cached_property + def outline_bbox(self) -> tuple[float, float, float, float]: + """(x_min, y_min, x_max, y_max) of the outline.""" + xs = [p.x for p in self.outline_path] + ys = [p.y for p in self.outline_path] + return (min(xs), min(ys), max(xs), max(ys)) + + @cached_property + def outline_centre(self) -> Point: + x_min, y_min, x_max, y_max = self.outline_bbox + return Point((x_min + x_max) / 2, (y_min + y_max) / 2) @property - def led_ahead_of_diode_mm(self) -> float: - """How far the RGB LED sits toward the nose from the photodiode. + def board_width_mm(self) -> float: + x_min, _, x_max, _ = self.outline_bbox + return x_max - x_min + + @property + def board_length_mm(self) -> float: + _, y_min, _, y_max = self.outline_bbox + return y_max - y_min - A camera tracking the LED reports this offset, rotated by the - robot's orientation, away from the photodiode's position. + def _axle_pair( + self, spacing_mm: float, width_mm: float, length_mm: float + ) -> tuple[tuple[Point, ...], ...]: + """One rectangle either side of the axle midpoint, the -x one first. + + `width_mm` is across the robot and `length_mm` along it, and each + rectangle's centre sits half of `spacing_mm` off the centreline on the + axle line. + """ + return tuple( + _rect(Point(cx, self.axle_midpoint.y), width_mm, length_mm) + for cx in ( + self.axle_midpoint.x - spacing_mm / 2, + self.axle_midpoint.x + spacing_mm / 2, + ) + ) + + @cached_property + def wheel_paths(self) -> tuple[tuple[Point, ...], ...]: + """Each driven wheel in plan view, as a rectangle in the board frame. + + A wheel is `tyre_width_mm` across the robot and `wheel_diameter_mm` + along it, at half the track either side of the axle midpoint. The left + wheel comes first, left being -x. """ - return self.diode_to_front_mm - self.led_to_front_mm + return self._axle_pair( + self.track_mm, self.tyre_width_mm, self.wheel_diameter_mm + ) + + @property + def connector_paths(self) -> tuple[tuple[Point, ...], ...]: + """Each motor connector in plan view, as a rectangle in the board frame. + + The pair the camera detector reads as the axle, in the same order and + the same frame as `wheel_paths`. + """ + return self._axle_pair( + self.connector_spacing_mm, + self.connector_width_mm, + self.connector_length_mm, + ) + + @property + def lever_arm_mm(self) -> float: + """Axle midpoint to photodiode, forward: `DB_LH2_LEVER_ARM`.""" + return self.axle_midpoint.y - self.photodiode.y + + @property + def lever_angle_deg(self) -> float: + """`DB_LH2_LEVER_ANGLE`; zero, as `__post_init__` enforces.""" + return 0.0 + + @property + def diode_to_front_mm(self) -> float: + return self.photodiode.y - self.outline_bbox[1] + + @property + def diode_to_rear_mm(self) -> float: + return self.outline_bbox[3] - self.photodiode.y + + @property + def diode_to_side_mm(self) -> float: + x_min, _, x_max, _ = self.outline_bbox + return min(self.photodiode.x - x_min, x_max - self.photodiode.x) + + @property + def led_to_front_mm(self) -> float: + return self.led.y - self.outline_bbox[1] + + @property + def led_ahead_of_diode_mm(self) -> float: + """How far the RGB LED sits toward the nose from the photodiode.""" + return self.photodiode.y - self.led.y + + @property + def diode_ahead_of_centre_mm(self) -> float: + return self.outline_centre.y - self.photodiode.y + + @cached_property + def reach_mm(self) -> float: + """Furthest outline or wheel point from the photodiode.""" + points = [*self.outline_path, *(p for w in self.wheel_paths for p in w)] + return max( + math.hypot(p.x - self.photodiode.x, p.y - self.photodiode.y) for p in points + ) + + @cached_property + def core_mm(self) -> float: + """Nearest board edge to the photodiode.""" + path = self.outline_path + return min( + _segment_distance(self.photodiode, a, b) + for a, b in zip(path, path[1:] + path[:1]) + ) + + @property + def mm_per_count(self) -> float: + """Wheel travel per encoder count: `DB_MM_PER_COUNT`.""" + return math.pi * self.wheel_diameter_mm / (self.encoder_cpr * self.gear_ratio) + + def body_pose( + self, sensor: Point, heading_deg: float, source: HeadingSource + ) -> BodyPose: + """The body around a photodiode fix at `sensor`, facing `heading_deg`.""" + theta = math.radians(heading_deg) + forward = (-math.sin(theta), math.cos(theta)) + right = (-math.cos(theta), -math.sin(theta)) + + def place(point: Point) -> Point: + ahead = self.photodiode.y - point.y + aside = point.x - self.photodiode.x + return Point( + sensor[0] + ahead * forward[0] + aside * right[0], + sensor[1] + ahead * forward[1] + aside * right[1], + ) + + return BodyPose( + heading_deg=heading_deg, + heading_source=source, + photodiode=Point(sensor[0], sensor[1]), + axle=place(self.axle_midpoint), + centre=place(self.outline_centre), + nose=place(Point(self.photodiode.x, self.outline_bbox[1])), + led=place(self.led), + outline=tuple(place(p) for p in self.outline_path), + wheels=tuple(tuple(place(p) for p in wheel) for wheel in self.wheel_paths), + reach_mm=self.reach_mm, + core_mm=self.core_mm, + envelope_mm=self.envelope_mm, + ) def clearance_mm(self, edge: str) -> float: """Distance from the photodiode to the body edge facing `edge`. `edge` is one of top / bottom / left / right in frame orientation, - with the nose toward the top edge and the rear toward the bottom - one, which is this class's own reference. + with the nose toward the top edge and the rear toward the bottom one. """ if edge == "top": return self.diode_to_front_mm @@ -71,24 +295,49 @@ def photodiode_inset(self, corner: str) -> tuple[float, float]: return (dx if horizontal == "left" else -dx, dy if vertical == "top" else -dy) -# Measured off the DotBot v3 main board bd1.3a in KiCad: the Edge.Cuts -# outline is 94.00 x 95.00 mm, the photodiode D17 (Osram BPW34S, footprint -# centre = active-area centre) sits on the centreline 18.5 mm from one long -# edge, and the RGB LED D18 sits 13.0 mm from the same edge. That edge is -# taken as the robot's front, which is the nest-sheet generator's convention. +# DotBot v3, main board bd1.3a: board points from its KiCad file, wheel and +# track caliper-measured, tyre width measured on a photograph. ROBOTS: dict[str, RobotGeometry] = { "dotbot-v3": RobotGeometry( model="dotbot-v3", - board_width_mm=94.0, - board_length_mm=95.0, - diode_to_front_mm=18.5, - diode_to_rear_mm=76.5, - diode_to_side_mm=47.0, - led_to_front_mm=13.0, + outline_path=( + Point(118.0, 60.5), + Point(122.0, 60.5), + Point(122.0, 98.5), + Point(103.5, 98.5), + Point(103.5, 147.5), + Point(46.5, 147.5), + Point(46.5, 98.5), + Point(28.0, 98.5), + Point(28.0, 60.5), + Point(32.0, 60.5), + Point(33.0, 59.5), + Point(33.0, 52.5), + Point(117.0, 52.5), + Point(117.0, 59.5), + ), + photodiode=Point(75.0, 71.0), # D17 + led=Point(75.0, 65.5), # D18 + caster=Point(75.0, 59.0), # J19/J20 midpoint + axle_midpoint=Point(75.0, 124.5), # M1/M2 midpoint + track_mm=78.0, + wheel_diameter_mm=44.0, + tyre_width_mm=17.5, + connector_spacing_mm=27.0, # J5 to J6 + connector_width_mm=12.0, + connector_length_mm=10.0, + encoder_cpr=28, + gear_ratio=50.0, + envelope_mm=95.0, ), } +# The geometry record for each device type swarmit's STATUS reports; a type +# missing here has none. +SWARMIT_DEVICE_MODELS: dict[str, str] = {"DotBotV3": "dotbot-v3"} + + def robot_geometry(model: str = ROBOT_DEFAULT) -> RobotGeometry: """The geometry record for `model`.""" try: diff --git a/dotbot/server.py b/dotbot/server.py index aaa75e3d..03731fce 100644 --- a/dotbot/server.py +++ b/dotbot/server.py @@ -7,7 +7,7 @@ import base64 import os -from typing import Annotated, List, Optional +from typing import Annotated, Dict, List, Optional import httpx from fastapi import ( @@ -49,6 +49,7 @@ DotBotNotificationCommand, DotBotNotificationModel, DotBotNotificationUpdate, + DotBotPoseModel, DotBotQueryModel, DotBotRgbLedCommandModel, DotBotSiteModel, @@ -296,6 +297,17 @@ async def dotbots(query: Annotated[DotBotQueryModel, Query()]): return api.controller.get_dotbots(query) +@api.get( + path="/controller/device_poses", + response_model=Dict[str, DotBotPoseModel], + summary="Return the headingless pose of each swarmit device type, at the origin", + tags=["controller"], +) +async def device_poses(): + """Device poses HTTP GET handler.""" + return api.controller.device_poses() + + @api.get( path="/controller/site", response_model=DotBotSiteModel, diff --git a/dotbot/simulator_init_state.toml b/dotbot/simulator_init_state.toml index 445d3f58..8c5e7f9a 100644 --- a/dotbot/simulator_init_state.toml +++ b/dotbot/simulator_init_state.toml @@ -31,3 +31,8 @@ direction = 180 [[dotbots]] address = "BADC0DE444444444" direction = 360 + +# No `direction`: this robot advertises no heading until it has travelled far +# enough for one, as a real robot does from boot until its first fix. +[[dotbots]] +address = "F00DFACE55555555" diff --git a/dotbot/tests/camera_bench_raster.jpg b/dotbot/tests/camera_bench_raster.jpg new file mode 100644 index 00000000..74526236 Binary files /dev/null and b/dotbot/tests/camera_bench_raster.jpg differ diff --git a/dotbot/tests/camera_fixtures.py b/dotbot/tests/camera_fixtures.py index c048f4d4..cf66b13b 100644 --- a/dotbot/tests/camera_fixtures.py +++ b/dotbot/tests/camera_fixtures.py @@ -18,12 +18,9 @@ from dotbot.area import Area from dotbot.camera.detection.pose import ( - AXLE_BEHIND_CENTRE_MM, CONN_MM, OUTLINE_MM, - TRACK_MM, - TYRE_D_MM, - TYRE_W_MM, + WHEELS_MM, axes, ) from dotbot.camera.sheets import MARKER_DICTIONARY, MARKER_SIDE_MM @@ -175,27 +172,6 @@ def draw_marker( # --- The robot, drawn from the estimator's own outline ----------------------- -def tyre_polygons_mm(): - """The two tyres in the robot frame, as the template places them.""" - out = [] - axle = -AXLE_BEHIND_CENTRE_MM - for side in (-1, 1): - x0 = side * TRACK_MM / 2 - TYRE_W_MM / 2 - x1 = side * TRACK_MM / 2 + TYRE_W_MM / 2 - out.append( - np.array( - [ - (x0, axle - TYRE_D_MM / 2), - (x1, axle - TYRE_D_MM / 2), - (x1, axle + TYRE_D_MM / 2), - (x0, axle + TYRE_D_MM / 2), - ], - float, - ) - ) - return out - - def carpet(width_px=250, height_px=250, seed=7): """Grey floor with the speckle a proposer has to average away.""" rng = np.random.default_rng(seed) @@ -247,7 +223,7 @@ def fill(polygon_mm, colour, offset_mm=(0.0, 0.0)): cv2.fillPoly(big, [np.round(points).astype(np.int32)], colour) fill(OUTLINE_MM, board, board_offset_mm) - for polygon in tyre_polygons_mm(): + for polygon in WHEELS_MM: fill(polygon, tyre) for polygon in CONN_MM: fill(polygon, connector) @@ -295,7 +271,7 @@ def fill(polygon_mm, colour, centre=centre_mm, r=right, f=forward): cv2.fillPoly(big, [np.round(q).astype(np.int32)], colour) fill(OUTLINE_MM, BOARD_BGR) - for tyre in tyre_polygons_mm(): + for tyre in WHEELS_MM: fill(tyre, TYRE_BGR) for connector in CONN_MM: fill(connector, CONNECTOR_BGR) diff --git a/dotbot/tests/test_camera_detection.py b/dotbot/tests/test_camera_detection.py index 4d9bd784..c5cdcba7 100644 --- a/dotbot/tests/test_camera_detection.py +++ b/dotbot/tests/test_camera_detection.py @@ -3,8 +3,9 @@ The robot these tests draw is built from the estimator's own outline, so it is model-consistent by construction: what they guard is the plumbing, the unit and frame conventions and the two confidence thresholds. Accuracy -against a real photograph is not something a synthetic raster can show, and -is left to the bench. +against a real photograph is not something a synthetic raster can show: one +bench raster pins the detector's answer on a real frame, and the rest is left +to the bench. Everything is drawn at 2.0 mm/px, which is the raster the controller warps a camera into, and the polygons are filled at four times that and box-filtered @@ -13,6 +14,7 @@ import subprocess import sys +from pathlib import Path import cv2 import numpy as np @@ -89,6 +91,17 @@ def test_finds_a_robot_and_reports_its_pose(heading): assert across.min() == pytest.approx(-47.0, abs=0.2) +# A 2.0 mm/px raster warped from the bench camera, two robots on the floor. +BENCH_RASTER = Path(__file__).with_name("camera_bench_raster.jpg") + + +def test_finds_the_robot_in_a_bench_raster(): + detection = RobotDetector(MM_PER_PX).detect(cv2.imread(str(BENCH_RASTER))) + assert detection.status == "found" + assert detection.pose.centre_px == pytest.approx((418.7, 374.6), abs=3.0) + assert abs(wrap180(detection.pose.heading_atan2_deg - 3.0)) < 3.0 + + def test_direction_convention(): """The heading the console draws is the detector's, turned by 90 degrees. diff --git a/dotbot/tests/test_control_loop_geometry.py b/dotbot/tests/test_control_loop_geometry.py index c760d961..69075abf 100644 --- a/dotbot/tests/test_control_loop_geometry.py +++ b/dotbot/tests/test_control_loop_geometry.py @@ -1,8 +1,8 @@ -"""Guards the robot dimensions the simulator duplicates from the C control loop. +"""Pins the Python geometry record to the C one in DotBot-libs `drv/geometry.h`. -The compiled loop is the source: it is what runs on a robot. `dotbot_simulator` -keeps a copy so it can generate encoder counts that loop will read back -correctly, and nothing enforces the copy at build time. +The two cannot share a file at build time, so the record for the board the +library was built for is checked against the compiled +`control_loop_get_geometry()`, and so is the simulator that reads the record. Skips unless the library is built. Point `DOTBOT_CONTROL_LOOP_LIBRARY` at it, or build it into `build/` per `utils/control_loop/README.md`. @@ -15,6 +15,7 @@ import pytest from dotbot.dotbot_simulator import ENCODER_CPR, MM_PER_COUNT, D, L, R +from dotbot.robots import ROBOTS LIBRARY_ENV = "DOTBOT_CONTROL_LOOP_LIBRARY" DEFAULT_BUILD_DIR = Path(__file__).resolve().parents[2] / "build" @@ -45,11 +46,32 @@ def _library_path() -> Path | None: return None -@pytest.fixture(name="geometry") -def geometry_fixture() -> ControlLoopGeometry: +def _built_model(path: Path) -> str: + """The robot model the library was built for, from its CMake cache. + + Falls back to the CMake default, version 3, when there is no cache beside + the library. + """ + version = "3" + cache = path.parent / "CMakeCache.txt" + if cache.exists(): + for line in cache.read_text().splitlines(): + if line.startswith("DOTBOT_VERSION:"): + version = line.partition("=")[2].strip() + return f"dotbot-v{version}" + + +@pytest.fixture(name="library_path") +def library_path_fixture() -> Path: path = _library_path() if path is None or not path.exists(): pytest.skip(f"control loop library not built; set {LIBRARY_ENV}") + return path + + +@pytest.fixture(name="geometry") +def geometry_fixture(library_path) -> ControlLoopGeometry: + path = library_path library = ctypes.CDLL(str(path)) if not hasattr(library, "control_loop_get_geometry"): pytest.skip("library predates control_loop_get_geometry") @@ -72,7 +94,27 @@ def test_simulator_matches_compiled_geometry(geometry, field, python_value): assert getattr(geometry, field) == pytest.approx(python_value, rel=1e-6) -def test_lever_arm_is_exported(geometry): - """Phase-5 estimator input: it is not in the Python copy yet, so only the - export is checked.""" - assert geometry.lh2_lever_arm_mm > 0.0 +@pytest.fixture(name="record") +def record_fixture(library_path): + model = _built_model(library_path) + if model not in ROBOTS: + pytest.skip(f"no geometry record for {model}") + return ROBOTS[model] + + +@pytest.mark.parametrize( + "field,record_field", + [ + ("lh2_lever_arm_mm", "lever_arm_mm"), + ("lh2_lever_angle_deg", "lever_angle_deg"), + ("track_mm", "track_mm"), + ("wheel_diameter_mm", "wheel_diameter_mm"), + ("encoder_cpr", "encoder_cpr"), + ("gear_ratio", "gear_ratio"), + ("mm_per_count", "mm_per_count"), + ], +) +def test_record_matches_compiled_geometry(geometry, record, field, record_field): + assert getattr(geometry, field) == pytest.approx( + getattr(record, record_field), rel=1e-6 + ) diff --git a/dotbot/tests/test_controller.py b/dotbot/tests/test_controller.py index 9df852bf..f1b92a96 100644 --- a/dotbot/tests/test_controller.py +++ b/dotbot/tests/test_controller.py @@ -9,11 +9,19 @@ from dotbot_utils.hdlc import hdlc_encode from dotbot_utils.protocol import Frame, Header, Packet from dotbot_utils.serial_interface import SerialInterface +from structlog.testing import capture_logs from dotbot import addr_to_hex from dotbot.adapter import SerialAdapter from dotbot.area import Area -from dotbot.controller import Controller, ControllerSettings, gps_distance, lh2_distance +from dotbot.controller import ( + PLACEHOLDER_HEADING_DEG, + Controller, + ControllerSettings, + device_pose, + gps_distance, + lh2_distance, +) from dotbot.models import ( DotBotGPSPosition, DotBotLH2Position, @@ -21,7 +29,14 @@ DotBotQueryModel, DotBotStatus, ) -from dotbot.protocol import ApplicationType, ControlModeType, PayloadControlMode +from dotbot.protocol import ( + DIRECTION_NONE, + ApplicationType, + ControlModeType, + PayloadControlMode, + PayloadDotBotAdvertisement, +) +from dotbot.robots import HeadingSource, Point, robot_geometry from dotbot.site import Site # A measured site, which the package never ships. @@ -741,3 +756,160 @@ async def test_a_calibration_notification_keeps_the_session_s_nulls(controller): assert first["calibration_session"]["outstanding"] is None assert "outstanding" in first["calibration_session"] assert second == {"cmd": 5, "calibration_session": None} + + +# --- DotBot advertisements, through the bytes the gateway delivers ---------- + + +def _advertised(source: int, **fields) -> Frame: + """A DotBot advertisement encoded to bytes and parsed back, as received.""" + sent = Frame( + header=Header(destination=0, source=source), + packet=Packet().from_payload(PayloadDotBotAdvertisement(**fields)), + ) + return Frame().from_bytes(sent.to_bytes()) + + +BOT = 0x42 + + +@pytest.mark.asyncio +async def test_a_new_robot_with_no_heading_is_tracked(controller): + controller.handle_received_frame( + _advertised(BOT, direction=DIRECTION_NONE, pos_x=1000, pos_y=1000) + ) + dotbot = controller.dotbots[addr_to_hex(BOT)] + assert dotbot.direction is None + assert (dotbot.lh2_position.x, dotbot.lh2_position.y) == (1000, 1000) + + +@pytest.mark.asyncio +async def test_an_advertisement_without_a_heading_clears_the_last_one(controller): + """-1000 is the no-heading sentinel: a restarted robot has no heading.""" + controller.handle_received_frame( + _advertised(BOT, direction=90, pos_x=1000, pos_y=1000) + ) + controller.handle_received_frame( + _advertised(BOT, direction=DIRECTION_NONE, pos_x=1000, pos_y=1000) + ) + dotbot = controller.dotbots[addr_to_hex(BOT)] + assert dotbot.direction is None + assert dotbot.pose.heading_source == "none" + + +@pytest.mark.asyncio +async def test_the_advertisement_debug_log_reports_y(controller): + with capture_logs() as logs: + controller.handle_received_frame( + _advertised(BOT, direction=90, pos_x=1000, pos_y=2000) + ) + (entry,) = (e for e in logs if e["event"] == "Advertisement Data") + assert (entry["X"], entry["Y"]) == (1000, 2000) + + +@pytest.mark.asyncio +async def test_a_travel_heading_puts_the_centre_behind_the_photodiode(controller): + """The centre is 29 mm behind the photodiode, along (-sin, +cos).""" + controller.handle_received_frame( + _advertised(BOT, direction=90, pos_x=1000, pos_y=1000) + ) + dotbot = controller.dotbots[addr_to_hex(BOT)] + assert (dotbot.lh2_position.x, dotbot.lh2_position.y) == (1000, 1000) + assert dotbot.pose.heading_source == "travel" + assert dotbot.pose.heading_deg == 90 + assert (dotbot.pose.centre.x, dotbot.pose.centre.y) == pytest.approx( + (1029.0, 1000.0) + ) + + +@pytest.mark.asyncio +async def test_no_heading_gives_a_placeholder_pose_that_says_so(controller): + controller.handle_received_frame( + _advertised(BOT, direction=DIRECTION_NONE, pos_x=1000, pos_y=1000) + ) + pose = controller.dotbots[addr_to_hex(BOT)].pose + assert pose.heading_source == "none" + assert pose.heading_deg == PLACEHOLDER_HEADING_DEG + assert pose.reach_mm == pytest.approx(89.33, abs=0.01) + assert pose.core_mm == pytest.approx(18.5) + + +@pytest.mark.asyncio +async def test_the_rest_surface_serves_the_photodiode_and_the_body(controller): + from httpx import ASGITransport, AsyncClient + + from dotbot.server import api + + controller.handle_received_frame( + _advertised(BOT, direction=0, pos_x=1000, pos_y=1000) + ) + previous, api.controller = getattr(api, "controller", None), controller + try: + async with AsyncClient( + transport=ASGITransport(app=api), base_url="http://testserver" + ) as client: + response = await client.get("/controller/dotbots") + finally: + api.controller = previous + (bot,) = (b for b in response.json() if b["address"] == addr_to_hex(BOT)) + assert bot["lh2_position"] == {"x": 1000.0, "y": 1000.0} + assert bot["model"] == "dotbot-v3" + assert bot["pose"]["photodiode"] == {"x": 1000.0, "y": 1000.0} + assert bot["pose"]["centre"] == pytest.approx({"x": 1000.0, "y": 971.0}) + assert bot["pose"]["heading_source"] == "travel" + assert len(bot["pose"]["outline"]) == 14 + assert bot["pose"]["reach_mm"] == pytest.approx(89.33, abs=0.01) + assert bot["pose"]["core_mm"] == pytest.approx(18.5) + assert bot["pose"]["envelope_mm"] == 95.0 + + +@pytest.mark.asyncio +async def test_the_csv_log_carries_the_body_centre_and_heading(controller, tmp_path): + from dotbot.csv_data_logger import CSVDataLogger + + controller.csv_data_logger = CSVDataLogger(tmp_path / "run.csv") + controller.handle_received_frame( + _advertised(BOT, direction=90, pos_x=1000, pos_y=1000) + ) + controller.csv_data_logger.close() + row = _last_row(tmp_path / "run.csv") + assert (row["pose_centre_x"], row["pose_centre_y"]) == ("1029.0", "1000.0") + assert (row["heading_deg"], row["heading_source"]) == ("90.0", "travel") + + +def test_a_status_only_dotbot_v3_is_sized_from_the_v3_record(): + at = DotBotLH2Position(x=1864, y=738) + pose = device_pose("DotBotV3", at) + v3 = robot_geometry("dotbot-v3").body_pose(Point(1864, 738), 0, HeadingSource.NONE) + assert pose.heading_source == "none" + assert (pose.photodiode.x, pose.photodiode.y) == (1864, 738) + assert pose.reach_mm == v3.reach_mm + assert pose.core_mm == v3.core_mm + assert pose.envelope_mm == 95.0 + + +def test_a_device_type_without_a_record_has_no_pose(): + at = DotBotLH2Position(x=1864, y=738) + for device in ("DotBotV2", "SailBot", "LH2_mini_mote", ""): + assert device_pose(device, at) is None + + +def test_the_device_poses_sit_on_the_origin(controller): + poses = controller.device_poses() + assert set(poses) == {"DotBotV3"} + assert (poses["DotBotV3"].photodiode.x, poses["DotBotV3"].photodiode.y) == (0, 0) + + +def test_the_twin_measures_its_first_heading_from_where_it_was_created( + controller, monkeypatch +): + now = [1000.0] + monkeypatch.setattr("dotbot.controller.time.time", lambda: now[0]) + for _ in range(3): + twin = controller._update_dotbot_twin( + "AA", 60, 60, init_pos_x=1500, init_pos_y=1500, init_direction=90 + ) + now[0] += 0.5 + assert twin.pos_x < 1500 - 50 + assert twin.pos_y == pytest.approx(1500) + assert twin.direction == 90 diff --git a/dotbot/tests/test_dotbot_simulator.py b/dotbot/tests/test_dotbot_simulator.py index 8e2be476..f408ec23 100644 --- a/dotbot/tests/test_dotbot_simulator.py +++ b/dotbot/tests/test_dotbot_simulator.py @@ -8,6 +8,10 @@ from dotbot import addr_to_hex from dotbot.area import Area from dotbot.dotbot_simulator import ( + DIRECTION_THRESHOLD_MM, + MOTOR_SPEED, + SIMULATOR_STEP_DELTA_T, + SIMULATOR_UPDATE_INTERVAL_S, DotBotSimulator, DotBotSimulatorCommunicationInterface, SimulatedDotBotSettings, @@ -16,9 +20,17 @@ place_dotbots, placement_area, ) -from dotbot.protocol import PayloadCommandMoveRaw +from dotbot.protocol import ( + DIRECTION_NONE, + ControlModeType, + PayloadCommandMoveRaw, + PayloadLH2Location, + PayloadLH2Waypoints, +) from dotbot.site import Site +ADDRESS = "BADCAFE111111111" + def _bot(address: str) -> DotBotSimulator: return DotBotSimulator( @@ -71,6 +83,73 @@ def test_the_address_rendering_round_trips(): assert addr_to_hex(int(address, 16)) == address +# --- Heading ---------------------------------------------------------------- + + +def test_a_fresh_bot_has_no_heading_until_it_has_travelled_past_the_threshold(): + bot = DotBotSimulator(SimulatedDotBotSettings(address=ADDRESS), queue.Queue()) + assert bot.direction == DIRECTION_NONE + + # Started at the frame origin facing north, so pos_y is the travel so far. + bot.pwm_left = bot.pwm_right = MOTOR_SPEED + while bot.pos_y <= DIRECTION_THRESHOLD_MM: + assert bot.direction == DIRECTION_NONE + bot.diff_drive_model_update() + assert bot.direction == 0 + + +def test_the_next_heading_waits_for_another_threshold_of_travel(): + """The recorded point advances with the heading, not with every step.""" + bot = DotBotSimulator(SimulatedDotBotSettings(address=ADDRESS), queue.Queue()) + bot.pwm_left = bot.pwm_right = MOTOR_SPEED + while bot.direction == DIRECTION_NONE: + bot.diff_drive_model_update() + + bot.theta = 90 # turned east, where a recomputed heading reads -90 + bot.diff_drive_model_update() + assert bot.direction == 0 + while bot.pos_x <= DIRECTION_THRESHOLD_MM: + bot.diff_drive_model_update() + assert bot.direction == -90 + + +def _drive_to(bot: DotBotSimulator, x: int, y: int, timeout_s: float) -> None: + """Run control and physics at their real rates until the waypoint run ends.""" + waypoints = [PayloadLH2Location(pos_x=x, pos_y=y)] + _deliver( + bot, + Frame( + header=Header(destination=int(bot.address, 16), source=0), + packet=Packet().from_payload( + PayloadLH2Waypoints(threshold=50, count=1, waypoints=waypoints) + ), + ), + ) + physics_per_control = round(SIMULATOR_UPDATE_INTERVAL_S / SIMULATOR_STEP_DELTA_T) + elapsed = 0.0 + while bot.controller_mode == ControlModeType.AUTO and elapsed < timeout_s: + bot._control_loop_default() + for _ in range(physics_per_control): + bot.diff_drive_model_update() + elapsed += SIMULATOR_UPDATE_INTERVAL_S + + +@pytest.mark.parametrize("direction", [90, DIRECTION_NONE], ids=["heading", "none"]) +def test_the_default_control_loop_reaches_a_waypoint_it_must_turn_toward(direction): + """Guards against steering on the advertised heading, which a bot turning + in place never updates, so it spins without arriving.""" + bot = DotBotSimulator( + SimulatedDotBotSettings( + address=ADDRESS, pos_x=1000, pos_y=1000, direction=direction + ), + queue.Queue(), + ) + _drive_to(bot, 1000, 300, timeout_s=10) + assert bot.controller_mode == ControlModeType.MANUAL + assert (bot.pos_x - 1000) ** 2 + (bot.pos_y - 300) ** 2 < 50**2 + assert bot.direction != DIRECTION_NONE + + # --- Placement of a world file's unpositioned robots ------------------------- @@ -162,16 +241,16 @@ def test_a_fully_positioned_fleet_is_returned_unchanged(): def test_the_packaged_world_spreads_its_fleet_over_the_active_arena(): - """End to end from the shipped world file: the default four robots must - start inside the site's arena, not in a corner of the floor.""" + """End to end from the shipped world file: every declared robot must start + inside the site's arena, not in a corner of the floor.""" interface = DotBotSimulatorCommunicationInterface( on_frame_received=lambda *_: None, simulator_init_state=str(packaged_init_state_path()), site=HALL, ) arena = HALL.areas["arena"] - assert len(interface.dotbots) == 4 - assert len({(bot.pos_x, bot.pos_y) for bot in interface.dotbots}) == 4 + assert len(interface.dotbots) == 5 + assert len({(bot.pos_x, bot.pos_y) for bot in interface.dotbots}) == 5 assert all( arena.x < bot.pos_x < arena.x_max and arena.y < bot.pos_y < arena.y_max for bot in interface.dotbots @@ -183,7 +262,17 @@ def test_the_packaged_world_still_runs_with_no_site_at_all(): on_frame_received=lambda *_: None, simulator_init_state=str(packaged_init_state_path()), ) - assert len(interface.dotbots) == 4 + assert len(interface.dotbots) == 5 assert all( 0 < bot.pos_x < 2000 and 0 < bot.pos_y < 2000 for bot in interface.dotbots ) + + +def test_the_packaged_world_ships_a_robot_with_no_heading(): + """`dotbot run simulator` must be able to show the no-heading case.""" + interface = DotBotSimulatorCommunicationInterface( + on_frame_received=lambda *_: None, + simulator_init_state=str(packaged_init_state_path()), + site=HALL, + ) + assert [bot.direction for bot in interface.dotbots].count(DIRECTION_NONE) == 1 diff --git a/dotbot/tests/test_robots.py b/dotbot/tests/test_robots.py new file mode 100644 index 00000000..b9054757 --- /dev/null +++ b/dotbot/tests/test_robots.py @@ -0,0 +1,215 @@ +"""The geometry record's derived quantities, recovered from its board points.""" + +import math + +import numpy as np +import pytest +from swarmit.testbed.protocol import DeviceType + +from dotbot.robots import ( + ROBOT_DEFAULT, + ROBOTS, + SWARMIT_DEVICE_MODELS, + HeadingSource, + Point, + RobotGeometry, + robot_geometry, +) + +V3 = ROBOTS["dotbot-v3"] + + +@pytest.mark.parametrize( + "prop,expected", + [ + ("diode_to_front_mm", 18.5), + ("diode_to_rear_mm", 76.5), + ("diode_to_side_mm", 47.0), + ("led_to_front_mm", 13.0), + ("led_ahead_of_diode_mm", 5.5), + ("diode_ahead_of_centre_mm", 29.0), + ("lever_arm_mm", 53.5), + ("lever_angle_deg", 0.0), + ("board_width_mm", 94.0), + ("board_length_mm", 95.0), + ], +) +def test_v3_derived_distances(prop, expected): + assert getattr(V3, prop) == pytest.approx(expected) + + +def test_v3_reach_is_the_far_tyre_corner(): + """The rear outer corner of a tyre, not of the board, is the furthest point.""" + assert V3.reach_mm == pytest.approx(math.hypot(47.75, 75.5)) + assert V3.reach_mm == pytest.approx(89.33, abs=0.01) + + +def test_v3_core_is_the_front_edge(): + assert V3.core_mm == pytest.approx(V3.diode_to_front_mm) + assert V3.core_mm == pytest.approx(18.5) + + +def test_v3_outline(): + assert V3.outline_bbox == (28.0, 52.5, 122.0, 147.5) + assert V3.outline_centre == Point(75.0, 100.0) + assert len(V3.outline_path) == 14 + + +def test_v3_drivetrain_matches_the_c_constants(): + """`DB_MM_PER_COUNT` is about 0.0987 mm per count on a v3.""" + assert V3.mm_per_count == pytest.approx(0.098736, rel=1e-4) + + +def test_default_is_v3(): + assert robot_geometry() is V3 + assert ROBOT_DEFAULT == "dotbot-v3" + + +def test_unknown_model_raises(): + with pytest.raises(ValueError, match="unknown robot model"): + robot_geometry("dotbot-v9") + + +def test_an_off_centre_photodiode_is_refused(): + """A non-zero lever angle is not handled, so a record implying one fails.""" + with pytest.raises(ValueError, match="centreline"): + RobotGeometry( + model="off-centre", + outline_path=V3.outline_path, + photodiode=Point(76.0, 71.0), + led=V3.led, + caster=V3.caster, + axle_midpoint=V3.axle_midpoint, + track_mm=V3.track_mm, + wheel_diameter_mm=V3.wheel_diameter_mm, + tyre_width_mm=V3.tyre_width_mm, + connector_spacing_mm=V3.connector_spacing_mm, + connector_width_mm=V3.connector_width_mm, + connector_length_mm=V3.connector_length_mm, + encoder_cpr=V3.encoder_cpr, + gear_ratio=V3.gear_ratio, + envelope_mm=V3.envelope_mm, + ) + + +# --- the expansion from the photodiode to the body -------------------------- + +SENSOR = Point(1000.0, 1000.0) + + +@pytest.mark.parametrize( + "heading,centre,nose", + [ + (0.0, (1000.0, 971.0), (1000.0, 1018.5)), + (90.0, (1029.0, 1000.0), (981.5, 1000.0)), + (180.0, (1000.0, 1029.0), (1000.0, 981.5)), + (270.0, (971.0, 1000.0), (1018.5, 1000.0)), + ], +) +def test_the_centre_is_29_mm_behind_the_sensor(heading, centre, nose): + """Body-forward at heading theta is (-sin, +cos).""" + pose = V3.body_pose(SENSOR, heading, HeadingSource.TRAVEL) + assert pose.centre == pytest.approx(centre) + assert pose.nose == pytest.approx(nose) + forward = (-math.sin(math.radians(heading)), math.cos(math.radians(heading))) + assert pose.axle == pytest.approx( + (SENSOR.x - 53.5 * forward[0], SENSOR.y - 53.5 * forward[1]) + ) + assert pose.led == pytest.approx( + (SENSOR.x + 5.5 * forward[0], SENSOR.y + 5.5 * forward[1]) + ) + assert pose.heading_deg == heading + assert pose.heading_source == HeadingSource.TRAVEL + + +@pytest.mark.parametrize("heading", [0.0, 37.0, 90.0, 213.0]) +def test_a_pose_s_radii_hold_whatever_the_heading(heading): + pose = V3.body_pose(SENSOR, heading, HeadingSource.TRAVEL) + points = [*pose.outline, *(p for w in pose.wheels for p in w)] + far = max(math.hypot(p.x - SENSOR.x, p.y - SENSOR.y) for p in points) + assert pose.reach_mm == pytest.approx(far) + assert pose.core_mm == pytest.approx(18.5) + assert pose.envelope_mm == V3.envelope_mm + + +@pytest.mark.parametrize("heading", [0.0, 37.0, 90.0]) +def test_the_pose_places_the_photodiode_on_the_sensor(heading): + pose = V3.body_pose(SENSOR, heading, HeadingSource.TRAVEL) + assert pose.photodiode == SENSOR + + +def test_the_robot_s_left_side_is_on_its_left(): + """At heading 0 the robot faces +y; seen from above with y down, its left + is +x. The board's left edge is its low-x edge.""" + pose = V3.body_pose(SENSOR, 0.0, HeadingSource.TRAVEL) + left_edge = [p for q, p in zip(V3.outline_path, pose.outline) if q.x == 28.0] + right_edge = [p for q, p in zip(V3.outline_path, pose.outline) if q.x == 122.0] + assert all(p.x == pytest.approx(SENSOR.x + 47.0) for p in left_edge) + assert all(p.x == pytest.approx(SENSOR.x - 47.0) for p in right_edge) + + +@pytest.mark.parametrize("heading", [0.0, 37.0, 90.0, -123.0, 180.0]) +def test_the_body_is_the_camera_detector_s_body(heading): + """The detector draws the same board and the same tyres from its centre + and its own heading convention; the two must agree point for point.""" + from dotbot.camera.detection.pose import OUTLINE_MM, WHEELS_MM, axes + + pose = V3.body_pose(SENSOR, heading, HeadingSource.TRAVEL) + right, forward = axes(heading + 90.0) + centre = np.asarray(pose.centre) + + def detector(path): + return [centre + p[0] * right + p[1] * forward for p in path] + + assert np.allclose(np.asarray(pose.outline), detector(OUTLINE_MM), atol=1e-9) + assert len(pose.wheels) == len(WHEELS_MM) + for wheel, tyre in zip(pose.wheels, WHEELS_MM): + assert np.allclose(np.asarray(wheel), detector(tyre), atol=1e-9) + + +def test_an_unknown_model_has_no_body(): + with pytest.raises(ValueError): + robot_geometry("dotbot-v2").body_pose(SENSOR, 0.0, HeadingSource.TRAVEL) + + +def test_a_wheel_is_the_record_s_track_width_and_diameter(): + """The plan-view tyre rectangle is the drivetrain scalars and nothing else, + so the console and the detector can draw the same wheel.""" + left, right = V3.wheel_paths + for wheel, sign in ((left, -1), (right, 1)): + xs = [p.x for p in wheel] + ys = [p.y for p in wheel] + assert max(xs) - min(xs) == pytest.approx(V3.tyre_width_mm) + assert max(ys) - min(ys) == pytest.approx(V3.wheel_diameter_mm) + assert (min(xs) + max(xs)) / 2 == pytest.approx( + V3.axle_midpoint.x + sign * V3.track_mm / 2 + ) + assert (min(ys) + max(ys)) / 2 == pytest.approx(V3.axle_midpoint.y) + + +def test_the_wheels_stand_in_the_notches_beside_the_rear_tab(): + """A tyre is outside the board's rear tab and inside its widest part, which + is why it shows on a map that draws the board over it.""" + x_min, _, x_max, _ = V3.outline_bbox + tab = [p.x for p in V3.outline_path if p.y > V3.axle_midpoint.y] + for wheel in V3.wheel_paths: + xs = [p.x for p in wheel] + assert min(xs) >= x_min - V3.tyre_width_mm / 2 + assert max(xs) <= x_max + V3.tyre_width_mm / 2 + assert min(xs) > max(tab) or max(xs) < min(tab) + + +def test_a_pose_carries_its_wheels_where_it_carries_its_board(): + """The wheels arrive placed in the arena frame, like the outline: the + console is never asked to rotate a rectangle of its own.""" + pose = V3.body_pose(SENSOR, 0.0, HeadingSource.TRAVEL) + assert len(pose.wheels) == len(V3.wheel_paths) + # Heading 0 faces +y and the robot's left is +x, so the left wheel's + # centre lands half a track to +x of the axle. + left = pose.wheels[0] + assert sum(p.x for p in left) / 4 == pytest.approx(pose.axle.x + V3.track_mm / 2) + assert sum(p.y for p in left) / 4 == pytest.approx(pose.axle.y) + + +def test_each_swarmit_device_model_is_keyed_by_a_swarmit_device_type(): + assert set(SWARMIT_DEVICE_MODELS) <= {d.name for d in DeviceType} diff --git a/dotbot/tests/test_server.py b/dotbot/tests/test_server.py index 80a8ca27..bb9f745e 100644 --- a/dotbot/tests/test_server.py +++ b/dotbot/tests/test_server.py @@ -8,7 +8,7 @@ from httpx import ASGITransport, AsyncClient from dotbot.area import Area -from dotbot.controller import ControllerSettings +from dotbot.controller import ControllerSettings, device_pose from dotbot.models import ( DotBotGPSPosition, DotBotLH2Position, @@ -981,6 +981,21 @@ async def test_the_area_routes_are_gone(method): assert response.status_code == 404 +@pytest.mark.asyncio +async def test_get_device_poses(): + """The console sizes a robot placed from swarmit's STATUS from these.""" + origin = DotBotLH2Position(x=0, y=0) + api.controller.device_poses.return_value = { + "DotBotV3": device_pose("DotBotV3", origin) + } + response = await client.get("/controller/device_poses") + assert response.status_code == 200 + body = response.json() + assert list(body) == ["DotBotV3"] + assert body["DotBotV3"]["heading_source"] == "none" + assert body["DotBotV3"]["photodiode"] == {"x": 0, "y": 0} + + @pytest.mark.asyncio async def test_get_controller_site(): """The console draws the whole site, so it needs the extent and the areas.""" diff --git a/simulator_init_state.toml b/simulator_init_state.toml index 445d3f58..8c5e7f9a 100644 --- a/simulator_init_state.toml +++ b/simulator_init_state.toml @@ -31,3 +31,8 @@ direction = 180 [[dotbots]] address = "BADC0DE444444444" direction = 360 + +# No `direction`: this robot advertises no heading until it has travelled far +# enough for one, as a real robot does from boot until its first fix. +[[dotbots]] +address = "F00DFACE55555555"