First commit

simulator/modules/robot_logging.py

@@ -0,0 +1,132 @@
+"""
+Classes and functions for logging robot data to a file.
+
+Includes utilities for prefixing log lines and teeing a stream to multiple
+destinations.
+"""
+from __future__ import annotations
+
+import sys
+from datetime import datetime
+from io import TextIOWrapper
+from pathlib import Path
+from typing import Callable, TextIO
+
+from robot_utils import get_match_data
+
+DATE_IDENTIFIER = datetime.now().strftime("%Y_%m_%dT%H_%M_%S")
+
+
+class Tee(TextIOWrapper):
+    """Forwards calls from its `write` and `flush` methods to each of the given targets."""
+
+    def __init__(self, *streams: TextIO) -> None:
+        self.streams = streams
+
+    def write(self, data: str, /) -> int:
+        """
+        Writes the given data to all streams in the logger.
+
+        :param data: The data to be written to the stream.
+        """
+        written = 0
+        for stream in self.streams:
+            written = stream.write(data)
+        self.flush()
+        return written
+
+    def flush(self) -> None:
+        """Flushes all the streams in the logger."""
+        for stream in self.streams:
+            stream.flush()
+
+
+class InsertPrefix(TextIOWrapper):
+    """Inserts a prefix into the data written to the stream."""
+
+    def __init__(self, stream: TextIO, prefix: Callable[[], str] | str | None) -> None:
+        self.stream = stream
+        self.prefix = prefix
+        self._line_start = True
+
+    def _get_prefix(self) -> str:
+        if not self.prefix:
+            return ''
+
+        prefix = self.prefix() if callable(self.prefix) else self.prefix
+        return prefix
+
+    def write(self, data: str, /) -> int:
+        """
+        Writes the given data to the stream, applying a prefix to each line if necessary.
+
+        :param data: The data to be written to the stream.
+        """
+        prefix = self._get_prefix()
+        if not prefix:
+            return self.stream.write(data)
+
+        if self._line_start:
+            data = prefix + data
+
+        self._line_start = data.endswith('\n')
+        # Append our prefix just after all inner newlines. Don't append to a
+        # trailing newline as we don't know if the next line in the log will be
+        # from this zone.
+        data = data.replace('\n', '\n' + prefix)
+        if self._line_start:
+            data = data[:-len(prefix)]
+
+        return self.stream.write(data)
+
+    def flush(self) -> None:
+        """
+        Flushes the stream.
+
+        This method flushes the stream to ensure that all buffered data is written
+        to the underlying file or device.
+        """
+        self.stream.flush()
+        self.stream.flush()
+
+
+def prefix_and_tee_streams(name: Path, prefix: Callable[[], str] | str | None = None) -> None:
+    """
+    Tee stdout and stderr also to the named log file.
+
+    Note: we intentionally don't provide a way to clean up the stream
+    replacement so that any error handling from Python which causes us to exit
+    is also captured by the log file.
+    """
+    log_file = name.open(mode='w')
+
+    sys.stdout = InsertPrefix(
+        Tee(
+            sys.stdout,
+            log_file,
+        ),
+        prefix=prefix,
+    )
+    sys.stderr = InsertPrefix(
+        Tee(
+            sys.stderr,
+            log_file,
+        ),
+        prefix=prefix,
+    )
+
+
+def get_match_identifier() -> str:
+    """
+    Get the identifier for this run of the simulator.
+
+    This identifier is used to name the log files.
+
+    :return: The match identifier
+    """
+    match_data = get_match_data()
+
+    if match_data.match_number is not None:
+        return f"match-{match_data.match_number}"
+    else:
+        return DATE_IDENTIFIER

simulator/modules/robot_utils.py

@@ -0,0 +1,120 @@
+"""General utilities that are useful across runners."""
+from __future__ import annotations
+
+import json
+import platform
+import subprocess
+import sys
+from pathlib import Path
+from typing import NamedTuple
+
+# Configure path to import the environment configuration
+sys.path.insert(0, str(Path(__file__).parents[1]))
+import environment
+
+# Reset the path
+del sys.path[0]
+
+
+class MatchData(NamedTuple):
+    """
+    Data about the current match.
+
+    :param match_number: The current match number
+    :param match_duration: The duration of the match in seconds
+    :param video_enabled: Whether video recording is enabled
+    :param video_resolution: The resolution of the video recording
+    """
+
+    match_number: int | None = None
+    match_duration: int = environment.DEFAULT_MATCH_DURATION
+    video_enabled: bool = True
+    video_resolution: tuple[int, int] = (1920, 1080)
+
+
+def get_robot_file(robot_zone: int) -> Path:
+    """
+    Get the path to the robot file for the given zone.
+
+    :param robot_zone: The zone number
+    :return: The path to the robot file
+    :raises FileNotFoundError: If no robot controller is found for the given zone
+    """
+    robot_file = environment.ZONE_ROOT / f'zone_{robot_zone}' / 'robot.py'
+
+    # Check if the robot file exists
+    if not robot_file.exists():
+        raise FileNotFoundError(f"No robot code to run for zone {robot_zone}")
+
+    return robot_file
+
+
+def get_game_mode() -> str:
+    """
+    Get the game mode from the game mode file.
+
+    Default to 'dev' if the file does not exist.
+
+    :return: The game mode
+    """
+    if environment.GAME_MODE_FILE.exists():
+        game_mode = environment.GAME_MODE_FILE.read_text().strip()
+    else:
+        game_mode = 'dev'
+
+    assert game_mode in ['dev', 'comp'], f'Invalid game mode: {game_mode}'
+
+    return game_mode
+
+
+def print_simulation_version() -> None:
+    """
+    Print the version of the simulator that is running.
+
+    Uses a VERSION file in the root of the simulator to determine the version.
+    For development, the version is uses the git describe command.
+
+    The version is printed to the console.
+    """
+    version_file = environment.SIM_ROOT / 'VERSION'
+    if version_file.exists():
+        version = version_file.read_text().strip()
+    else:
+        try:
+            version = subprocess.check_output(
+                ['git', 'describe', '--tags', '--always'],
+                cwd=str(environment.SIM_ROOT.resolve()),
+            ).decode().strip()
+        except subprocess.CalledProcessError:
+            version = 'unknown'
+
+    print(
+        f"Running simulator version: {version} in Python {platform.python_version()} "
+        f"({platform.system()}-{platform.machine()})"
+    )
+
+
+def get_match_data() -> MatchData:
+    """Load the match data from the match data file."""
+    match_data_file = environment.ARENA_ROOT / 'match.json'
+    default_match_data = MatchData()
+
+    if match_data_file.exists():
+        # TODO error handling for invalid json
+        raw_data = json.loads(match_data_file.read_text())
+        match_data = MatchData(
+            match_number=raw_data.get('match_number', default_match_data.match_number),
+            match_duration=raw_data.get('duration', default_match_data.match_duration),
+            video_enabled=(
+                raw_data.get('recording_config', {})
+                .get('enabled', default_match_data.video_enabled)
+            ),
+            video_resolution=(
+                raw_data.get('recording_config', {})
+                .get('video_resolution', default_match_data.video_resolution)
+            ),
+        )
+    else:
+        match_data = default_match_data
+
+    return match_data

simulator/modules/sbot_interface/boards/__init__.py

@@ -0,0 +1,22 @@
+"""
+A set of board simulators for the SRO robot.
+
+When connected to the SocketServer, these can be used with the sr-robot3 project.
+"""
+from sbot_interface.boards.arduino import Arduino
+from sbot_interface.boards.camera import CameraBoard
+from sbot_interface.boards.led_board import LedBoard
+from sbot_interface.boards.motor_board import MotorBoard
+from sbot_interface.boards.power_board import PowerBoard
+from sbot_interface.boards.servo_board import ServoBoard
+from sbot_interface.boards.time_server import TimeServer
+
+__all__ = [
+    'Arduino',
+    'CameraBoard',
+    'LedBoard',
+    'MotorBoard',
+    'PowerBoard',
+    'ServoBoard',
+    'TimeServer',
+]

simulator/modules/sbot_interface/boards/arduino.py

