dotbot: expand each LH2 fix into a body pose and draw the robot from it

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
         )

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

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),

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