Update vision to (mostly) match Zoloto

controllers/example_controller/keyboard_controller.py

@@ -1,3 +1,5 @@
+import math
+
 from sr.robot3 import *
 from controller import Keyboard
 
@@ -15,9 +17,21 @@
     "boost": (Keyboard.SHIFT, Keyboard.CONTROL),
     "grabber_open": (ord("R"), ord("P")),
     "grabber_close": (ord("E"), ord("O")),
+    "angle_unit": (ord("B"), ord("B")),
 }
 
 
+USE_DEGREES = False
+
+
+def angle_str(angle: float) -> str:
+    if USE_DEGREES:
+        degrees = math.degrees(angle)
+        return f"{degrees:.2g}°"
+
+    return f"{angle:.4g} rad"
+
+
 def print_sensors(robot: Robot) -> None:
     distance_sensor_names = {
         A0: "Front Left",
@@ -41,6 +55,31 @@ def print_sensors(robot: Robot) -> None:
         touching = R.ruggeduino.pins[pin].digital_read()
         print(f"{pin} {name: <6}: {touching}")
 
+    try:
+        camera = R.camera
+    except ValueError:
+        print("No camera on this robot")
+    else:
+        markers = camera.see()
+        if markers:
+            print(f"Found {len(markers)} makers:")
+            for marker in markers:
+                print(f" #{marker.id}")
+                x, y, z = marker.cartesian
+                print(f" Cartesian: {x:.4g}, {y:.4g}, {z:.4g}")
+                rot_x, rot_y, dist = marker.spherical
+                print(
+                    f" Spherical: {angle_str(rot_x)}, {angle_str(rot_y)}, {dist}",
+                )
+                rot_x, rot_y, rot_z = marker.orientation
+                print(
+                    f" Orientation: {angle_str(rot_x)}, {angle_str(rot_y)}, "
+                    f"{angle_str(rot_z)}",
+                )
+                print()
+        else:
+            print("No markers")
+
     print()
 
 
@@ -57,6 +96,7 @@ def print_sensors(robot: Robot) -> None:
 key_boost = CONTROLS["boost"][R.zone]
 key_grab_open = CONTROLS["grabber_open"][R.zone]
 key_grab_close = CONTROLS["grabber_close"][R.zone]
+key_angle_unit = CONTROLS["angle_unit"][R.zone]
 
 print(
     "Note: you need to click on 3D viewport for keyboard events to be picked "
@@ -106,6 +146,9 @@ def print_sensors(robot: Robot) -> None:
             R.servo_board.servos[0].position = 1
             R.servo_board.servos[1].position = 1
 
+        elif key_ascii == key_angle_unit:
+            USE_DEGREES = not USE_DEGREES
+
         # Work our way through all the enqueued key presses before dropping
         # out to the timestep
         key = keyboard.getKey()

modules/sr/robot3/__init__.py

@@ -2,12 +2,6 @@
 from sr.robot3.motor import BRAKE, COAST
 from sr.robot3.power import Note, Outputs
 from sr.robot3.robot import Robot
-from sr.robot3.camera import (
-    MarkerType,
-    MARKER_ARENA,
-    MARKER_TOKEN_GOLD,
-    MARKER_TOKEN_SILVER,
-)
 from sr.robot3.metadata import RobotMode
 from sr.robot3.ruggeduino import AnaloguePin, GPIOPinMode
 
@@ -55,8 +49,4 @@
     'OUT_L1',
     'OUT_L3',
     'OUT_FIVE_VOLT',
-    'MarkerType',
-    'MARKER_ARENA',
-    'MARKER_TOKEN_GOLD',
-    'MARKER_TOKEN_SILVER',
 )

modules/sr/robot3/camera.py

@@ -1,65 +1,86 @@
 from __future__ import annotations
 
 import re
+import enum
+import functools
 import threading
-from enum import Enum
-from typing import NamedTuple
+from typing import Container, NamedTuple
 
 from controller import Robot, Camera as WebotCamera
-from sr.robot3.vision import Face, Orientation, tokens_from_objects
-from sr.robot3.coordinates import Point
+from sr.robot3.vision import (
+    Face,
+    Token,
+    FlatToken,
+    Orientation,
+    tokens_from_objects,
+)
+from sr.robot3.coordinates import (
+    Vector,
+    Spherical,
+    ThreeDCoordinates,
+    spherical_from_cartesian,
+)
 
 from .utils import maybe_get_robot_device
 
-MARKER_MODEL_RE = re.compile(r"^[AGS]\d{0,2}$")
+MARKER_MODEL_RE = re.compile(r"^[FB]\d{0,2}$")
 
 
-class MarkerType(Enum):
-    ARENA = "ARENA"
-    GOLD = "TOKEN_GOLD"
-    SILVER = "TOKEN_SILVER"
-
-
-# Existing token types
-MARKER_ARENA = MarkerType.ARENA
-MARKER_TOKEN_GOLD = MarkerType.GOLD
-MARKER_TOKEN_SILVER = MarkerType.SILVER
+# Zoloto's markers API doesn't expose any information derived from the marker's
+# identity, however we need to track whether the marker is of a kind that would
+# be on a box or a wall and its size. This enum lets us do that.
+class ObjectType(enum.Enum):
+    FLAT = 'F'
+    BOX = 'B'
 
 
 class MarkerInfo(NamedTuple):
     code: int
-    marker_type: MarkerType
-    offset: int
-    size: float
+    size_mm: int
+    object_type: ObjectType
 
+    @property
+    def size_m(self) -> float:
+        # Webots uses metres.
+        return self.size_mm / 1000
 
-MARKER_MODEL_TYPE_MAP = {
-    'A': MarkerType.ARENA,
-    'G': MarkerType.GOLD,
-    'S': MarkerType.SILVER,
-}
+    def get_token_class(self) -> type[Token]:
+        if self.object_type == ObjectType.BOX:
+            return Token
+        if self.object_type == ObjectType.FLAT:
+            # See class docstring for how this works and coupling to the proto files
+            return FlatToken
 
-MARKER_TYPE_OFFSETS = {
-    MarkerType.ARENA: 0,
-    MarkerType.GOLD: 32,
-    MarkerType.SILVER: 40,
-}
+        raise AssertionError("Unknown object type")
 
-MARKER_TYPE_SIZE = {
-    MarkerType.ARENA: 0.25,
-    MarkerType.GOLD: 0.2,
-    MarkerType.SILVER: 0.2,
+
+MARKER_SIZES: dict[Container[int], int] = {
+    range(28): 200,  # 0 - 27 for arena boundary
+    range(28, 100): 100,  # Everything else is a token
 }
 
 
+def get_marker_size_mm(marker_id: int) -> int:
+    """
+    Return the marker size in millimetres.
+    """
+    for bucket, size in MARKER_SIZES.items():
+        if marker_id in bucket:
+            return size
+
+    raise ValueError(f"Unknown marker id {marker_id}")
+
+
 def parse_marker_info(model_id: str) -> MarkerInfo | None:
     """
     Parse the model id of a maker model into a `MarkerInfo`.
 
     Expected input format is a letter and two digits. The letter indicates the
-    type of the marker, the digits its "libkoki" 'code'.
+    type of the marker, indicating whether or not the marker is on a flat
+    object. The digits which form the 'code' are used for determining the
+    properties visible in the API.
 
-    Examples: 'A00', 'A01', ..., 'G32', 'G33', ..., 'S40', 'S41', ...
+    Examples: 'F00', 'F01', ..., 'B32', 'B33', ...
     """
 
     match = MARKER_MODEL_RE.match(model_id)
@@ -68,67 +89,69 @@ def parse_marker_info(model_id: str) -> MarkerInfo | None:
 
     kind, number = model_id[0], model_id[1:]
 
-    marker_type = MARKER_MODEL_TYPE_MAP[kind]
     code = int(number)
 
-    type_offset = MARKER_TYPE_OFFSETS[marker_type]
-
     return MarkerInfo(
         code=code,
-        marker_type=marker_type,
-        offset=code - type_offset,
-        size=MARKER_TYPE_SIZE[marker_type],
+        size_mm=get_marker_size_mm(code),
+        object_type=ObjectType(kind),
     )
 
 
 class Marker:
     # Note: properties in the same order as in the docs.
-    # Note: we are _not_ supporting image-related properties, so no `res`.
+    # Note: we are _not_ supporting image-related properties, so no `pixel_*`.
 
     def __init__(self, face: Face, marker_info: MarkerInfo, timestamp: float) -> None:
         self._face = face
 
-        self.info = marker_info
+        self._info = marker_info
         self.timestamp = timestamp
 
     def __repr__(self) -> str:
-        return '<Marker: {}>'.format(', '.join((
-            f'info={self.info}',
-            f'centre={self.centre}',
-            f'dist={self.dist}',
-            f'orientation={self.orientation}',
+        return '<Marker {}>'.format(' '.join((
+            f'id={self.id!r}',
+            f'size={self.size!r}',
+            f'distance={self.distance!r}',
+            f'rot_y={self.spherical.rot_y!r}',
         )))
 
     @property
-    def centre(self) -> Point:
-        """A `Point` describing the position of the centre of the marker."""
-        return Point.from_vector(self._face.centre_global())
+    def id(self) -> int:  # noqa:A003
+        return self._info.code
 
     @property
-    def vertices(self) -> list[Point]:
-        """
-        A list of 4 `Point` instances, each representing the position of the
-        black corners of the marker.
-        """
-        # Note quite the black corners of the marker, though fairly close --
-        # actually the corners of the face of the modelled token.
-        return [Point.from_vector(x) for x in self._face.corners_global().values()]
+    def size(self) -> int:
+        return self._info.size_mm
 
-    @property
-    def dist(self) -> float:
-        """An alias for `centre.polar.length`."""
-        return self._face.centre_global().magnitude()
+    # No pixel values as there is no underlying image.
+
+    @functools.cached_property
+    def _position(self) -> Vector:
+        # Webots uses metres, Zoloto uses millimetres. Convert before exposing
+        # from our simulated API.
+        return self._face.centre_global() * 1000
 
     @property
-    def rot_y(self) -> float:
-        """An alias for `centre.polar.rot_y`."""
-        return self.centre.polar.rot_y
+    def distance(self) -> float:
+        """Distance to the centre of the marker, in millimetres."""
+        return self._position.magnitude()
 
     @property
     def orientation(self) -> Orientation:
         """An `Orientation` instance describing the orientation of the marker."""
         return self._face.orientation()
 
+    @property
+    def spherical(self) -> Spherical:
+        """A `Spherical` instance describing the position relative to the camera."""
+        return spherical_from_cartesian(self._position)
+
+    @property
+    def cartesian(self) -> ThreeDCoordinates:
+        """An `ThreeDCoordinates` instance describing the position relative to the camera."""
+        return ThreeDCoordinates(*self._position.data)
+
 
 class Camera:
     def __init__(self, webot: Robot, camera: WebotCamera, lock: threading.Lock) -> None:
@@ -162,14 +185,15 @@ def _see(self) -> list[Marker]:
 
         for recognition_object in self.camera.getRecognitionObjects():
             marker_info = parse_marker_info(
-                recognition_object.get_model().decode(errors='replace'),
+                recognition_object.getModel().decode(errors='replace'),
             )
             if marker_info:
                 object_infos[recognition_object] = marker_info
 
         tokens = tokens_from_objects(
             object_infos.keys(),
-            lambda o: object_infos[o].size,
+            lambda o: object_infos[o].size_m,
+            lambda o: object_infos[o].get_token_class(),
         )
 
         when = self._webot.getTime()
@@ -178,8 +202,7 @@ def _see(self) -> list[Marker]:
 
         for token, recognition_object in tokens:
             marker_info = object_infos[recognition_object]
-            is_2d = marker_info.marker_type == MarkerType.ARENA
-            for face in token.visible_faces(is_2d=is_2d):
+            for face in token.visible_faces():
                 markers.append(Marker(face, marker_info, when))
 
         return markers
@@ -189,7 +212,7 @@ def see_ids(self) -> list[int]:
         # speed doesn't matter much in the simulator and with the locking we
         # need to do it's much easier to let this be a shallow wrapper around
         # the full implementation.
-        return [x.info.code for x in self.see()]
+        return [x._info.code for x in self.see()]
 
     # The simulator does not emulate the `capture` or `save` methods.
 

modules/sr/robot3/coordinates/__init__.py

@@ -1,13 +1,12 @@
 from __future__ import annotations
 
-from .polar import PolarCoord, polar_from_cartesian
 from .vectors import Vector
-from .coordinates import Point, Cartesian
+from .cartesian import ThreeDCoordinates
+from .twin_angle import Spherical, spherical_from_cartesian
 
 __all__ = (
-    'Point',
     'Vector',
-    'Cartesian',
-    'PolarCoord',
-    'polar_from_cartesian',
+    'ThreeDCoordinates',
+    'Spherical',
+    'spherical_from_cartesian',
 )

modules/sr/robot3/coordinates/cartesian.py

@@ -0,0 +1,11 @@
+from __future__ import annotations
+
+from typing import NamedTuple
+
+
+class ThreeDCoordinates(NamedTuple):
+    """Analogue of zoloto's twin-angle `ThreeDCoordinates` type."""
+
+    x: float
+    y: float
+    z: float