@@ -0,0 +1,112 @@
+"""
+A simulator for the SR Arduino board.
+
+Provides a message parser that simulates the behavior of an Arduino board.
+
+Based on the Arduino v2.0 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.arduino_devices import BasePin, GPIOPinMode, UltrasonicSensor
+
+LOGGER = logging.getLogger(__name__)
+
+
+class Arduino:
+    """
+    A simulator for the SR Arduino board.
+
+    :param pins: A list of simulated devices connected to the Arduino board.
+                 The list is indexed by the pin number and EmptyPin is used for
+                 unconnected pins.
+    :param asset_tag: The asset tag to report for the Arduino board.
+    :param software_version: The software version to report for the Arduino board.
+    """
+
+    def __init__(self, pins: list[BasePin], asset_tag: str, software_version: str = '2'):
+        self.pins = pins
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified pins automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        if command[0] == 'a':  # analog read
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number == -1:
+                return '0'
+            return str(self.pins[pin_number].get_analog())
+        elif command[0] == 'r':  # digital read
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number == -1:
+                return 'l'
+            return 'h' if self.pins[pin_number].get_digital() else 'l'
+        elif command[0] == 'l':  # digital write low
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_digital(False)
+            return ''
+        elif command[0] == 'h':  # digital write high
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_digital(True)
+            return ''
+        elif command[0] == 'i':  # set pin mode to input
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.INPUT)
+            return ''
+        elif command[0] == 'o':  # set pin mode to output
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.OUTPUT)
+            return ''
+        elif command[0] == 'p':  # set pin mode to input pullup
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.INPUT_PULLUP)
+            return ''
+        elif command[0] == 'u':  # ultrasonic measurement
+            pulse_pin = self._convert_pin_number(command, 1)
+            echo_pin = self._convert_pin_number(command, 2)
+
+            if pulse_pin == -1 or echo_pin == -1:
+                return '0'
+
+            ultrasound_sensor = self.pins[echo_pin]
+            if isinstance(ultrasound_sensor, UltrasonicSensor):
+                return str(ultrasound_sensor.get_distance())
+            else:
+                return '0'
+        elif command[0] == 'v':  # software version
+            return f"SRduino:{self.software_version}"
+        else:
+            # A problem here: we do not know how to handle the command!
+            # Just ignore this for now.
+            return ''
+
+    def _convert_pin_number(self, command: str, index: int) -> int:
+        if len(command) < index + 1:
+            LOGGER.warning(f'Incomplete arduino command: {command}')
+            return -1
+
+        pin_str = command[index]
+        try:
+            pin_number = ord(pin_str) - ord('a')
+        except ValueError:
+            LOGGER.warning(f'Invalid pin number in command: {command}')
+            return -1
+
+        if 0 < pin_number < len(self.pins):
+            return pin_number
+        else:
+            LOGGER.warning(f'Invalid pin number in command: {command}')
+            return -1

simulator/modules/sbot_interface/boards/camera.py

@@ -0,0 +1,75 @@
+"""
+A simulator for the SRO Camera interface.
+
+Provides a message parser that simulates the behavior of a Camera.
+Interfaces to a WebotsRemoteCameraSource in the sr-robot3 package.
+"""
+from __future__ import annotations
+
+import logging
+import struct
+
+from sbot_interface.devices.camera import BaseCamera
+
+LOGGER = logging.getLogger(__name__)
+
+# *IDN?
+# *STATUS?
+# *RESET
+# CAM:CALIBRATION?
+# CAM:RESOLUTION?
+# CAM:FRAME!
+
+
+class CameraBoard:
+    """
+    A simulator for the SRO Camera interface.
+
+    :param camera: The camera object to interface with.
+    :param asset_tag: The asset tag to report for the camera board.
+    :param software_version: The software version to report for the camera board.
+    """
+
+    def __init__(self, camera: BaseCamera, asset_tag: str, software_version: str = '1.0'):
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.camera = camera
+
+    def handle_command(self, command: str) -> str | bytes:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on the camera automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'Student Robotics:CAMv1a:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return 'ACK'
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting camera board {self.asset_tag}')
+            return 'ACK'
+        elif args[0] == 'CAM':
+            if len(args) < 2:
+                return 'NACK:Missing camera command'
+
+            if args[1] == 'CALIBRATION?':
+                LOGGER.info(f'Getting calibration data from camera on board {self.asset_tag}')
+                return ':'.join(map(str, self.camera.get_calibration()))
+            elif args[1] == 'RESOLUTION?':
+                LOGGER.info(f'Getting resolution from camera on board {self.asset_tag}')
+                resolution = self.camera.get_resolution()
+                return f'{resolution[0]}:{resolution[1]}'
+            elif args[1] == 'FRAME!':
+                LOGGER.info(f'Getting image from camera on board {self.asset_tag}')
+                resolution = self.camera.get_resolution()
+                img_len = resolution[0] * resolution[1] * 4  # 4 bytes per pixel
+                return struct.pack('>BI', 0, img_len) + self.camera.get_image()
+            else:
+                return 'NACK:Unknown camera command'
+        else:
+            return f'NACK:Unknown command {command}'
+        return 'NACK:Command failed'

simulator/modules/sbot_interface/boards/led_board.py

@@ -0,0 +1,117 @@
+"""
+A simulator for the SRO LED hat.
+
+Provides a message parser that simulates the behavior of the LED hat.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.led import RGB_COLOURS, BaseLed
+
+LOGGER = logging.getLogger(__name__)
+
+# *IDN?
+# *STATUS?
+# *RESET
+# LED:<n>:SET:<r>:<g>:<b>
+# LED:<n>:GET?
+# LED:START:SET:<0/1>
+# LED:START:GET?
+
+LED_START = 4
+
+
+class LedBoard:
+    """
+    A simulator for the SRO LED hat.
+
+    :param leds: A list of simulated LEDs connected to the LED hat.
+                    The list is indexed by the LED number.
+    :param asset_tag: The asset tag to report for the LED hat.
+    :param software_version: The software version to report for the LED hat.
+    """
+
+    def __init__(self, leds: list[BaseLed], asset_tag: str, software_version: str = '1.0'):
+        self.leds = leds
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified LEDs automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'Student Robotics:KCHv1B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return 'ACK'
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting led board {self.asset_tag}')
+            for led in self.leds:
+                led.set_colour(0)
+            return 'ACK'
+        elif args[0] == 'LED':
+            if len(args) < 2:
+                return 'NACK:Missing LED number'
+
+            if args[1] == 'START':
+                led_number = LED_START
+                if len(args) < 3:
+                    return 'NACK:Missing LED command'
+                if args[2] == 'SET':
+                    if len(args) < 4:
+                        return 'NACK:Missing LED start'
+                    try:
+                        start = int(args[3])
+                    except ValueError:
+                        return 'NACK:Invalid LED start'
+                    if start not in [0, 1]:
+                        return 'NACK:Invalid LED start'
+                    LOGGER.info(f'Setting start LED on board {self.asset_tag} to {start}')
+                    self.leds[led_number].set_colour(start)
+                    return 'ACK'
+                elif args[2] == 'GET?':
+                    return str(self.leds[led_number].get_colour())
+                else:
+                    return "NACK:Unknown start command"
+            else:
+                try:
+                    led_number = int(args[1])
+                except ValueError:
+                    return 'NACK:Invalid LED number'
+                if not (0 <= led_number < len(self.leds)):
+                    return 'NACK:Invalid LED number'
+
+                if len(args) < 3:
+                    return 'NACK:Missing LED command'
+                if args[2] == 'SET':
+                    if len(args) < 6:
+                        return 'NACK:Missing LED colour'
+                    try:
+                        r = bool(int(args[3]))
+                        g = bool(int(args[4]))
+                        b = bool(int(args[5]))
+                    except ValueError:
+                        return 'NACK:Invalid LED colour'
+                    if r not in [0, 1] or g not in [0, 1] or b not in [0, 1]:
+                        return 'NACK:Invalid LED colour'
+                    LOGGER.info(
+                        f'Setting LED {led_number} on board {self.asset_tag} to '
+                        f'{r:d}:{g:d}:{b:d} (colour {RGB_COLOURS.index((r, g, b))}',
+                    )
+                    self.leds[led_number].set_colour(RGB_COLOURS.index((r, g, b)))
+                    return 'ACK'
+                elif args[2] == 'GET?':
+                    colour = RGB_COLOURS[self.leds[led_number].get_colour()]
+                    return f"{colour[0]:d}:{colour[1]:d}:{colour[2]:d}"
+                else:
+                    return 'NACK:Unknown LED command'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'

