sr-robot3 2024.0.0rc2__py3-none-any.whl → 2025.0.0__py3-none-any.whl

sr/robot3/_version.py

@@ -1,4 +1,16 @@
 # file generated by setuptools_scm
 # don't change, don't track in version control
-__version__ = version = '2024.0.0rc2'
-__version_tuple__ = version_tuple = (2024, 0, 0)
+TYPE_CHECKING = False
+if TYPE_CHECKING:
+    from typing import Tuple, Union
+    VERSION_TUPLE = Tuple[Union[int, str], ...]
+else:
+    VERSION_TUPLE = object
+
+version: str
+__version__: str
+__version_tuple__: VERSION_TUPLE
+version_tuple: VERSION_TUPLE
+
+__version__ = version = '2025.0.0'
+__version_tuple__ = version_tuple = (2025, 0, 0)

sr/robot3/arduino.py

@@ -11,10 +11,18 @@ from serial.tools.list_ports import comports
 from .exceptions import IncorrectBoardError
 from .logging import log_to_debug
 from .serial_wrapper import SerialWrapper
-from .utils import Board, BoardIdentity, get_USB_identity, map_to_float
+from .utils import (
+    IN_SIMULATOR, Board, BoardIdentity,
+    get_simulator_boards, get_USB_identity, map_to_float,
+)
 
 logger = logging.getLogger(__name__)
 BAUDRATE = 115200