simulator/modules/sbot_interface/boards/motor_board.py

@@ -0,0 +1,107 @@
+"""
+A simulator for the SRv4 Motor Board.
+
+Provides a message parser that simulates the behavior of a motor board.
+
+Based on the Motor Board v4.4.1 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.motor import MAX_POWER, MIN_POWER, BaseMotor
+
+LOGGER = logging.getLogger(__name__)
+
+
+class MotorBoard:
+    """
+    A simulator for the SRv4 Motor Board.
+
+    :param motors: A list of simulated motors connected to the motor board.
+                     The list is indexed by the motor number.
+    :param asset_tag: The asset tag to report for the motor board.
+    :param software_version: The software version to report for the motor board.
+    """
+
+    def __init__(
+        self,
+        motors: list[BaseMotor],
+        asset_tag: str,
+        software_version: str = '4.4.1'
+    ):
+        self.motors = motors
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified motors automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'Student Robotics:MCv4B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            # Output faults are unsupported
+            return "0,0:12000"
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting motor board {self.asset_tag}')
+            for motor in self.motors:
+                motor.disable()
+            return 'ACK'
+        elif args[0] == 'MOT':
+            if len(args) < 2:
+                return 'NACK:Missing motor number'
+
+            try:
+                motor_number = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid motor number'
+            if not (0 <= motor_number < len(self.motors)):
+                return 'NACK:Invalid motor number'
+
+            if len(args) < 3:
+                return 'NACK:Missing motor command'
+            if args[2] == 'SET':
+                if len(args) < 4:
+                    return 'NACK:Missing motor power'
+                try:
+                    power = int(args[3])
+                except ValueError:
+                    return 'NACK:Invalid motor power'
+                if not (MIN_POWER <= power <= MAX_POWER):
+                    return 'NACK:Invalid motor power'
+                LOGGER.info(
+                    f'Setting motor {motor_number} on board {self.asset_tag} to {power}'
+                )
+                self.motors[motor_number].set_power(power)
+                return 'ACK'
+            elif args[2] == 'GET?':
+                return ':'.join([
+                    f'{int(self.motors[motor_number].enabled())}',
+                    f'{self.motors[motor_number].get_power()}',
+                ])
+            elif args[2] == 'DISABLE':
+                LOGGER.info(f'Disabling motor {motor_number} on board {self.asset_tag}')
+                self.motors[motor_number].disable()
+                return 'ACK'
+            elif args[2] == 'I?':
+                return str(self.current())
+            else:
+                return 'NACK:Unknown motor command'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'
+
+    def current(self) -> int:
+        """
+        Get the total current draw of all motors.
+
+        :return: The total current draw of all motors in mA.
+        """
+        return sum(motor.get_current() for motor in self.motors)

simulator/modules/sbot_interface/boards/power_board.py

@@ -0,0 +1,192 @@
+"""
+A simulator for the SRv4 Power Board.
+
+Provides a message parser that simulates the behavior of a power board.
+
+Based on the Power Board v4.4.2 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.led import BaseLed
+from sbot_interface.devices.power import BaseButton, BaseBuzzer, Output
+
+LOGGER = logging.getLogger(__name__)
+
+NUM_OUTPUTS = 7  # 6 12V outputs, 1 5V output
+SYS_OUTPUT = 4  # L2 output for the brain
+
+RUN_LED = 0
+ERR_LED = 1
+
+
+class PowerBoard:
+    """
+    A simulator for the SRv4 Power Board.
+
+    :param outputs: A list of simulated outputs connected to the power board.
+                        The list is indexed by the output number.
+    :param buzzer: A simulated buzzer connected to the power board.
+    :param button: A simulated button connected to the power board.
+    :param leds: A tuple of simulated LEDs connected to the power board.
+    :param asset_tag: The asset tag to report for the power board.
+    :param software_version: The software version to report for the power board.
+    """
+
+    def __init__(
+        self,
+        outputs: list[Output],
+        buzzer: BaseBuzzer,
+        button: BaseButton,
+        leds: tuple[BaseLed, BaseLed],
+        asset_tag: str,
+        software_version: str = '4.4.2',
+    ):
+        self.outputs = outputs
+        self.buzzer = buzzer
+        self.button = button
+        self.leds = leds
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.temp = 25
+        self.battery_voltage = 12000
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified outputs, LEDs,
+        or the buzzer automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'Student Robotics:PBv4B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            # Output faults are unsupported, fan is always off
+            return f"0,0,0,0,0,0,0:{self.temp}:0:5000"
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting power board {self.asset_tag}')
+            for output in self.outputs:
+                output.set_output(False)
+            self.buzzer.set_note(0, 0)
+            self.leds[RUN_LED].set_colour(0)
+            self.leds[ERR_LED].set_colour(0)
+            return 'ACK'
+        elif args[0] == 'BTN':
+            if len(args) < 2:
+                return 'NACK:Missing button command'
+            if args[1] == 'START' and args[2] == 'GET?':
+                return f'{self.button.get_state():d}:0'
+            else:
+                return 'NACK:Unknown button command'
+        elif args[0] == 'OUT':
+            if len(args) < 2:
+                return 'NACK:Missing output number'
+            try:
+                output_number = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid output number'
+            if not (0 <= output_number < NUM_OUTPUTS):
+                return 'NACK:Invalid output number'
+
+            if len(args) < 3:
+                return 'NACK:Missing output command'
+            if args[2] == 'SET':
+                if output_number == SYS_OUTPUT:
+                    return 'NACK:Brain output cannot be controlled'
+                if len(args) < 4:
+                    return 'NACK:Missing output state'
+                try:
+                    state = int(args[3])
+                except ValueError:
+                    return 'NACK:Invalid output state'
+                if state not in [0, 1]:
+                    return 'NACK:Invalid output state'
+                LOGGER.info(
+                    f'Setting output {output_number} on board {self.asset_tag} to {state}'
+                )
+                self.outputs[output_number].set_output(bool(state))
+                return 'ACK'
+            elif args[2] == 'GET?':
+                return '1' if self.outputs[output_number].get_output() else '0'
+            elif args[2] == 'I?':
+                return str(self.outputs[output_number].get_current())
+            else:
+                return 'NACK:Unknown output command'
+        elif args[0] == 'BATT':
+            if len(args) < 2:
+                return 'NACK:Missing battery command'
+            if args[1] == 'V?':
+                return str(self.battery_voltage)
+            elif args[1] == 'I?':
+                return str(self.current())
+            else:
+                return 'NACK:Unknown battery command'
+        elif args[0] == 'LED':
+            if len(args) < 3:
+                return 'NACK:Missing LED command'
+            if args[1] not in ['RUN', 'ERR']:
+                return 'NACK:Invalid LED type'
+
+            led_type = RUN_LED if args[1] == 'RUN' else ERR_LED
+
+            if args[2] == 'SET':
+                if len(args) < 4:
+                    return 'NACK:Missing LED state'
+                if args[3] in ['0', '1', 'F']:
+                    LOGGER.info(
+                        f'Setting {args[1]} LED on board {self.asset_tag} to {args[3]}'
+                    )
+                    if args[3] == 'F':
+                        self.leds[led_type].set_colour(1)
+                    else:
+                        self.leds[led_type].set_colour(int(args[3]))
+                    return 'ACK'
+                else:
+                    return 'NACK:Invalid LED state'
+            elif args[2] == 'GET?':
+                return str(self.leds[led_type].get_colour())
+            else:
+                return 'NACK:Invalid LED command'
+        elif args[0] == 'NOTE':
+            if len(args) < 2:
+                return 'NACK:Missing note command'
+            if args[1] == 'GET?':
+                return ':'.join(map(str, self.buzzer.get_note()))
+            else:
+                if len(args) < 3:
+                    return 'NACK:Missing note frequency'
+                try:
+                    freq = int(args[1])
+                except ValueError:
+                    return 'NACK:Invalid note frequency'
+                if not (0 <= freq < 10000):
+                    return 'NACK:Invalid note frequency'
+
+                try:
+                    dur = int(args[2])
+                except ValueError:
+                    return 'NACK:Invalid note duration'
+                if dur < 0:
+                    return 'NACK:Invalid note duration'
+
+                LOGGER.info(
+                    f'Setting buzzer on board {self.asset_tag} to {freq}Hz for {dur}ms'
+                )
+                self.buzzer.set_note(freq, dur)
+                return 'ACK'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'
+
+    def current(self) -> int:
+        """
+        Get the total current draw of all outputs.
+
+        :return: The total current draw of all outputs in mA.
+        """
+        return sum(output.get_current() for output in self.outputs)

simulator/modules/sbot_interface/boards/servo_board.py

@@ -0,0 +1,105 @@
+"""
+A simulator for the SRv4 Servo Board.
+
+Provides a message parser that simulates the behavior of a servo board.
+
+Based on the Servo Board v4.4 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.servo import MAX_POSITION, MIN_POSITION, BaseServo
+
+LOGGER = logging.getLogger(__name__)
+
+
+class ServoBoard:
+    """
+    A simulator for the SRv4 Servo Board.
+
+    :param servos: A list of simulated servos connected to the servo board.
+                        The list is indexed by the servo number.
+    :param asset_tag: The asset tag to report for the servo board.
+    :param software_version: The software version to report for the servo board.
+    """
+
+    def __init__(self, servos: list[BaseServo], asset_tag: str, software_version: str = '4.4'):
+        self.servos = servos
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.watchdog_fail = False
+        self.pgood = True
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified servos automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'Student Robotics:SBv4B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return f"{self.watchdog_fail:d}:{self.pgood:d}"
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting servo board {self.asset_tag}')
+            for servo in self.servos:
+                servo.disable()
+            return 'ACK'
+        elif args[0] == 'SERVO':
+            if len(args) < 2:
+                return 'NACK:Missing servo number'
+            if args[1] == 'I?':
+                return str(self.current())
+            elif args[1] == 'V?':
+                return '5000'
+
+            try:
+                servo_number = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid servo number'
+            if not (0 <= servo_number < len(self.servos)):
+                return 'NACK:Invalid servo number'
+
+            if len(args) < 3:
+                return 'NACK:Missing servo command'
+
+            if args[2] == 'DISABLE':
+                LOGGER.info(f'Disabling servo {servo_number} on board {self.asset_tag}')
+                self.servos[servo_number].disable()
+                return 'ACK'
+            elif args[2] == 'GET?':
+                return str(self.servos[servo_number].get_position())
+            elif args[2] == 'SET':
+                if len(args) < 4:
+                    return 'NACK:Missing servo setpoint'
+
+                try:
+                    setpoint = int(args[3])
+                except ValueError:
+                    return 'NACK:Invalid servo setpoint'
+                if not (MIN_POSITION <= setpoint <= MAX_POSITION):
+                    return 'NACK:Invalid servo setpoint'
+
+                LOGGER.info(
+                    f'Setting servo {servo_number} on board {self.asset_tag} to {setpoint}'
+                )
+                self.servos[servo_number].set_position(setpoint)
+                return 'ACK'
+            else:
+                return 'NACK:Unknown servo command'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'
+
+    def current(self) -> int:
+        """
+        Get the total current draw of all servos.
+
+        :return: The total current draw of all servos in mA.
+        """
+        return sum(servo.get_current() for servo in self.servos)

simulator/modules/sbot_interface/boards/time_server.py

@@ -0,0 +1,66 @@
+"""
+A simulator for handling time based commands using simulated time.
+
+Provides a message parser that simulates the behavior of sleep and time.
+"""
+from __future__ import annotations
+
+import logging
+from datetime import datetime, timedelta
+
+from sbot_interface.devices.util import get_globals
+
+LOGGER = logging.getLogger(__name__)
+g = get_globals()
+
+
+class TimeServer:
+    """
+    A simulator for handling time based commands using simulated time.
+
+    :param asset_tag: The asset tag to report for the time server.
+    :param software_version: The software version to report for the time server.
+    :param start_time: The start time for the time server (reported time to simulator time 0).
+    """
+
+    def __init__(
+        self,
+        asset_tag: str,
+        software_version: str = '1.0',
+        start_time: str = '2024-06-01T00:00:00+00:00',
+    ):
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.start_time = datetime.fromisoformat(start_time)
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'SourceBots:TimeServer:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return "Yes"
+        elif args[0] == '*RESET':
+            return "NACK:Reset not supported"
+        elif args[0] == 'TIME?':
+            sim_time = g.robot.getTime()
+            current_time = self.start_time + timedelta(seconds=sim_time)
+            return current_time.isoformat('T', timespec='milliseconds')
+        elif args[0] == 'SLEEP':
+            if len(args) < 2:
+                return 'NACK:Missing duration'
+            try:
+                duration = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid duration'
+            LOGGER.info(f'Sleeping for {duration} ms')
+            g.sleep(duration / 1000)
+            return 'ACK'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'

simulator/modules/sbot_interface/devices/__init__.py

@@ -0,0 +1 @@
+"""A set of wrappers for board simulators to interact with Webots devices."""

simulator/modules/sbot_interface/devices/arduino_devices.py

@@ -0,0 +1,275 @@
+"""A collection of wrappers for the devices that can be connected to the Arduino."""
+from abc import ABC, abstractmethod
+from enum import Enum
+
+from sbot_interface.devices.led import Led as _Led
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+ANALOG_MAX = 1023
+
+
+class GPIOPinMode(str, Enum):
+    """The possible modes for a GPIO pin."""
+
+    INPUT = 'INPUT'
+    INPUT_PULLUP = 'INPUT_PULLUP'
+    OUTPUT = 'OUTPUT'
+
+
+class BasePin(ABC):
+    """The base class for all the devices that can be connected to the Arduino."""
+
+    @abstractmethod
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        pass
+
+    @abstractmethod
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        pass
+
+    @abstractmethod
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        pass
+
+    @abstractmethod
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin."""
+        pass
+
+    @abstractmethod
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin."""
+        pass
+
+
+class EmptyPin(BasePin):
+    """A pin that does nothing. Used for pins that are not connected."""
+
+    def __init__(self) -> None:
+        self._mode = GPIOPinMode.INPUT
+        self._digital = False
+        self._analog = 0
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        return self._digital
+
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin."""
+        self._digital = value
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin."""
+        return self._analog
+
+
+class UltrasonicSensor(BasePin):
+    """
+    A sensor that can measure the distance to an object.
+
+    This is attached to the pin specified to be the echo pin, with the trigger pin unused.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.DistanceSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin. This is always False."""
+        return False
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is always 0."""
+        return 0
+
+    def get_distance(self) -> int:
+        """
+        Get the distance measured by the sensor in mm.
+
+        Relies on the lookup table mapping to the distance in mm.
+        """
+        return int(self._device.getValue())
+
+
+class MicroSwitch(BasePin):
+    """A simple switch that can be pressed or released."""
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.TouchSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        return bool(self._device.getValue())
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin, either 0 or 1023."""
+        return 1023 if self.get_digital() else 0
+
+
+class PressureSensor(BasePin):
+    """
+    A sensor that can measure the force applied to it.
+
+    This is attached to the pin specified, with the force proportional to the analog value.
+    """
+
+    # Use lookupTable [0 0 0, 50 1023 0] // 50 Newton max force
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.TouchSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the force is above half the maximum value.
+        """
+        return self.get_analog() > ANALOG_MAX / 2
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is proportional to the force applied."""
+        return int(self._device.getValue())
+
+
+class ReflectanceSensor(BasePin):
+    """
+    A simple sensor that can detect the reflectance of a surface.
+
+    Used for line following, with a higher value indicating a lighter surface.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.DistanceSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the reflectance is above half the maximum value.
+        """
+        return self.get_analog() > ANALOG_MAX / 2
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """
+        Get the analog input value of the pin.
+
+        This is proportional to the reflectance of the surface.
+        """
+        return int(self._device.getValue())
+
+
+class Led(BasePin):
+    """A simple LED that can be turned on or off."""
+
+    def __init__(self, device_name: str) -> None:
+        self._led = _Led(device_name)
+        self._mode = GPIOPinMode.OUTPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the LED is on.
+        """
+        return self._led.get_colour() > 0
+
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin. This turns the LED on or off."""
+        self._led.set_colour(1 if value else 0)
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is always 0."""
+        return 0