+if IN_SIMULATOR:
+    # Place each command on a new line in the simulator to simplify the implementation
+    ENDLINE = '\n'
+else:
+    ENDLINE = ''
 
 SUPPORTED_VID_PIDS = {
     (0x2341, 0x0043),  # Arduino Uno rev 3
@@ -58,7 +66,7 @@ class Arduino(Board):
     """
     The Arduino board interface.
 
-    This is intended to be used with Arduino Uno boards running the sbot firmware.
+    This is intended to be used with Arduino Uno boards running the SR firmware.
 
     :param serial_port: The serial port to connect to.
     :param initial_identity: The identity of the board, as reported by the USB descriptor.
@@ -127,17 +135,46 @@ class Arduino(Board):
                 f"expected {err.expected_type!r}. Ignoring this device")
             return None
         except Exception:
-            if initial_identity is not None and initial_identity.board_type == 'manual':
-                logger.warning(
-                    f"Manually specified Arduino at port {serial_port!r}, "
-                    "could not be identified. Ignoring this device")
-            else:
-                logger.warning(
-                    f"Found Arduino-like serial port at {serial_port!r}, "
-                    "but it could not be identified. Ignoring this device")
+            if initial_identity is not None:
+                if initial_identity.board_type == 'manual':
+                    logger.warning(
+                        f"Manually specified Arduino at port {serial_port!r} "
+                        "could not be identified. Ignoring this device")
+                elif initial_identity.manufacturer == 'sbot_simulator':
+                    logger.warning(
+                        f"Simulator specified arduino at port {serial_port!r} "
+                        "could not be identified. Ignoring this device")
+                return None
+
+            logger.warning(
+                f"Found Arduino-like serial port at {serial_port!r}, "
+                "but it could not be identified. Ignoring this device")
             return None
         return board
 
+    @classmethod
+    def _get_simulator_boards(cls) -> MappingProxyType[str, Arduino]:
+        """
+        Get the simulator boards.
+
+        :return: A mapping of board serial numbers to Arduinos
+        """
+        boards = {}
+        # The filter here is the name of the emulated board in the simulator
+        for board_info in get_simulator_boards('Arduino'):
+
+            # Create board identity from the info given
+            initial_identity = BoardIdentity(
+                manufacturer='sbot_simulator',
+                board_type=board_info.type_str,
+                asset_tag=board_info.serial_number,
+            )
+            if (board := cls._get_valid_board(board_info.url, initial_identity)) is None:
+                continue
+
+            boards[board._identity.asset_tag] = board
+        return MappingProxyType(boards)
+
     @classmethod
     def _get_supported_boards(
         cls,
@@ -149,8 +186,12 @@ class Arduino(Board):
 
         :param manual_boards: A list of manually specified board port strings,
             defaults to None
+        :param ignored_serials: A list of serial number to ignore during board discovery
         :return: A mapping of board serial numbers to Arduinos
         """
+        if IN_SIMULATOR:
+            return cls._get_simulator_boards()
+
         boards = {}
         if ignored_serials is None:
             ignored_serials = []
@@ -191,7 +232,7 @@ class Arduino(Board):
 
         :return: The identity of the board.
         """
-        response = self._serial.query('v', endl='')
+        response = self._serial.query('v', endl=ENDLINE)
         response_fields = response.split(':')
 
         # The arduino firmware cannot access the serial number reported in the USB descriptor
@@ -223,13 +264,16 @@ class Arduino(Board):
         :param command: The command to send to the board.
         :return: The response from the board.
         """
-        return self._serial.query(command, endl='')
+        if IN_SIMULATOR:
+            logger.warning("The command method is not fully supported in the simulator")
+        return self._serial.query(command, endl=ENDLINE)
 
     def map_pin_number(self, pin_number: int) -> str:
         """
         Map the pin number to the the serial format.
         Pin numbers are sent as printable ASCII characters, with 0 being 'a'.
 
+        :param pin_number: The pin number to encode.
         :return: The pin number in the serial format.
         :raises ValueError: If the pin number is invalid.
         """
@@ -239,6 +283,34 @@ class Arduino(Board):
             raise ValueError("Invalid pin provided") from None
         return chr(pin_number + ord('a'))
 
+    @log_to_debug
+    def ultrasound_measure(
+        self,
+        pulse_pin: int,
+        echo_pin: int,
+    ) -> int:
+        """
+        Measure the distance to an object using an ultrasound sensor.
+
+        The sensor can only measure distances up to 4m.
+
+        :param pulse_pin: The pin to send the ultrasound pulse from.
+        :param echo_pin: The pin to read the ultrasound echo from.
+        :raises ValueError: If either of the pins are invalid
+        :return: The distance measured by the ultrasound sensor in mm.
+        """
+        try:  # bounds check
+            pulse_id = self.map_pin_number(pulse_pin)
+        except ValueError:
+            raise ValueError("Invalid pulse pin provided") from None
+        try:
+            echo_id = self.map_pin_number(echo_pin)
+        except ValueError:
+            raise ValueError("Invalid echo pin provided") from None
+
+        response = self._serial.query(f'u{pulse_id}{echo_id}', endl=ENDLINE)
+        return int(response)
+
     def __repr__(self) -> str:
         return f"<{self.__class__.__qualname__}: {self._serial}>"
 
@@ -250,6 +322,7 @@ class Pin:
     :param serial: The serial wrapper to use to communicate with the board.
     :param index: The index of the pin.
     :param supports_analog: Whether the pin supports analog reads.
+    :param disabled: Whether the pin can be controlled.
     """
     __slots__ = ('_serial', '_index', '_supports_analog', '_disabled', '_mode')
 
@@ -300,7 +373,7 @@ class Pin:
         mode_char = MODE_CHAR_MAP.get(value)
         if mode_char is None:
             raise IOError(f'Pin mode {value} is not supported')
-        self._serial.write(self._build_command(mode_char), endl='')
+        self._serial.write(self._build_command(mode_char), endl=ENDLINE)
         self._mode = value
 
     @log_to_debug
@@ -315,7 +388,7 @@ class Pin:
         self._check_if_disabled()
         if self.mode not in DIGITAL_READ_MODES:
             raise IOError(f'Digital read is not supported in {self.mode}')
-        response = self._serial.query(self._build_command('r'), endl='')
+        response = self._serial.query(self._build_command('r'), endl=ENDLINE)
         return response == 'h'
 
     @log_to_debug
@@ -331,9 +404,9 @@ class Pin:
         if self.mode not in DIGITAL_WRITE_MODES:
             raise IOError(f'Digital write is not supported in {self.mode}')
         if value:
-            self._serial.write(self._build_command('h'), endl='')
+            self._serial.write(self._build_command('h'), endl=ENDLINE)
         else:
-            self._serial.write(self._build_command('l'), endl='')
+            self._serial.write(self._build_command('l'), endl=ENDLINE)
 
     @log_to_debug
     def analog_read(self) -> float:
@@ -354,7 +427,7 @@ class Pin:
             raise IOError(f'Analog read is not supported in {self.mode}')
         if not self._supports_analog:
             raise IOError('Pin does not support analog read')
-        response = self._serial.query(self._build_command('a'), endl='')
+        response = self._serial.query(self._build_command('a'), endl=ENDLINE)
         # map the response from the ADC range to the voltage range
         return map_to_float(int(response), ADC_MIN, ADC_MAX, 0.0, 5.0)
 
@@ -380,7 +453,7 @@ class Pin:
         """
         Generate the command to send to the board.
 
-        :param command: The command character to send.
+        :param cmd_char: The command character to send.
         :return: The command string.
         """
         return f'{cmd_char}{self._map_pin_number()}'
@@ -393,13 +466,6 @@ class Pin:
         )
 
 
-# PIN:<n>:MODE:GET?
-# PIN:<n>:MODE:SET:<value>
-# PIN:<n>:DIGITAL:GET?
-# PIN:<n>:DIGITAL:SET:<1/0>
-# PIN:<n>:ANALOG:GET?
-# ULTRASOUND:<pulse>:<echo>:MEASURE?
-
 if __name__ == '__main__':  # pragma: no cover
     arduinos = Arduino._get_supported_boards()
     for serial_num, board in arduinos.items():

sr/robot3/astoria.py

@@ -9,8 +9,9 @@ from threading import Event, Lock
 from time import sleep
 from typing import Any, ClassVar, NewType, Optional, Tuple
 
+import paho.mqtt.client as mqtt
 from paho.mqtt.client import Client as MQTT
-from paho.mqtt.client import MQTTMessage, MQTTv5, MQTTv311
+from paho.mqtt.client import MQTTMessage
 from pydantic import BaseModel, ValidationError
 
 from .mqtt import MQTTClient
@@ -278,7 +279,11 @@ def init_mqtt(config: AstoriaConfig, client_name: str = 'sr-robot3') -> 'MQTTCli
         host=config.mqtt.host,
         port=config.mqtt.port,
         client_name=client_name,
-        mqtt_version=MQTTv311 if config.mqtt.force_protocol_version_3_1 else MQTTv5,
+        mqtt_version=(
+            mqtt.MQTTProtocolVersion.MQTTv311
+            if config.mqtt.force_protocol_version_3_1 else
+            mqtt.MQTTProtocolVersion.MQTTv5
+        ),
         topic_prefix=config.mqtt.topic_prefix,
     )
     return client

sr/robot3/camera.py

@@ -1,9 +1,11 @@
 """An implementation of a camera board using the april_vision library."""
+from __future__ import annotations
+
 import logging
 from pathlib import Path
 from typing import Callable, Dict, Iterable, List, Optional, Union
 
-from april_vision import CalibratedCamera, Frame
+from april_vision import CalibratedCamera, Frame, FrameSource
 from april_vision import Marker as AprilMarker
 from april_vision import (
     Processor, USBCamera, __version__, calibrations,
@@ -13,8 +15,11 @@ from april_vision.helpers import Base64Sender
 from numpy.typing import NDArray
 
 from .marker import Marker
-from .utils import Board, BoardIdentity
+from .utils import (
+    IN_SIMULATOR, Board, BoardIdentity, BoardInfo, get_simulator_boards,
+)
 
+PathLike = Union[Path, str]
 LOGGER = logging.getLogger(__name__)
 
 robot_calibrations = calibrations.copy()
@@ -30,9 +35,11 @@ class AprilCamera(Board):
     in order to determine the spatial positon and orientation of the markers
     that it has detected.
 
-    :param camera_id: The index of the camera to use.
-    :param camera_data: The calibration data for the camera.
+    :param camera_source: The source of the camera frames.
+    :param calibration: The intrinsic calibration of the camera.
     :param serial_num: The serial number of the camera.
+    :param name: The name of the camera.
+    :param vidpid: The VID:PID of the camera.
     """
     __slots__ = ('_serial_num', '_cam')
 
@@ -56,28 +63,82 @@ class AprilCamera(Board):
 
         :return: A dict of cameras, keyed by their name and index.
         """
+        if IN_SIMULATOR:
+            return {
+                camera_info.serial_number: cls.from_webots_camera(camera_info)
+                for camera_info in get_simulator_boards('CameraBoard')
+            }
+
         return {
             (serial := f"{camera_data.name} - {camera_data.index}"):
-            cls(camera_data.index, camera_data=camera_data, serial_num=serial)
+            cls.from_id(camera_data.index, camera_data=camera_data, serial_num=serial)
             for camera_data in find_cameras(robot_calibrations)
         }
 
-    def __init__(self, camera_id: int, camera_data: CalibratedCamera, serial_num: str) -> None:
+    def __init__(
+        self, camera_source: FrameSource,
+        calibration: tuple[float, float, float, float] | None,
+        serial_num: str,
+        name: str,
+        vidpid: str = "",
+    ) -> None:
+        # The processor handles the detection and pose estimation
+        self._cam = Processor(
+            camera_source,
+            calibration=calibration,
+            name=name,
+            vidpid=vidpid,
+            mask_unknown_size_tags=True,
+        )
+        self._serial_num = serial_num
+
+    @classmethod
+    def from_webots_camera(cls, camera_info: BoardInfo) -> 'AprilCamera':
+        """
+        Create a camera from a webots camera.
+
+        :param camera_info: The information about the virtual camera,
+                            including the url to connect to.
+        :return: The camera object.
+        """
+        from .simulator.camera import WebotsRemoteCameraSource
+
+        camera_source = WebotsRemoteCameraSource(camera_info)
+        return cls(
+            camera_source,
+            calibration=camera_source.calibration,
+            serial_num=camera_info.serial_number,
+            name=camera_info.serial_number,
+        )
+
+    @classmethod
+    def from_id(
+        cls,
+        camera_id: int,
+        camera_data: CalibratedCamera,
+        serial_num: str,
+    ) -> 'AprilCamera':
+        """
+        Create a camera from an ID.
+
+        :param camera_id: The ID of the camera to create.
+        :param camera_data: The calibration data for the camera.
+        :param serial_num: The serial number of the camera.
+        :return: The camera object.
+        """
         # The camera source handles the connection between the camera and the processor
         camera_source = USBCamera.from_calibration_file(
             camera_id,
             calibration_file=camera_data.calibration,
             vidpid=camera_data.vidpid,
         )
-        # The processor handles the detection and pose estimation
-        self._cam = Processor(
+        return cls(
             camera_source,
             calibration=camera_source.calibration,
+            serial_num=serial_num,
             name=camera_data.name,
             vidpid=camera_data.vidpid,
-            mask_unknown_size_tags=True,
         )
-        self._serial_num = serial_num
 
     def identify(self) -> BoardIdentity:
         """
@@ -103,34 +164,41 @@ class AprilCamera(Board):
         """
         self._cam.close()
 
-    def see(self, *, frame: Optional[NDArray] = None) -> List[Marker]:
+    def see(
+        self,
+        *,
+        frame: Optional[NDArray] = None,
+        save: Optional[PathLike] = None,
+    ) -> List[Marker]:
         """
         Capture an image and identify fiducial markers.
 
         :param frame: An image to detect markers in, instead of capturing a new one,
+        :param save: If given, save the annotated frame to the path.
+                     This is given a JPEG extension if none is provided.
         :returns: list of markers that the camera could see.
         """
+        if frame is None:
+            frame = self._cam.capture()
+
         markers = self._cam.see(frame=frame)
+
+        if save:
+            self._cam.save(save, frame=frame, detections=markers)
         return [Marker.from_april_vision_marker(marker) for marker in markers]
 
-    def capture(self) -> NDArray:
+    def capture(self, *, save: Optional[PathLike] = None) -> NDArray:
         """
         Get the raw image data from the camera.
 
+        :param save: If given, save the annotated frame to the path.
+                     This is given a JPEG extension if none is provided.
         :returns: Camera pixel data
         """
-        return self._cam.capture()
-
-    def save(self, path: Union[Path, str], *, frame: Optional[NDArray] = None) -> None:
-        """
-        Save an annotated image to a path.
-
-        :param path: The path to save the image to,
-            this is given a JPEG extension if none is provided.
-        :param frame: An image to annotate and save, instead of capturing a new one,
-            defaults to None
-        """
-        self._cam.save(path, frame=frame)
+        raw_frame = self._cam.capture()
+        if save:
+            self._cam.save(save, frame=raw_frame, annotated=False)
+        return raw_frame
 
     def _set_marker_sizes(
         self,