simulator/modules/sbot_interface/devices/camera.py

@@ -0,0 +1,105 @@
+"""A wrapper for the Webots camera device."""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from functools import lru_cache
+from math import tan
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+g = get_globals()
+
+
+class BaseCamera(ABC):
+    """Base class for camera devices."""
+
+    @abstractmethod
+    def get_image(self) -> bytes:
+        """Get a frame from the camera, encoded as a byte string."""
+        pass
+
+    @abstractmethod
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        pass
+
+    @abstractmethod
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        pass
+
+
+class NullCamera(BaseCamera):
+    """
+    Null camera device.
+
+    Allows the robot to run without a camera device attached.
+    """
+
+    def get_image(self) -> bytes:
+        """Get a frame from the camera, encoded as a byte string."""
+        return b''
+
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        return 0, 0
+
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        return 0, 0, 0, 0
+
+
+# Camera
+class Camera(BaseCamera):
+    """
+    A wrapper for the Webots camera device.
+
+    The camera will sleep for 1 frame time before capturing an image to ensure the
+    image is up to date.
+
+    :param device_name: The name of the camera device.
+    :param frame_rate: The frame rate of the camera in frames per second.
+    """
+
+    def __init__(self, device_name: str, frame_rate: int) -> None:
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Camera)
+        # round down to the nearest timestep
+        self.sample_time = int(((1000 / frame_rate) // g.timestep) * g.timestep)
+
+    def get_image(self) -> bytes:
+        """
+        Get a frame from the camera, encoded as a byte string.
+
+        Sleeps for 1 frame time before capturing the image to ensure the image is up to date.
+
+        NOTE The image data buffer is automatically freed at the end of the timestep,
+        so must not be accessed after any sleep.
+
+        :return: The image data as a byte string.
+        """
+        # A frame is only captured every sample_time milliseconds the camera is enabled
+        # so we need to wait for a frame to be captured after enabling the camera.
+        # The image data buffer is automatically freed at the end of the timestep.
+        self._device.enable(self.sample_time)
+        g.sleep(self.sample_time / 1000)
+
+        image_data_raw = self._device.getImage()
+        # Disable the camera to save computation
+        self._device.disable()  # type: ignore[no-untyped-call]
+
+        return image_data_raw
+
+    @lru_cache
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        return self._device.getWidth(), self._device.getHeight()
+
+    @lru_cache
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        return (
+            (self._device.getWidth() / 2) / tan(self._device.getFov() / 2),  # fx
+            (self._device.getWidth() / 2) / tan(self._device.getFov() / 2),  # fy
+            self._device.getWidth() // 2,  # cx
+            self._device.getHeight() // 2,  # cy
+        )

simulator/modules/sbot_interface/devices/led.py

@@ -0,0 +1,88 @@
+"""
+A wrapper for the Webots LED device.
+
+Supports both single and multi-colour non-PWM LEDs.
+"""
+from abc import ABC, abstractmethod
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+RGB_COLOURS = [
+    (False, False, False),  # OFF
+    (True, False, False),  # RED
+    (True, True, False),  # YELLOW
+    (False, True, False),  # GREEN
+    (False, True, True),  # CYAN
+    (False, False, True),  # BLUE
+    (True, False, True),  # MAGENTA
+    (True, True, True),  # WHITE
+]
+
+
+class BaseLed(ABC):
+    """Base class for LED devices."""
+
+    def __init__(self) -> None:
+        self.colour = 0
+
+    @abstractmethod
+    def set_colour(self, colour: int) -> None:
+        """Set the colour of the LED."""
+        pass
+
+    @abstractmethod
+    def get_colour(self) -> int:
+        """Get the colour of the LED."""
+        pass
+
+
+class NullLed(BaseLed):
+    """Null LED device. Allows the robot to run without an LED device attached."""
+
+    def __init__(self) -> None:
+        self.colour = 0
+
+    def set_colour(self, colour: int) -> None:
+        """Set the colour of the LED."""
+        self.colour = colour
+
+    def get_colour(self) -> int:
+        """Get the colour of the LED."""
+        return self.colour
+
+
+class Led(BaseLed):
+    """
+    A wrapper for the Webots LED device.
+
+    :param device_name: The name of the LED device.
+    :param num_colours: The number of colours the LED supports.
+    """
+
+    def __init__(self, device_name: str, num_colours: int = 1) -> None:
+        g = get_globals()
+        self.num_colours = num_colours
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.LED)
+
+    def set_colour(self, colour: int) -> None:
+        """
+        Set the colour of the LED.
+
+        :param colour: The colour to set the LED to. A 1-based index for the lookup
+                       table of the LED. 0 is OFF.
+        """
+        if 0 <= colour < self.num_colours:
+            # NOTE: value 0 is OFF
+            self._device.set(colour)
+        else:
+            raise ValueError(f'Invalid colour: {colour}')
+
+    def get_colour(self) -> int:
+        """
+        Get the colour of the LED.
+
+        :return: The colour of the LED. A 1-based index for the lookup table of the LED.
+                 0 is OFF.
+        """
+        # webots uses 1-based indexing
+        return self._device.get()

simulator/modules/sbot_interface/devices/motor.py

@@ -0,0 +1,155 @@
+"""
+A wrapper for the Webots motor device.
+
+The motor will apply a small amount of variation to the power setting to simulate
+inaccuracies in the motor.
+"""
+import logging
+from abc import ABC, abstractmethod
+
+from sbot_interface.devices.util import (
+    WebotsDevice,
+    add_jitter,
+    get_globals,
+    get_robot_device,
+    map_to_range,
+)
+
+MAX_POWER = 1000
+MIN_POWER = -1000
+
+
+class BaseMotor(ABC):
+    """Base class for motor devices."""
+
+    @abstractmethod
+    def disable(self) -> None:
+        """Disable the motor."""
+        pass
+
+    @abstractmethod
+    def set_power(self, value: int) -> None:
+        """Set the power of the motor (±1000)."""
+        pass
+
+    @abstractmethod
+    def get_power(self) -> int:
+        """Get the power of the motor."""
+        pass
+
+    @abstractmethod
+    def get_current(self) -> int:
+        """Get the current draw of the motor in mA."""
+        pass
+
+    @abstractmethod
+    def enabled(self) -> bool:
+        """Check if the motor is enabled."""
+        pass
+
+
+class NullMotor(BaseMotor):
+    """Null motor device. Allows the robot to run without a motor device attached."""
+
+    def __init__(self) -> None:
+        self.power = 0
+        self._enabled = False
+
+    def disable(self) -> None:
+        """Disable the motor."""
+        self._enabled = False
+
+    def set_power(self, value: int) -> None:
+        """Set the power of the motor."""
+        self.power = value
+        self._enabled = True
+
+    def get_power(self) -> int:
+        """Get the power of the motor."""
+        return self.power
+
+    def get_current(self) -> int:
+        """Get the current draw of the motor in mA."""
+        return 0
+
+    def enabled(self) -> bool:
+        """Check if the motor is enabled."""
+        return self._enabled
+
+
+class Motor(BaseMotor):
+    """
+    A wrapper for the Webots motor device.
+
+    The motor will apply a small amount of variation to the power setting to simulate
+    inaccuracies in the motor.
+
+    :param device_name: The name of the motor device.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        self.power = 0
+        self._enabled = False
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Motor)
+        # Put the motor in velocity control mode
+        self._device.setPosition(float('inf'))
+        self._device.setVelocity(0)
+        self._max_speed = self._device.getMaxVelocity()
+        # Limit the torque the motor can apply to have realistic acceleration
+        self._device.setAvailableTorque(1)
+
+    def disable(self) -> None:
+        """Disable the motor."""
+        self._device.setVelocity(0)
+        self._enabled = False
+
+    def set_power(self, value: int) -> None:
+        """
+        Set the power of the motor.
+
+        :param value: The power setting for the motor. A value between -1000 and 1000.
+        """
+        if value != 0:
+            if abs(value) < 0.05:
+                logging.warning(
+                    "Motor power is too low, values below 0.05 will not move the motor."
+                )
+                value = 0
+            else:
+                # Apply a small amount of variation to the power setting to simulate
+                # inaccuracies in the motor
+                value = int(add_jitter(value, (MIN_POWER, MAX_POWER)))
+
+        self._device.setVelocity(map_to_range(
+            value,
+            (MIN_POWER, MAX_POWER),
+            (-self._max_speed, self._max_speed),
+        ))
+        self.power = value
+        self._enabled = True
+
+    def get_power(self) -> int:
+        """
+        Get the power of the motor.
+
+        :return: The power setting for the motor. A value between -1000 and 1000.
+        """
+        return self.power
+
+    def get_current(self) -> int:
+        """
+        Get the current draw of the motor in mA.
+
+        :return: The current draw of the motor in mA.
+        """
+        # TODO calculate from torque feedback
+        return 0
+
+    def enabled(self) -> bool:
+        """
+        Check if the motor is enabled.
+
+        :return: True if the motor is enabled, False otherwise.
+        """
+        return self._enabled

simulator/modules/sbot_interface/devices/power.py

@@ -0,0 +1,137 @@
+"""A module to define the power devices used in the simulator."""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from typing import Callable
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+
+class Output:
+    """
+    A class to represent a power output.
+
+    This does not actually represent any device in the simulator,
+    but is used to simulate how the outputs on the power board would behave.
+
+    :param downstream_current: A function to get the current draw of the downstream device.
+    """
+
+    def __init__(self, downstream_current: Callable[[], int] | None = None) -> None:
+        self._enabled = False
+        self._current_func = downstream_current
+
+    def set_output(self, enable: bool) -> None:
+        """Set the output state."""
+        self._enabled = enable
+
+    def get_output(self) -> bool:
+        """Get the output state."""
+        return self._enabled
+
+    def get_current(self) -> int:
+        """Get the current draw of the output in mA."""
+        if self._current_func is not None:
+            return self._current_func()
+        return 0
+
+
+class ConnectorOutput(Output):
+    """
+    A class to represent a power output that controls a webots connector device.
+
+    :param device_name: The name of the device in webots.
+    :param downstream_current: A function to get the current draw of the downstream device.
+    """
+
+    def __init__(
+        self,
+        device_name: str,
+        downstream_current: Callable[[], int] | None = None,
+    ) -> None:
+        super().__init__(downstream_current)
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Connector)
+        self._enabled = False
+
+    def set_output(self, enable: bool) -> None:
+        """Set the output state."""
+        if enable:
+            self._device.lock()  # type: ignore[no-untyped-call]
+        else:
+            self._device.unlock()  # type: ignore[no-untyped-call]
+
+    def get_output(self) -> bool:
+        """Get the output state."""
+        return self._device.isLocked()
+
+
+class BaseBuzzer(ABC):
+    """The base class for the buzzer device."""
+
+    @abstractmethod
+    def set_note(self, freq: int, dur: int) -> None:
+        """Set the note to play and its duration."""
+        pass
+
+    @abstractmethod
+    def get_note(self) -> tuple[int, int]:
+        """Get the note that is currently playing and its duration."""
+        pass
+
+
+class BaseButton(ABC):
+    """The base class for the button device."""
+
+    @abstractmethod
+    def get_state(self) -> bool:
+        """Get whether the button is pressed."""
+        pass
+
+
+class NullBuzzer(BaseBuzzer):
+    """A buzzer that does nothing. Used for buzzers that are not connected."""
+
+    def __init__(self) -> None:
+        self.frequency = 0
+        self.duration = 0
+        super().__init__()
+
+    def set_note(self, freq: int, dur: int) -> None:
+        """Set the note to play."""
+        self.frequency = freq
+        self.duration = dur
+
+    def get_note(self) -> tuple[int, int]:
+        """Get the note that is currently playing and its duration."""
+        return self.frequency, self.duration
+
+
+class NullButton(BaseButton):
+    """A button that does nothing. Used for buttons that are not connected."""
+
+    def get_state(self) -> bool:
+        """Return whether the button is pressed. Always returns True."""
+        # button is always pressed
+        return True
+
+
+class StartButton(BaseButton):
+    """
+    A button to represent the start button on the robot.
+
+    Uses the robot's custom data to determine if the robot is ready to start.
+    """
+
+    def __init__(self) -> None:
+        self._initialized = False
+
+    def get_state(self) -> bool:
+        """Return whether the start button is pressed."""
+        g = get_globals()
+        if not self._initialized:
+            if g.robot.getCustomData() != 'start':
+                g.robot.setCustomData('ready')
+            self._initialized = True
+
+        return bool(g.robot.getCustomData() == 'start')

simulator/modules/sbot_interface/devices/servo.py

@@ -0,0 +1,157 @@
+"""
+A wrapper for the Webots servo device.
+
+The servo will apply a small amount of variation to the power setting to simulate
+inaccuracies in the servo.
+"""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from typing import TYPE_CHECKING
+
+from sbot_interface.devices.util import (
+    WebotsDevice,
+    add_jitter,
+    get_globals,
+    get_robot_device,
+    map_to_range,
+)
+
+if TYPE_CHECKING:
+    from controller import PositionSensor
+
+MAX_POSITION = 4000
+MIN_POSITION = 500
+SERVO_MAX = 2000
+SERVO_MIN = 1000
+
+
+class BaseServo(ABC):
+    """The base class for all the servos that can be connected to the Servo board."""
+
+    @abstractmethod
+    def disable(self) -> None:
+        """Disable the servo."""
+        pass
+
+    @abstractmethod
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        pass
+
+    @abstractmethod
+    def get_position(self) -> int:
+        """Return the current position of the servo."""
+        pass
+
+    @abstractmethod
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        pass
+
+    @abstractmethod
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        pass
+
+
+class NullServo(BaseServo):
+    """A servo that does nothing. Used for servos that are not connected."""
+
+    def __init__(self) -> None:
+        self.position = 1500
+        self._enabled = False
+
+    def disable(self) -> None:
+        """Disable the servo."""
+        self._enabled = False
+
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        self.position = value
+        self._enabled = True
+
+    def get_position(self) -> int:
+        """
+        Return the current position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        return self.position
+
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        return 0
+
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        return self._enabled
+
+
+class Servo(BaseServo):
+    """A servo connected to the Servo board."""
+
+    def __init__(self, device_name: str) -> None:
+        self.position = (SERVO_MAX + SERVO_MIN) // 2
+        # TODO use setAvailableForce to simulate disabled
+        self._enabled = False
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Motor)
+        self._pos_sensor: PositionSensor | None = self._device.getPositionSensor()  # type: ignore[no-untyped-call]
+        self._max_position = self._device.getMaxPosition()
+        self._min_position = self._device.getMinPosition()
+        if self._pos_sensor is not None:
+            self._pos_sensor.enable(g.timestep)
+
+    def disable(self) -> None:
+        """Disable the servo."""
+        self._enabled = False
+
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        # Apply a small amount of variation to the power setting to simulate
+        # inaccuracies in the servo
+        value = int(add_jitter(value, (SERVO_MIN, SERVO_MAX), std_dev_percent=0.5))
+
+        self._device.setPosition(map_to_range(
+            value,
+            (SERVO_MIN, SERVO_MAX),
+            (self._min_position + 0.001, self._max_position - 0.001),
+        ))
+        self.position = value
+        self._enabled = True
+
+    def get_position(self) -> int:
+        """
+        Return the current position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        if self._pos_sensor is not None:
+            self.position = int(map_to_range(
+                self._pos_sensor.getValue(),
+                (self._min_position + 0.001, self._max_position - 0.001),
+                (MIN_POSITION, MAX_POSITION),
+            ))
+        return self.position
+
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        # TODO calculate from torque feedback
+        return 0
+
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        return self._enabled

simulator/modules/sbot_interface/devices/util.py

@@ -0,0 +1,159 @@
+"""Utility functions for the devices module."""
+from __future__ import annotations
+
+import threading
+from dataclasses import dataclass
+from math import ceil
+from random import gauss
+from typing import TypeVar
+
+from controller import (
+    GPS,
+    LED,
+    Accelerometer,
+    Camera,
+    Compass,
+    Connector,
+    DistanceSensor,
+    Emitter,
+    Gyro,
+    InertialUnit,
+    Lidar,
+    LightSensor,
+    Motor,
+    PositionSensor,
+    Radar,
+    RangeFinder,
+    Receiver,
+    Robot,
+    Speaker,
+    TouchSensor,
+    VacuumGripper,
+)
+from controller.device import Device
+
+TDevice = TypeVar('TDevice', bound=Device)
+__GLOBALS: 'GlobalData' | None = None
+
+
+class WebotsDevice:
+    """
+    A collection of Webots device classes.
+
+    Each class represents a different device that can be attached to the robot.
+    """
+
+    Accelerometer = Accelerometer
+    Camera = Camera
+    Compass = Compass
+    Connector = Connector
+    DistanceSensor = DistanceSensor
+    Emitter = Emitter
+    GPS = GPS
+    Gyro = Gyro
+    InertialUnit = InertialUnit
+    LED = LED
+    Lidar = Lidar
+    LightSensor = LightSensor
+    Motor = Motor
+    PositionSensor = PositionSensor
+    Radar = Radar
+    RangeFinder = RangeFinder
+    Receiver = Receiver
+    Speaker = Speaker
+    TouchSensor = TouchSensor
+    VacuumGripper = VacuumGripper
+
+
+@dataclass
+class GlobalData:
+    """
+    Global data and functions for the simulator.
+
+    When accessed through the get_globals function, a single instance of this
+    class is created and stored in the module's global scope.
+
+    :param robot: The robot object.
+    :param timestep: The timestep size of the simulation.
+    :param stop_event: The event to stop the simulation.
+    """
+
+    robot: Robot
+    timestep: int
+    stop_event: threading.Event | None = None
+
+    def sleep(self, secs: float) -> None:
+        """Sleeps for a given duration in simulator time."""
+        if secs == 0:
+            return
+        elif secs < 0:
+            raise ValueError("Sleep duration must be non-negative.")
+
+        # Convert to a multiple of the timestep
+        msecs = ceil((secs * 1000) / self.timestep) * self.timestep
+
+        # Sleep for the given duration
+        result = self.robot.step(msecs)
+
+        # If the simulation has stopped, set the stop event
+        if (result == -1) and (self.stop_event is not None):
+            self.stop_event.set()
+
+
+def get_globals() -> GlobalData:
+    """Returns the global dictionary."""
+    global __GLOBALS
+    if __GLOBALS is None:
+        # Robot constructor lacks a return type annotation in R2023b
+        robot = Robot() if Robot.created is None else Robot.created  # type: ignore[no-untyped-call]
+
+        __GLOBALS = GlobalData(
+            robot=robot,
+            timestep=int(robot.getBasicTimeStep()),
+        )
+    return __GLOBALS
+
+
+def map_to_range(
+    value: float,
+    old_min_max: tuple[float, float],
+    new_min_max: tuple[float, float],
+) -> float:
+    """Maps a value from within one range of inputs to within a range of outputs."""
+    old_min, old_max = old_min_max
+    new_min, new_max = new_min_max
+    return ((value - old_min) / (old_max - old_min)) * (new_max - new_min) + new_min
+
+
+def get_robot_device(robot: Robot, name: str, kind: type[TDevice]) -> TDevice:
+    """
+    A helper function to get a device from the robot.
+
+    Raises a TypeError if the device is not found or is not of the correct type.
+    Weboots normally just returns None if the device is not found.
+
+    :param robot: The robot object.
+    :param name: The name of the device.
+    :param kind: The type of the device.
+    :return: The device object.
+    :raises TypeError: If the device is not found or is not of the correct type.
+    """
+    device = robot.getDevice(name)
+    if not isinstance(device, kind):
+        raise TypeError(f"Failed to get device: {name}.")
+    return device
+
+
+def add_jitter(
+    value: float,
+    value_range: tuple[float, float],
+    std_dev_percent: float = 2.0,
+    offset_percent: float = 0.0,
+) -> float:
+    """Adds normally distributed jitter to a given value."""
+    std_dev = value * (std_dev_percent / 100.0)
+    mean_offset = value * (offset_percent / 100.0)
+
+    error = gauss(mean_offset, std_dev)
+    # Ensure the error is within the range
+    return max(value_range[0], min(value_range[1], value + error))

simulator/modules/sbot_interface/setup.py

@@ -0,0 +1,134 @@
+"""
+Setup the devices connected to the robot.
+
+The main configuration for the devices connected to the robot is the devices
+list in the setup_devices function.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.boards import (
+    Arduino,
+    CameraBoard,
+    LedBoard,
+    MotorBoard,
+    PowerBoard,
+    ServoBoard,
+    TimeServer,
+)
+from sbot_interface.devices.arduino_devices import (
+    EmptyPin,
+    MicroSwitch,
+    ReflectanceSensor,
+    UltrasonicSensor,
+)
+from sbot_interface.devices.camera import Camera
+from sbot_interface.devices.led import Led, NullLed
+from sbot_interface.devices.motor import Motor
+from sbot_interface.devices.power import NullBuzzer, Output, StartButton
+from sbot_interface.devices.servo import NullServo
+from sbot_interface.socket_server import Board, DeviceServer, SocketServer
+
+
+def setup_devices(log_level: int | str = logging.WARNING) -> SocketServer:
+    """
+    Setup the devices connected to the robot.
+
+    Contains the main configuration for the devices connected to the robot.
+
+    :param log_level: The logging level to use for the device logger.
+    :return: The socket server which will handle all connections and commands.
+    """
+    device_logger = logging.getLogger('sbot_interface')
+    device_logger.setLevel(log_level)
+
+    # this is the configuration of devices connected to the robot
+    devices: list[Board] = [
+        PowerBoard(
+            outputs=[Output() for _ in range(7)],
+            buzzer=NullBuzzer(),
+            button=StartButton(),
+            leds=(NullLed(), NullLed()),
+            asset_tag='PWR',
+        ),
+        MotorBoard(
+            motors=[
+                Motor('left motor'),
+                Motor('right motor'),
+            ],
+            asset_tag='MOT',
+        ),
+        ServoBoard(
+            servos=[NullServo() for _ in range(8)],
+            asset_tag='SERVO',
+        ),
+        LedBoard(
+            leds=[
+                Led('led 1', num_colours=8),
+                Led('led 2', num_colours=8),
+                Led('led 3', num_colours=8),
+            ],
+            asset_tag='LED',
+        ),
+        Arduino(
+            pins=[
+                EmptyPin(),  # pin 0
+                EmptyPin(),  # pin 1
+                EmptyPin(),  # ultrasonic trigger pin, pin 2
+                UltrasonicSensor('ultrasound front'),  # pin 3
+                EmptyPin(),  # ultrasonic trigger pin, pin 4
+                UltrasonicSensor('ultrasound left'),  # pin 5
+                EmptyPin(),  # ultrasonic trigger pin, pin 6
+                UltrasonicSensor('ultrasound right'),  # pin 7
+                EmptyPin(),  # ultrasonic trigger pin, pin 8
+                UltrasonicSensor('ultrasound back'),  # pin 9
+                MicroSwitch('front left bump sensor'),  # pin 10
+                MicroSwitch('front right bump sensor'),  # pin 11
+                MicroSwitch('rear left bump sensor'),  # pin 12
+                MicroSwitch('rear right bump sensor'),  # pin 13
+                ReflectanceSensor('left reflectance sensor'),  # pin A0
+                ReflectanceSensor('center reflectance sensor'),  # pin A1
+                ReflectanceSensor('right reflectance sensor'),  # pin A2
+                EmptyPin(),  # pin A3
+                EmptyPin(),  # pin A4
+                EmptyPin(),  # pin A5
+            ],
+            asset_tag='Arduino1',
+        ),
+        TimeServer(
+            asset_tag='TimeServer',
+        ),
+        CameraBoard(
+            Camera('camera', frame_rate=15),
+            asset_tag='Camera',
+        ),
+    ]
+
+    device_servers: list[DeviceServer] = []
+
+    for device in devices:
+        # connect each device to a socket to receive commands from sr-robot3
+        device_servers.append(DeviceServer(device))
+
+    # collect all device servers into a single server which will handle all connections
+    # and commands
+    return SocketServer(device_servers)
+
+
+def main() -> None:
+    """
+    Main function to setup and run the devices. Only used for testing.
+
+    This function will setup the devices and start the select loop to handle all connections.
+    """
+    server = setup_devices(logging.DEBUG)
+    # generate and print the socket url and information for each device
+    print(server.links_formatted())
+    # start select loop for all server sockets and device sockets
+    server.run()
+
+
+if __name__ == '__main__':
+    logging.basicConfig(level=logging.INFO)
+    main()

simulator/modules/sbot_interface/socket_server.py

@@ -0,0 +1,228 @@
+"""A server for multiple devices that can be connected to the simulator."""
+from __future__ import annotations
+
+import logging
+import os
+import select
+import signal
+import socket
+import sys
+from threading import Event
+from typing import Protocol
+
+from sbot_interface.devices.util import get_globals
+
+LOGGER = logging.getLogger(__name__)
+g = get_globals()
+
+
+class Board(Protocol):
+    """The interface for all board simulators that can be connected to the simulator."""
+
+    asset_tag: str
+    software_version: str
+
+    def handle_command(self, command: str) -> str | bytes:
+        """
+        Process a command string and return the response.
+
+        Bytes type are treated as tag-length-value (TLV) encoded data.
+        """
+        pass
+
+
+class DeviceServer:
+    """
+    A server for a single device that can be connected to the simulator.
+
+    The process_data method is called when data is received from the socket.
+    Line-delimited commands are processed and responses are sent back.
+    """
+
+    def __init__(self, board: Board) -> None:
+        self.board = board
+        # create TCP socket server
+        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.server_socket.bind(('127.0.0.1', 0))
+        self.server_socket.listen(1)  # only allow one connection per device
+        self.server_socket.setblocking(True)
+        LOGGER.info(
+            f'Started server for {self.board_type} ({self.board.asset_tag}) '
+            f'on port {self.port}'
+        )
+
+        self.device_socket: socket.socket | None = None
+        self.buffer = b''
+
+    def process_data(self, data: bytes) -> bytes | None:
+        """Process incoming data if a line has been received and return the response."""
+        self.buffer += data
+        if b'\n' in self.buffer:
+            # Sleep to simulate processing time
+            g.sleep(g.timestep / 1000)
+            data, self.buffer = self.buffer.split(b'\n', 1)
+            return self.run_command(data.decode().strip())
+        else:
+            return None
+
+    def run_command(self, command: str) -> bytes:
+        """
+        Process a command and return the response.
+
+        Wraps the board's handle_command method and deals with exceptions and data types.
+        """
+        LOGGER.debug(f'> {command}')
+        try:
+            response = self.board.handle_command(command)
+            if isinstance(response, bytes):
+                LOGGER.debug(f'< {len(response)} bytes')
+                return response
+            else:
+                LOGGER.debug(f'< {response}')
+                return response.encode() + b'\n'
+        except Exception as e:
+            LOGGER.exception(f'Error processing command: {command}')
+            return f'NACK:{e}\n'.encode()
+
+    def flush_buffer(self) -> None:
+        """Clear the internal buffer of received data."""
+        self.buffer = b''
+
+    def socket(self) -> socket.socket:
+        """
+        Return the socket to select on.
+
+        If the device is connected, return the device socket.
+        Otherwise, return the server socket.
+        """
+        if self.device_socket is not None:
+            # ignore the server socket while we are connected
+            return self.device_socket
+        else:
+            return self.server_socket
+
+    def accept(self) -> None:
+        """Accept a connection from a device and set the device socket to blocking."""
+        if self.device_socket is not None:
+            self.disconnect_device()
+        self.device_socket, _ = self.server_socket.accept()
+        self.device_socket.setblocking(True)
+        LOGGER.info(f'Connected to {self.asset_tag} from {self.device_socket.getpeername()}')
+
+    def disconnect_device(self) -> None:
+        """Close the device socket, flushing the buffer first."""
+        self.flush_buffer()
+        if self.device_socket is not None:
+            self.device_socket.close()
+            self.device_socket = None
+            LOGGER.info(f'Disconnected from {self.asset_tag}')
+
+    def close(self) -> None:
+        """Close the server and client sockets."""
+        self.disconnect_device()
+        self.server_socket.close()
+
+    def __del__(self) -> None:
+        self.close()
+
+    @property
+    def port(self) -> int:
+        """Return the port number of the server socket."""
+        if self.server_socket is None:
+            return -1
+        return int(self.server_socket.getsockname()[1])
+
+    @property
+    def asset_tag(self) -> str:
+        """Return the asset tag of the board."""
+        return self.board.asset_tag
+
+    @property
+    def board_type(self) -> str:
+        """Return the class name of the board object."""
+        return self.board.__class__.__name__
+
+
+class SocketServer:
+    """
+    A server for multiple devices that can be connected to the simulator.
+
+    The run method blocks until the stop_event is set.
+    """
+
+    def __init__(self, devices: list[DeviceServer]) -> None:
+        self.devices = devices
+        self.stop_event = Event()
+        g.stop_event = self.stop_event
+        # flag to indicate that we are exiting because the usercode has completed
+        self.completed = False
+
+    def run(self) -> None:
+        """
+        Run the server, accepting connections and processing data.
+
+        This method blocks until the stop_event is set.
+        """
+        while not self.stop_event.is_set():
+            # select on all server sockets and device sockets
+            sockets = [device.socket() for device in self.devices]
+
+            readable, _, _ = select.select(sockets, [], [], 0.5)
+
+            for device in self.devices:
+                try:
+                    if device.server_socket in readable:
+                        device.accept()
+
+                    if device.device_socket in readable and device.device_socket is not None:
+                        try:
+                            if sys.platform == 'win32':
+                                data = device.device_socket.recv(4096)
+                            else:
+                                data = device.device_socket.recv(4096, socket.MSG_DONTWAIT)
+                        except ConnectionError:
+                            device.disconnect_device()
+                            continue
+
+                        if not data:
+                            device.disconnect_device()
+                        else:
+                            response = device.process_data(data)
+                            if response is not None:
+                                try:
+                                    device.device_socket.sendall(response)
+                                except ConnectionError:
+                                    device.disconnect_device()
+                                    continue
+                except Exception as e:
+                    LOGGER.exception(f"Failure in simulated boards: {e}")
+
+        LOGGER.info('Stopping server')
+        for device in self.devices:
+            device.close()
+
+        if self.stop_event.is_set() and self.completed is False:
+            # Stop the usercode
+            os.kill(os.getpid(), signal.SIGINT)
+
+    def links(self) -> dict[str, dict[str, str]]:
+        """Return a mapping of asset tags to ports, grouped by board type."""
+        return {
+            device.asset_tag: {
+                'board_type': device.board_type,
+                'port': str(device.port),
+            }
+            for device in self.devices
+        }
+
+    def links_formatted(self, address: str = '127.0.0.1') -> str:
+        """
+        Return a formatted string of all the links to the devices.
+
+        The format is 'socket://address:port/board_type/asset_tag'.
+        Each link is separated by a newline.
+        """
+        return '\n'.join(
+            f"socket://{address}:{data['port']}/{data['board_type']}/{asset_tag}"
+            for asset_tag, data in self.links().items()
+        )