First commit

simulator/VERSION

@@ -0,0 +1 @@
+2026.0.1

simulator/controllers/competition_supervisor/competition_supervisor.py

@@ -0,0 +1,263 @@
+"""."""
+from __future__ import annotations
+
+import sys
+import time
+from contextlib import contextmanager
+from pathlib import Path
+from typing import Iterator
+
+from controller import Supervisor
+
+# Robot constructor lacks a return type annotation in R2023b
+sys.path.insert(0, Supervisor().getProjectPath())  # type: ignore[no-untyped-call]
+# from lighting_control import LightingControl
+import environment  # configure path to include modules
+from robot_logging import get_match_identifier, prefix_and_tee_streams
+from robot_utils import get_game_mode, get_match_data, get_robot_file
+
+# Get the robot object that was created when setting up the environment
+_robot = Supervisor.created
+assert _robot is not None, "Robot object not created"
+supervisor: Supervisor = _robot  # type: ignore[assignment]
+
+
+class RobotData:
+    """Data about a robot in the arena."""
+
+    def __init__(self, zone: int):
+        self.registered_ready = False
+        self.zone = zone
+        self.robot = supervisor.getFromDef(f'ROBOT{zone}')
+        if self.robot is None:
+            raise ValueError(f"Failed to get Webots node for zone {zone}")
+
+    def zone_occupied(self) -> bool:
+        """Check if this zone has a robot.py file associated with it."""
+        try:
+            _ = get_robot_file(self.zone)
+        except FileNotFoundError:
+            return False
+        return True
+
+    def remove_robot(self) -> None:
+        """Delete the robot proto from the world."""
+        self.robot.remove()  # type: ignore[attr-defined]
+
+    def preset_robot(self) -> None:
+        """Arm the robot so that it waits for the start signal."""
+        self.robot.getField('customData').setSFString('prestart')  # type: ignore[attr-defined]
+
+    def robot_ready(self) -> bool:
+        """Check if robot has set its pre-start flag."""
+        return bool(self.robot.getField('customData').getSFString() == 'ready')  # type: ignore[attr-defined]
+
+    def start_robot(self) -> None:
+        """Signal to the robot that the start button has been pressed."""
+        self.robot.getField('customData').setSFString('start')  # type: ignore[attr-defined]
+
+
+class Robots:
+    """A collection of robots in the arena."""
+
+    def __init__(self) -> None:
+        self.robots: dict[int, RobotData] = {}
+
+        for zone in range(0, environment.NUM_ZONES):
+            try:
+                robot_data = RobotData(zone)
+            except ValueError as e:
+                print(e)
+            else:
+                self.robots[zone] = robot_data
+
+    def remove_unoccupied_robots(self) -> None:
+        """Remove all robots that don't have usercode."""
+        for robot in list(self.robots.values()):
+            if not robot.zone_occupied():
+                robot.remove_robot()
+                _ = self.robots.pop(robot.zone)
+
+    def preset_robots(self) -> None:
+        """Arm all robots so that they wait for the start signal."""
+        for robot in self.robots.values():
+            robot.preset_robot()
+
+    def wait_for_ready(self, timeout: float) -> None:
+        """Wait for all robots to set their pre-start flags."""
+        end_time = supervisor.getTime() + timeout
+        while supervisor.getTime() < end_time:
+            all_ready = True
+            # Sleep in individual timesteps to allow the robots to update
+            supervisor.step()
+
+            for zone, robot in self.robots.items():
+                if not robot.registered_ready:
+                    if robot.robot_ready():
+                        print(f"Robot in zone {zone} is ready.")
+                        # Log only once per robot when ready
+                        robot.registered_ready = True
+                    else:
+                        all_ready = False
+            if all_ready:
+                break
+        else:
+            pending_robots = ', '.join([
+                str(zone)
+                for zone, robot in self.robots.items()
+                if not robot.robot_ready()
+            ])
+            raise TimeoutError(
+                f"Robots in zones {pending_robots} failed to initialise. "
+                f"Failed to reach wait_start() within {timeout} seconds."
+            )
+
+    def start_robots(self) -> None:
+        """Signal to all robots that their start buttons have been pressed."""
+        for robot in self.robots.values():
+            robot.start_robot()
+
+
+def is_dev_mode() -> bool:
+    """Load the mode file and check if we are in dev mode."""
+    return (get_game_mode() == 'dev')
+
+
+@contextmanager
+def record_animation(filename: Path) -> Iterator[None]:
+    """Record an animation for the duration of the manager."""
+    filename.parent.mkdir(parents=True, exist_ok=True)
+    print(f"Saving animation to {filename}")
+    supervisor.animationStartRecording(str(filename))
+    yield
+    supervisor.animationStopRecording()  # type: ignore[no-untyped-call]
+
+
+@contextmanager
+def record_video(
+    filename: Path,
+    resolution: tuple[int, int],
+    skip: bool = False
+) -> Iterator[None]:
+    """Record a video for the duration of the manager."""
+    filename.parent.mkdir(parents=True, exist_ok=True)
+
+    if skip:
+        print('Not recording movie')
+        yield
+        return
+    else:
+        print(f"Saving video to {filename}")
+
+    supervisor.movieStartRecording(
+        str(filename),
+        width=resolution[0],
+        height=resolution[1],
+        quality=100,
+        codec=0,
+        acceleration=1,
+        caption=False,
+    )
+    yield
+    supervisor.movieStopRecording()  # type: ignore[no-untyped-call]
+
+    while not supervisor.movieIsReady():  # type: ignore[no-untyped-call]
+        time.sleep(0.1)
+
+    if supervisor.movieFailed():  # type: ignore[no-untyped-call]
+        print("Movie failed to record")
+
+
+def save_image(filename: Path) -> None:
+    """Capture an image of the arena."""
+    filename.parent.mkdir(parents=True, exist_ok=True)
+    print(f"Saving image to {filename}")
+    supervisor.exportImage(str(filename), 100)
+
+
+def run_match(
+    match_duration: int,
+    media_path_stem: Path,
+    video_resolution: tuple[int, int],
+    skip_video: bool,
+) -> None:
+    """Run a match in the arena."""
+    robots = Robots()
+    robots.remove_unoccupied_robots()
+
+    time_step = int(supervisor.getBasicTimeStep())
+    match_timesteps = (match_duration * 1000) // time_step
+    # lighting_control = LightingControl(supervisor, match_timesteps)
+
+    robots.preset_robots()
+
+    robots.wait_for_ready(5)
+
+    with record_animation(media_path_stem.with_suffix('.html')):
+        # Animations don't support lighting changes so start the animation before
+        # setting the lighting. Step the simulation to allow the animation to start.
+        supervisor.step()
+        # Set initial lighting
+        # lighting_control.service_lighting(0)
+        with record_video(media_path_stem.with_suffix('.mp4'), video_resolution, skip_video):
+            print("===========")
+            print("Match start")
+            print("===========")
+
+            # We are ready to start the match now. "Press" the start button on the robots
+            robots.start_robots()
+            supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST)  # type: ignore[attr-defined]
+
+            # for current_step in range(match_timesteps + 1):
+            #     lighting_control.service_lighting(current_step)
+            #     supervisor.step(time_step)
+            supervisor.step(match_timesteps)
+
+            print("==================")
+            print("Game over, pausing")
+            print("==================")
+            supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)  # type: ignore[attr-defined]
+
+        # To allow for a clear image of the final state, we have reset the
+        # lighting after the final frame of the video.
+        save_image(media_path_stem.with_suffix('.jpg'))
+        # TODO score match
+
+
+def main() -> None:
+    """Run the competition supervisor."""
+    if is_dev_mode():
+        robots = Robots()
+        robots.remove_unoccupied_robots()
+        exit()
+
+    match_data = get_match_data()
+    match_id = get_match_identifier()
+
+    prefix_and_tee_streams(
+        environment.ARENA_ROOT / f'supervisor-log-{match_id}.txt',
+        prefix=lambda: f'[{supervisor.getTime():0.3f}] ',
+    )
+
+    try:
+        # TODO check for required libraries?
+
+        run_match(
+            match_data.match_duration,
+            environment.ARENA_ROOT / 'recordings' / match_id,
+            video_resolution=match_data.video_resolution,
+            skip_video=(not match_data.video_enabled),
+        )
+        # Set the overall Webots exit code to follow the supervisor's exit code
+    except Exception as e:
+        # Print and step so error is printed to console
+        print(f"Error: {e}")
+        supervisor.step()
+        supervisor.simulationQuit(1)
+        raise
+    else:
+        supervisor.simulationQuit(0)
+
+
+if __name__ == '__main__':
+    main()

simulator/controllers/competition_supervisor/lighting_control.py

@@ -0,0 +1,287 @@
+"""
+The controller for altering arena lighting provided by a DirectionalLight and a Background.
+
+Currently doesn't support:
+- Timed pre-match lighting changes
+"""
+from __future__ import annotations
+
+from typing import NamedTuple
+
+from controller import Node, Supervisor
+
+MATCH_LIGHTING_INTENSITY = 1.5
+DEFAULT_LUMINOSITY = 1
+
+
+class FromEnd(NamedTuple):
+    """
+    Represents a time relative to the end of the match.
+
+    Negative values are times before the end of the match. 0 is the last frame
+    of the video. All positive values will only appear in the post-match image.
+    """
+
+    time: float
+
+
+class ArenaLighting(NamedTuple):
+    """Represents a lighting configuration for the arena."""
+
+    light_def: str
+    intensity: float
+    colour: tuple[float, float, float] = (1, 1, 1)
+
+
+class LightingEffect(NamedTuple):
+    """Represents a lighting effect to be applied to the arena."""
+
+    start_time: float | FromEnd
+    fade_time: float | None = None
+    lighting: ArenaLighting = ArenaLighting('SUN', intensity=MATCH_LIGHTING_INTENSITY)
+    luminosity: float = DEFAULT_LUMINOSITY
+    name: str = ""
+
+    def __repr__(self) -> str:
+        light = self.lighting
+        lights_info = [
+            f"({light.light_def}, int={light.intensity}, col={light.colour})"
+        ]
+        return (
+            f"<LightingEffect: {self.name!r}, "
+            f"start={self.start_time}, fade={self.fade_time}, "
+            f"lum={self.luminosity}, "
+            f"{', '.join(lights_info)}"
+            ">"
+        )
+
+
+class LightingStep(NamedTuple):
+    """Represents a step in a lighting fade."""
+
+    timestep: int
+    light_node: Node
+    intensity: float | None
+    colour: tuple[float, float, float] | None
+    luminosity: float | None
+    name: str | None = None
+
+
+CUE_STACK = [
+    LightingEffect(
+        0,
+        lighting=ArenaLighting('SUN', intensity=0.2),
+        luminosity=0.05,
+        name="Pre-set",
+    ),
+    LightingEffect(
+        0,
+        fade_time=1.5,
+        lighting=ArenaLighting('SUN', intensity=MATCH_LIGHTING_INTENSITY),
+        luminosity=1,
+        name="Fade-up",
+    ),
+    LightingEffect(
+        FromEnd(0),  # This time runs this cue as the last frame of the video
+        lighting=ArenaLighting('SUN', intensity=1, colour=(0.8, 0.1, 0.1)),
+        luminosity=0.1,
+        name="End of match",
+    ),
+    LightingEffect(
+        FromEnd(1),
+        lighting=ArenaLighting('SUN', intensity=MATCH_LIGHTING_INTENSITY),
+        luminosity=DEFAULT_LUMINOSITY,
+        name="Post-match image",
+    ),
+]
+
+
+class LightingControl:
+    """Controller for managing lighting effects in the arena."""
+
+    def __init__(self, supervisor: Supervisor, duration: int) -> None:
+        self._robot = supervisor
+        self._final_timestep = duration
+        self.timestep = self._robot.getBasicTimeStep()
+        self.ambient_node = supervisor.getFromDef('AMBIENT')
+
+        # fetch all nodes used in effects, any missing nodes will be flagged here
+        light_names = set(effect.lighting.light_def for effect in CUE_STACK)
+        self.lights = {
+            name: supervisor.getFromDef(name)
+            for name in light_names
+        }
+        missing_lights = [name for name, light in self.lights.items() if light is None]
+        if missing_lights:
+            raise ValueError(f"Missing light nodes: {missing_lights}")
+
+        # Convert FromEnd times to absolute times
+        cue_stack = self.convert_from_end_times(CUE_STACK)
+
+        self.lighting_steps = self.generate_lighting_steps(cue_stack)
+
+    def convert_from_end_times(self, cue_stack: list[LightingEffect]) -> list[LightingEffect]:
+        """Convert FromEnd times to absolute times."""
+        new_cue_stack = []
+        end_time = (self._final_timestep * self.timestep) / 1000
+        # @ 25 fps the last 5 timesteps are not included in the video
+        start_of_frame_offset = self.timestep * 6 / 1000
+        for cue in cue_stack:
+            if isinstance(cue.start_time, FromEnd):
+                abs_time = end_time + cue.start_time.time - start_of_frame_offset
+                new_cue_stack.append(cue._replace(start_time=abs_time))
+            else:
+                new_cue_stack.append(cue)
+
+        return new_cue_stack
+
+    def generate_lighting_steps(self, cue_stack: list[LightingEffect]) -> list[LightingStep]:
+        """Expand the cue stack into a list of lighting steps."""
+        steps: list[LightingStep] = []
+
+        # Generate current values for all lights
+        current_values = {
+            name: LightingStep(
+                0,
+                light,
+                light.getField('intensity').getSFFloat(),  # type: ignore[attr-defined]
+                light.getField('color').getSFColor(),  # type: ignore[attr-defined]
+                0,
+            )
+            for name, light in self.lights.items()
+        }
+        current_luminosity = self.ambient_node.getField('luminosity').getSFFloat()  # type: ignore[attr-defined]
+
+        for cue in cue_stack:
+            # Get the current state of the light with the current luminosity
+            current_state = current_values[cue.lighting.light_def]
+            current_state = current_state._replace(luminosity=current_luminosity)
+
+            expanded_cue = self.expand_lighting_fade(cue, current_state)
+
+            # Update current values from the last step of the cue
+            current_values[cue.lighting.light_def] = expanded_cue[-1]
+            current_luminosity = expanded_cue[-1].luminosity
+
+            steps.extend(expanded_cue)
+
+        steps.sort(key=lambda x: x.timestep)
+        # TODO optimise steps to remove duplicate steps
+
+        return steps
+
+    def expand_lighting_fade(
+        self,
+        cue: LightingEffect,
+        current_state: LightingStep,
+    ) -> list[LightingStep]:
+        """Expand a fade effect into a list of steps."""
+        fades = []
+
+        assert isinstance(cue.start_time, (float, int)), \
+            "FromEnd times should be converted to absolute times"
+        cue_start = int((cue.start_time * 1000) / self.timestep)
+
+        if cue.fade_time is None:
+            # no fade, just set values
+            return [LightingStep(
+                cue_start,
+                self.lights[cue.lighting.light_def],
+                cue.lighting.intensity,
+                cue.lighting.colour,
+                cue.luminosity,
+                cue.name
+            )]
+
+        assert current_state.intensity is not None, "Current intensity should be set"
+        assert current_state.colour is not None, "Current colour should be set"
+        assert current_state.luminosity is not None, "Current luminosity should be set"
+
+        fade_steps = int((cue.fade_time * 1000) / self.timestep)
+        if fade_steps == 0:
+            fade_steps = 1
+        intensity_step = (cue.lighting.intensity - current_state.intensity) / fade_steps
+        colour_step = [
+            (cue.lighting.colour[0] - current_state.colour[0]) / fade_steps,
+            (cue.lighting.colour[1] - current_state.colour[1]) / fade_steps,
+            (cue.lighting.colour[2] - current_state.colour[2]) / fade_steps,
+        ]
+        luminosity_step = (cue.luminosity - current_state.luminosity) / fade_steps
+
+        for step in range(fade_steps):
+            fades.append(
+                LightingStep(
+                    cue_start + step,
+                    self.lights[cue.lighting.light_def],
+                    current_state.intensity + intensity_step * step,
+                    (
+                        current_state.colour[0] + (colour_step[0] * step),
+                        current_state.colour[1] + (colour_step[1] * step),
+                        current_state.colour[2] + (colour_step[2] * step),
+                    ),
+                    current_state.luminosity + luminosity_step * step,
+                    cue.name if step == 0 else None,
+                )
+            )
+
+        # Replace the last step with the final values
+        fades.pop()
+        fades.append(LightingStep(
+            cue_start + fade_steps,
+            self.lights[cue.lighting.light_def],
+            cue.lighting.intensity,
+            cue.lighting.colour,
+            cue.luminosity,
+        ))
+
+        return fades
+
+    def set_luminosity(self, luminosity: float) -> None:
+        """Set the luminosity of the ambient node."""
+        self.ambient_node.getField('luminosity').setSFFloat(float(luminosity))  # type: ignore[attr-defined]
+
+    def set_node_intensity(self, node: Node, intensity: float) -> None:
+        """Set the intensity of a node."""
+        node.getField('intensity').setSFFloat(float(intensity))  # type: ignore[attr-defined]
+
+    def set_node_colour(self, node: Node, colour: tuple[float, float, float]) -> None:
+        """Set the colour of a node."""
+        node.getField('color').setSFColor(list(colour))  # type: ignore[attr-defined]
+
+    def service_lighting(self, current_timestep: int) -> int:
+        """Service the lighting effects for the current timestep."""
+        index = 0
+
+        if current_timestep >= self._final_timestep and self.lighting_steps:
+            # Run all remaining steps
+            current_timestep = self.lighting_steps[-1].timestep
+
+        while (
+            len(self.lighting_steps) > index
+            and self.lighting_steps[index].timestep == current_timestep
+        ):
+            lighting_step = self.lighting_steps[index]
+            if lighting_step.name is not None:
+                print(
+                    f"Running lighting effect: {lighting_step.name} @ "
+                    f"{current_timestep * self.timestep / 1000}"
+                )
+
+            if lighting_step.intensity is not None:
+                self.set_node_intensity(lighting_step.light_node, lighting_step.intensity)
+
+            if lighting_step.colour is not None:
+                self.set_node_colour(lighting_step.light_node, lighting_step.colour)
+
+            if lighting_step.luminosity is not None:
+                self.set_luminosity(lighting_step.luminosity)
+
+            index += 1
+
+        # Remove all steps that have been processed
+        self.lighting_steps = self.lighting_steps[index:]
+
+        if self.lighting_steps:
+            return self.lighting_steps[0].timestep - current_timestep
+        else:
+            return -1

simulator/controllers/competition_supervisor/runtime.ini

@@ -0,0 +1,3 @@
+
+[python]
+COMMAND = /home/syed/tmp/sbot-simulator-2026.0.1/venv/bin/python

simulator/controllers/usercode_runner/runtime.ini

@@ -0,0 +1,3 @@
+
+[python]
+COMMAND = /home/syed/tmp/sbot-simulator-2026.0.1/venv/bin/python

simulator/controllers/usercode_runner/usercode_runner.py

@@ -0,0 +1,150 @@
+"""
+The entry point for all robot controllers.
+
+This script is responsible for setting up the environment, starting the devices,
+and running the usercode.
+
+The board simulators are run in a separate thread to allow the usercode to run
+in the main thread. This provides the interface between the sr-robot3 module and Webots.
+"""
+import atexit
+import json
+import logging
+import os
+import runpy
+import sys
+import threading
+from pathlib import Path
+from tempfile import TemporaryDirectory
+
+from controller import Robot
+
+# Robot constructor lacks a return type annotation in R2023b
+sys.path.insert(0, Robot().getProjectPath())  # type: ignore[no-untyped-call]
+import environment  # configure path to include modules
+from robot_logging import get_match_identifier, prefix_and_tee_streams
+from robot_utils import get_game_mode, get_robot_file, print_simulation_version
+from sbot_interface.setup import setup_devices
+from sbot_interface.socket_server import SocketServer
+
+# Get the robot object that was created when setting up the environment
+_robot = Robot.created
+assert _robot is not None, "Robot object not created"
+robot = _robot
+
+LOGGER = logging.getLogger('usercode_runner')
+
+
+def start_devices() -> SocketServer:
+    """
+    Create the board simulators and return the SocketServer object.
+
+    Using the links or links_formatted method of the SocketServer object, the
+    devices' socket addresses can be accessed and passed to the usercode.
+
+    The WEBOTS_DEVICE_LOGGING environment variable, overrides the log level used.
+    Default is WARNING.
+    """
+    if log_level := os.environ.get('WEBOTS_DEVICE_LOGGING'):
+        return setup_devices(log_level)
+    else:
+        return setup_devices()
+
+
+def run_usercode(robot_file: Path, robot_zone: int, game_mode: str) -> None:
+    """
+    Run the user's code from the given file.
+
+    Metadata is created in a temporary directory and passed to the usercode.
+    The system path is modified to avoid the controller modules being imported
+    in the usercode.
+
+    :param robot_file: The path to the robot file
+    :param robot_zone: The zone number
+    :param game_mode: The game mode string ('dev' or 'comp')
+    :raises Exception: If the usercode raises an exception
+    """
+    # Remove this folder from the path
+    sys.path.remove(str(Path.cwd()))
+    # Remove our custom modules from the path
+    sys.path.remove(str(environment.MODULES_ROOT))
+    # Add the usercode folder to the path
+    sys.path.insert(0, str(robot_file.parent))
+
+    # Change the current directory to the usercode folder
+    os.chdir(robot_file.parent)
+
+    with TemporaryDirectory() as tmpdir:
+        # Setup metadata (zone, game_mode)
+        Path(tmpdir).joinpath('astoria.json').write_text(json.dumps({
+            "arena": "simulator",
+            "zone": robot_zone,
+            "mode": 'COMP' if game_mode == 'comp' else 'DEV',
+        }))
+        os.environ['SBOT_METADATA_PATH'] = tmpdir
+        os.environ['SBOT_USBKEY_PATH'] = str(Path.cwd())
+
+        # Run the usercode
+        # pass robot object to the usercode for keyboard robot control
+        runpy.run_path(str(robot_file), init_globals={'__robot__': robot})
+
+
+def main() -> bool:
+    """
+    The main entry point for the usercode runner.
+
+    This function is responsible for setting up the environment, starting the
+    devices, and running the usercode.
+
+    The zone number is passed as the first argument to the script using
+    controllerArgs on the robot.
+
+    On completion, the devices are stopped and atexit functions are run.
+    """
+    zone = int(sys.argv[1])
+    game_mode = get_game_mode()
+
+    # Get the robot file
+    try:
+        robot_file = get_robot_file(zone)
+    except FileNotFoundError as e:
+        print(e.args[0])
+        robot.step()
+        # Not having a robot file is not an error in dev mode
+        return game_mode != 'comp'
+
+    # Setup log file
+    prefix_and_tee_streams(
+        robot_file.parent / f'log-zone-{zone}-{get_match_identifier()}.txt',
+        prefix=lambda: f'[{zone}| {robot.getTime():0.3f}] ',
+    )
+
+    # Setup devices
+    devices = start_devices()
+
+    # Print the simulation version
+    print_simulation_version()
+
+    # Pass the devices to the usercode
+    os.environ['WEBOTS_SIMULATOR'] = '1'
+    os.environ['WEBOTS_ROBOT'] = devices.links_formatted()
+
+    # Start devices in a separate thread
+    thread = threading.Thread(target=devices.run)
+    thread.start()
+
+    # Run the usercode
+    try:
+        run_usercode(robot_file, zone, game_mode)
+    finally:
+        # Run cleanup code registered in the usercode
+        atexit._run_exitfuncs()  # noqa: SLF001
+        # Cleanup devices
+        devices.completed = True
+        devices.stop_event.set()
+
+    return True
+
+
+if __name__ == '__main__':
+    exit(0 if main() else 1)

simulator/environment.py

@@ -0,0 +1,42 @@
+"""
+Configure the sys.path list for importing simulator modules.
+
+Also contains constants for where several important files are located.
+"""
+import os
+import sys
+from pathlib import Path
+
+SIM_ROOT = Path(__file__).absolute().parent
+MODULES_ROOT = SIM_ROOT / 'modules'
+
+ARENA_ROOT = Path(os.environ.get('ARENA_ROOT', SIM_ROOT.parent))
+ZONE_ROOT = ARENA_ROOT
+GAME_MODE_FILE = ARENA_ROOT / 'mode.txt'
+
+NUM_ZONES = 2
+DEFAULT_MATCH_DURATION = 150  # seconds
+
+
+if not ARENA_ROOT.is_absolute():
+    # Webots sets the current directory of each controller to the directory of
+    # the controller file. As such, relative paths would be ambiguous.
+    # Hint: `$PWD` or `%CD%` may be useful to construct an absolute path from
+    # your relative path.
+    raise ValueError(f"'ARENA_ROOT' must be an absolute path, got {ARENA_ROOT!r}")
+
+
+def setup_environment() -> None:
+    """
+    Set up the environment for the simulator.
+
+    This function configures the sys.path list to allow importing of the included
+    simulator modules.
+    """
+    sys.path.insert(0, str(MODULES_ROOT))
+    this_dir = str(Path(__file__).parent)
+    if this_dir in sys.path:
+        sys.path.remove(this_dir)
+
+
+setup_environment()

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()
+        )

simulator/protos/SR2025bot.proto

@@ -0,0 +1,204 @@
+#VRML_SIM R2023b utf8
+EXTERNPROTO "./robot/MotorAssembly.proto"
+EXTERNPROTO "./robot/Caster.proto"
+EXTERNPROTO "./robot/RobotCamera.proto"
+EXTERNPROTO "./robot/UltrasoundModule.proto"
+EXTERNPROTO "./robot/RGBLed.proto"
+EXTERNPROTO "./robot/ReflectanceSensor.proto"
+EXTERNPROTO "./robot/Flag.proto"
+EXTERNPROTO "./robot/BumpSensor.proto"
+EXTERNPROTO "./robot/VacuumSucker.proto"
+
+PROTO SR2025bot [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFString controller "<generic>"
+  field MFString controllerArgs []
+  field SFString customData ""
+  field SFColor flagColour 1 1 1
+] {
+  Robot {
+    name IS name
+    translation IS translation
+    rotation IS rotation
+    controller IS controller
+    controllerArgs IS controllerArgs
+    customData IS customData
+    children [
+      Pose {
+        translation 0 0 0.049
+        children [
+          MotorAssembly {
+            name "left motor"
+            rotation 0 0 1 3.1415
+            reversed TRUE
+            translation 0 0.14 0
+          }
+          MotorAssembly {
+            name "right motor"
+            translation 0 -0.14 0
+          }
+          Caster {
+            name "caster"
+            translation -0.15 0 -0.045
+          }
+          DEF BASE Solid {
+            translation -0.065 0 -0.02
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.757 0.604 0.424
+                  roughness 1
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.25 0.25 0.02
+                }
+              }
+            ]
+            name "Chassis"
+            boundingObject DEF BASE_GEO Box {
+              size 0.25 0.25 0.02
+            }
+            physics Physics {
+              density 2000  # 66% Aluminium
+            }
+          }
+          DEF BOARD Solid {
+            translation -0.05 0 0
+            rotation 0 0 1 1.5708
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0 0 0
+                  roughness 1
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.08 0.06 0.02
+                }
+              }
+            ]
+            name "Board"
+          }
+          DEF STABILISER Solid {
+            translation -0.16 0 0
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.8 0.8 0.75
+                  roughness 1
+                  metalness 0
+                }
+                geometry DEF WEIGHT_GEO Box {
+                  size 0.04 0.15 0.02
+                }
+              }
+            ]
+            name "Chassis weight"
+            boundingObject USE WEIGHT_GEO
+            physics Physics {
+              density 8000  # Steel
+            }
+          }
+          RobotCamera {
+            name "camera"
+            translation 0.05 0 0.03
+          }
+          Solid {
+            translation 0.03 0 0
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.4 0.4 0.4
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.01 0.01 0.03
+                }
+              }
+            ]
+            name "Camera riser"
+          }
+          VacuumSucker {
+            name "vacuum sucker"
+            translation 0.01 0 -0.02
+          }
+          UltrasoundModule {
+            name "ultrasound front"
+            translation 0.04 0 0
+          }
+          UltrasoundModule {
+            name "ultrasound left"
+            translation -0.08 0.12 0
+            rotation 0 0 1 1.5708
+          }
+          UltrasoundModule {
+            name "ultrasound back"
+            translation -0.18 0 0
+            rotation 0 0 1 3.1416
+          }
+          UltrasoundModule {
+            name "ultrasound right"
+            translation -0.08 -0.12 0
+            rotation 0 0 1 -1.5708
+          }
+          RGBLed {
+            name "led 1"
+            translation -0.11 0.08 -0.008
+          }
+          RGBLed {
+            name "led 2"
+            translation -0.11 0 -0.008
+          }
+          RGBLed {
+            name "led 3"
+            translation -0.11 -0.08 -0.008
+          }
+          ReflectanceSensor {
+            name "left reflectance sensor"
+            translation 0.03 0.02 -0.03
+            rotation 0 0 1 1.5708
+          }
+          ReflectanceSensor {
+            name "center reflectance sensor"
+            translation 0.03 0 -0.03
+            rotation 0 0 1 1.5708
+          }
+          ReflectanceSensor {
+            name "right reflectance sensor"
+            translation 0.03 -0.02 -0.03
+            rotation 0 0 1 1.5708
+          }
+          BumpSensor {
+            name "front left bump sensor"
+            translation 0.06 0.06 -0.02
+          }
+          BumpSensor {
+            name "front right bump sensor"
+            translation 0.06 -0.06 -0.02
+          }
+          BumpSensor {
+            name "rear left bump sensor"
+            translation -0.19 0.06 -0.02
+          }
+          BumpSensor {
+            name "rear right bump sensor"
+            translation -0.19 -0.06 -0.02
+          }
+          Flag {
+            name "flag"
+            translation 0.03 0.07 0.09
+            flagColour IS flagColour
+          }
+        ]
+      }
+    ]
+    boundingObject Pose {
+      translation -0.065 0 0.029
+      children [USE BASE_GEO]
+    }
+    physics Physics {}
+  }
+}

simulator/protos/SRObot.proto

@@ -0,0 +1,178 @@
+#VRML_SIM R2023b utf8
+EXTERNPROTO "./robot/MotorAssembly.proto"
+EXTERNPROTO "./robot/Caster.proto"
+EXTERNPROTO "./robot/RobotCamera.proto"
+EXTERNPROTO "./robot/UltrasoundModule.proto"
+EXTERNPROTO "./robot/RGBLed.proto"
+EXTERNPROTO "./robot/ReflectanceSensor.proto"
+EXTERNPROTO "./robot/Flag.proto"
+EXTERNPROTO "./robot/BumpSensor.proto"
+
+PROTO SRObot [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFString controller "<generic>"
+  field MFString controllerArgs []
+  field SFString customData ""
+  field SFColor flagColour 1 1 1
+] {
+  Robot {
+    name IS name
+    translation IS translation
+    rotation IS rotation
+    controller IS controller
+    controllerArgs IS controllerArgs
+    customData IS customData
+    children [
+      Pose {
+        translation 0 0 0.049
+        children [
+          MotorAssembly {
+            name "left motor"
+            rotation 0 0 1 3.1415
+            reversed TRUE
+            translation 0 0.1 0
+          }
+          MotorAssembly {
+            name "right motor"
+            translation 0 -0.1 0
+          }
+          Caster {
+            name "caster"
+            translation -0.13 0 -0.045
+          }
+          DEF BASE Solid {
+            translation -0.045 0 -0.02
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.757 0.604 0.424
+                  roughness 1
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.21 0.16 0.02
+                }
+              }
+            ]
+            name "Chassis"
+            boundingObject DEF BASE_GEO Box {
+              size 0.21 0.16 0.02
+            }
+            physics Physics {
+              density 3000  # Aluminium
+            }
+          }
+          DEF BOARD Solid {
+            translation -0.06 0 0
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0 0 0
+                  roughness 1
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.08 0.06 0.02
+                }
+              }
+            ]
+            name "Board"
+          }
+          RobotCamera {
+            name "camera"
+            translation 0 0 0.03
+          }
+          Solid {
+            translation -0.01 0 0
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.4 0.4 0.4
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.01 0.01 0.03
+                }
+              }
+            ]
+            name "Camera riser"
+          }
+          UltrasoundModule {
+            name "ultrasound front"
+            translation 0.04 0 0
+          }
+          UltrasoundModule {
+            name "ultrasound left"
+            translation -0.08 0.07 0
+            rotation 0 0 1 1.5708
+          }
+          UltrasoundModule {
+            name "ultrasound back"
+            translation -0.14 0 0
+            rotation 0 0 1 3.1416
+          }
+          UltrasoundModule {
+            name "ultrasound right"
+            translation -0.08 -0.07 0
+            rotation 0 0 1 -1.5708
+          }
+          RGBLed {
+            name "led 1"
+            translation -0.12 0.05 -0.008
+          }
+          RGBLed {
+            name "led 2"
+            translation -0.12 0 -0.008
+          }
+          RGBLed {
+            name "led 3"
+            translation -0.12 -0.05 -0.008
+          }
+          ReflectanceSensor {
+            name "left reflectance sensor"
+            translation 0.03 0.02 -0.03
+            rotation 0 0 1 1.5708
+          }
+          ReflectanceSensor {
+            name "center reflectance sensor"
+            translation 0.03 0 -0.03
+            rotation 0 0 1 1.5708
+          }
+          ReflectanceSensor {
+            name "right reflectance sensor"
+            translation 0.03 -0.02 -0.03
+            rotation 0 0 1 1.5708
+          }
+          BumpSensor {
+            name "front left bump sensor"
+            translation 0.06 0.05 -0.02
+          }
+          BumpSensor {
+            name "front right bump sensor"
+            translation 0.06 -0.05 -0.02
+          }
+          BumpSensor {
+            name "rear left bump sensor"
+            translation -0.15 0.05 -0.02
+          }
+          BumpSensor {
+            name "rear right bump sensor"
+            translation -0.15 -0.05 -0.02
+          }
+          Flag {
+            name "flag"
+            translation 0.03 0.05 0.09
+            flagColour IS flagColour
+          }
+        ]
+      }
+    ]
+    boundingObject Pose {
+      translation -0.045 0 0.029
+      children [USE BASE_GEO]
+    }
+    physics Physics {}
+  }
+}

simulator/protos/arena/Arena.proto

@@ -0,0 +1,136 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO Arena [
+  field SFVec2f size 1 1
+  field SFColor wallColour 0.095395215 0.22841774 0.8000001
+  field MFString floorTexture []
+  field SFBool locked FALSE
+] {
+  Group {
+    children [
+      DEF ARENA_WALLS Group {
+        children [
+          Solid {  # North Wall
+            translation 0 %<= -(fields.size.value.y / 2 + 0.075) >% 0.15
+            children [
+              DEF NORTH_WALL Shape {
+                appearance DEF WALL_COLOUR PBRAppearance {
+                  baseColor IS wallColour
+                  roughness 1
+                  metalness 0
+                }
+                geometry Box {
+                  size %<= fields.size.value.x >% 0.15 0.3
+                }
+              }
+            ]
+            name "North Wall"
+            locked IS locked
+          }
+          Solid {  # East Wall
+            translation %<= -(fields.size.value.x / 2 + 0.075) >% 0 0.15
+            children [
+              DEF EAST_WALL Shape {
+                appearance USE WALL_COLOUR
+                geometry Box {
+                  size 0.15 %<= fields.size.value.y + 0.3 >% 0.3
+                }
+              }
+            ]
+            name "East Wall"
+            locked IS locked
+          }
+          Solid {  # West Wall
+            translation %<= fields.size.value.x / 2 + 0.075 >% 0 0.15
+            children [USE EAST_WALL]
+            name "West Wall"
+            locked IS locked
+          }
+          Solid {  # South Wall
+            translation 0 %<= fields.size.value.y / 2 + 0.075 >% 0.15
+            children [USE NORTH_WALL]
+            name "South Wall"
+            locked IS locked
+          }
+        ]
+      }
+      DEF ARENA_BOUNDING Group {
+        children [
+          Solid {  # Floor
+            translation 0 -0.002 0
+            rotation 0 0 1 3.1416
+            children [
+              DEF FLOOR Shape {
+                appearance Appearance {
+                  material Material {
+                    ambientIntensity 0
+                  }
+                  texture ImageTexture {
+                    url IS floorTexture
+                    repeatS FALSE
+                    repeatT FALSE
+                    filtering 1
+                  }
+                }
+                geometry Plane {
+                  size IS size
+                }
+              }
+            ]
+            name "Floor"
+            boundingObject Plane {
+              size IS size
+            }
+            locked IS locked
+          }
+          Solid {  # Ceiling
+            rotation 1 0 0 3.1419
+            translation 0 0 2
+            boundingObject Plane {
+              size IS size
+            }
+            name "Top boundary"
+            locked IS locked
+          }
+          Solid {  # North bound
+            rotation 1 0 0 -1.5708
+            translation 0 %<= -fields.size.value.y / 2 >% 1
+            boundingObject Plane {
+              size %<= fields.size.value.x >% 2
+            }
+            name "North boundary"
+            locked IS locked
+          }
+          Solid {  # East bound
+            rotation 0 -1 0 -1.5708
+            translation %<= -fields.size.value.x / 2 >% 0 1
+            boundingObject Plane {
+              size 2 %<= fields.size.value.y >%
+            }
+            name "East boundary"
+            locked IS locked
+          }
+          Solid {  # South bound
+            rotation 1 0 0 1.5708
+            translation 0 %<= fields.size.value.y / 2 >% 1
+            boundingObject Plane {
+              size %<= fields.size.value.x >% 2
+            }
+            name "South boundary"
+            locked IS locked
+          }
+          Solid {  # West bound
+            rotation 0 -1 0 1.5708
+            translation %<= fields.size.value.x / 2 >% 0 1
+            boundingObject Plane {
+              size 2 %<= fields.size.value.y >%
+            }
+            name "West boundary"
+            locked IS locked
+          }
+        ]
+      }
+    ]
+  }
+}

simulator/protos/arena/Deck.proto

@@ -0,0 +1,50 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO Deck [
+  field SFVec2f size 1 1
+  field SFVec3f translation 0 0 0.001
+  field SFRotation rotation 1 0 0 0
+  field SFFloat height 0.17
+  field SFColor sideColour 1 1 1
+  field SFColor topColour 0.1 0.1 0.1
+  field SFBool locked FALSE
+  field SFString name ""
+] {
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor IS sideColour
+          roughness 1
+          metalness 0.5
+        }
+        geometry DEF DECK Box {
+          size %<= fields.size.value.x >% %<= fields.size.value.y >% %<= fields.height.value >%
+        }
+      }
+      Solid {
+        translation 0 0 %<= fields.height.value / 2 + 0.002 >%
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor IS topColour
+              roughness 1
+              metalness 0
+            }
+            geometry Plane {
+              size %<= fields.size.value.x >% %<= fields.size.value.y >%
+            }
+            castShadows FALSE
+          }
+        ]
+        name "Top of deck"
+      }
+    ]
+    boundingObject USE DECK
+    name IS name
+    locked IS locked
+  }
+}

simulator/protos/arena/Pillar.proto

@@ -0,0 +1,76 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+# tags: nonDeterministic
+
+EXTERNPROTO "../props/Marker.proto"
+
+PROTO Pillar [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 1 0 0
+  field SFVec3f size 0.13 0.13 0.13
+  field SFVec2f {0.08 0.08, 0.15 0.15, 0.2 0.2} marker_size 0.08 0.08
+  field SFFloat marker_height 0.065
+  field SFColor colour 0.9 0.9 0.9
+  field SFString marker "0"
+  field SFString model ""
+  field MFString texture_url []
+]
+{
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Solid {
+        translation 0 0 %<= fields.size.value.z / 2 >%
+        children [
+          Shape {
+            appearance DEF PILLAR_APPEARANCE PBRAppearance {
+              baseColor IS colour
+              metalness 0
+              roughness 1
+            }
+            geometry DEF PILLAR_GEOMETRY Box {
+              size IS size
+            }
+          }
+          Marker {
+            translation 0 %<= fields.size.value.y / 2 + 0.001 >% %<= fields.marker_height.value - (fields.size.value.z / 2) >%
+            rotation 1 0 0 -1.5708
+            size IS marker_size
+            name "front"
+            model IS marker
+            texture_url IS texture_url
+          }
+          Marker {
+            translation 0 %<= -(fields.size.value.y / 2 + 0.001) >% %<= fields.marker_height.value - (fields.size.value.z / 2) >%
+            rotation 1 0 0 1.5708
+            size IS marker_size
+            name "back"
+            model IS marker
+            texture_url IS texture_url
+          }
+          Marker {
+            translation %<= fields.size.value.x / 2 + 0.001 >% 0 %<= fields.marker_height.value - (fields.size.value.z / 2) >%
+            rotation 0 1 0 1.5708
+            size IS marker_size
+            name "side-1"
+            model IS marker
+            texture_url IS texture_url
+          }
+          Marker {
+            translation %<= -(fields.size.value.x / 2 + 0.001) >% 0 %<= fields.marker_height.value - (fields.size.value.z / 2) >%
+            rotation 0 1 0 -1.5708
+            size IS marker_size
+            name "side-2"
+            model IS marker
+            texture_url IS texture_url
+          }
+        ]
+        name IS model
+        model IS model
+        boundingObject USE PILLAR_GEOMETRY
+        locked TRUE
+      }
+    ]
+  }
+}

simulator/protos/arena/TriangleDeck.proto

@@ -0,0 +1,68 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/geometries/protos/Extrusion.proto"
+
+PROTO TriangleDeck [
+  field SFVec2f size 1 1
+  field SFVec3f translation 0 0 0.001
+  field SFRotation rotation 1 0 0 0
+  field SFFloat height 0.17
+  field SFColor sideColour 1 1 1
+  field SFColor topColour 0.1 0.1 0.1
+  field SFBool locked FALSE
+  field SFString name ""
+] {
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Solid {
+        translation 0 0 %<= -fields.height.value / 2 + 0.002 >%
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor IS sideColour
+              roughness 1
+              metalness 0.5
+            }
+            geometry DEF DECK Extrusion {
+              crossSection [0.5 0.5, 0.5 -0.5, -0.5 0.5, 0.5 0.5]
+              scale %<= fields.size.value.x >% %<= fields.size.value.y >%
+              spine [0 0 0, 0 0 %<= fields.height.value >%]
+              splineSubdivision 1
+            }
+          }
+          Solid {
+            translation 0 0 %<= fields.height.value + 0.002 >%
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor IS topColour
+                  roughness 1
+                  metalness 0
+                }
+                geometry IndexedFaceSet {
+                  coord Coordinate {
+                    point [
+                      %<= -fields.size.value.x / 2 >% %<= -fields.size.value.y / 2 >% 0
+                      %<= fields.size.value.x / 2 >% %<= -fields.size.value.y / 2 >% 0
+                      %<= -fields.size.value.x / 2 >% %<= fields.size.value.y / 2 >% 0
+                    ]
+                  }
+                  coordIndex [
+                    0 1 2 0
+                  ]
+                }
+                castShadows FALSE
+              }
+            ]
+            name "Top of deck"
+          }
+        ]
+        boundingObject USE DECK
+        name IS name
+        locked IS locked
+      }
+    ]
+  }
+}

simulator/protos/props/BoxToken.proto

@@ -0,0 +1,125 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+# tags: nonDeterministic
+
+EXTERNPROTO "./Marker.proto"
+
+PROTO BoxToken [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 1 0 0
+  field SFVec3f size 0.13 0.13 0.13
+  field SFVec2f {0.08 0.08, 0.15 0.15, 0.2 0.2} marker_size 0.08 0.08
+  field SFColor colour 0.7 0.55 0.35
+  field SFString marker "0"
+  field SFString model ""
+  field SFFloat mass 0.080
+  field MFString texture_url []
+  field SFFloat connectorStrength 35
+  field SFFloat connectorShear 20
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Shape {
+        appearance DEF TOKEN_APPEARANCE PBRAppearance {
+          baseColor IS colour
+          metalness 0
+          roughness 1
+        }
+        geometry DEF TOKEN_GEOMETRY Box {
+          size IS size
+        }
+      }
+      Marker {
+        translation 0 %<= fields.size.value.y / 2 + 0.001 >% 0
+        rotation 1 0 0 -1.5708
+        size IS marker_size
+        name "front"
+        model IS marker
+        texture_url IS texture_url
+      }
+      Marker {
+        translation 0 %<= -(fields.size.value.y / 2 + 0.001) >% 0
+        rotation 1 0 0 1.5708
+        size IS marker_size
+        name "back"
+        model IS marker
+        texture_url IS texture_url
+      }
+      Marker {
+        translation %<= fields.size.value.x / 2 + 0.001 >% 0 0
+        rotation 0 1 0 1.5708
+        size IS marker_size
+        name "side-1"
+        model IS marker
+        texture_url IS texture_url
+      }
+      Marker {
+        translation %<= -(fields.size.value.x / 2 + 0.001) >% 0 0
+        rotation 0 1 0 -1.5708
+        size IS marker_size
+        name "side-2"
+        model IS marker
+        texture_url IS texture_url
+      }
+      Marker {
+        translation 0 0 %<= fields.size.value.z / 2 + 0.001 >%
+        rotation 0 0 1 0
+        size IS marker_size
+        name "top"
+        model IS marker
+        texture_url IS texture_url
+      }
+      Marker {
+        translation 0 0 %<= -(fields.size.value.z / 2 + 0.001) >%
+        rotation 0 1 0 3.1416
+        size IS marker_size
+        name "bottom"
+        model IS marker
+        texture_url IS texture_url
+      }
+      # Shape {
+      #   appearance PBRAppearance {
+      #     transparency 0.4
+      #     baseColor 1 0 0
+      #   }
+      #   geometry Sphere {
+      #     radius %<= fields.size.value.x / 2 * 1.4 >%
+      #     subdivision 5
+      #   }
+      # }
+      Connector {
+        type "passive"
+        distanceTolerance %<= fields.size.value.x / 2 * 1.4 >%
+        axisTolerance 3.1415
+        rotationTolerance 0
+        numberOfRotations 0
+        tensileStrength IS connectorStrength
+        shearStrength IS connectorShear
+        snap FALSE
+        name "Front Connector"
+      }
+      Connector {
+        rotation 0 0 1 3.1416
+        type "passive"
+        distanceTolerance %<= fields.size.value.x / 2 * 1.4 >%
+        axisTolerance 3.1415
+        rotationTolerance 0
+        numberOfRotations 0
+        tensileStrength IS connectorStrength
+        shearStrength IS connectorShear
+        snap FALSE
+        name "Rear Connector"
+      }
+    ]
+    name IS model
+    model IS model
+    boundingObject USE TOKEN_GEOMETRY
+    physics Physics {
+      density -1
+      mass IS mass
+    }
+  }
+}

simulator/protos/props/Can.proto

@@ -0,0 +1,36 @@
+#VRML_SIM R2023b utf8
+
+PROTO Can [
+  field SFVec3f translation 0 0 0.001
+  field SFRotation rotation 1 0 0 0
+  field SFString name ""
+] {
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Shape {
+        appearance DEF TOKEN_APPEARANCE PBRAppearance {
+          baseColor 1 1 1
+          roughness 1
+          metalness 1
+        }
+        geometry DEF TOKEN_GEOMETRY Cylinder {
+          height 0.1
+          radius 0.034
+          subdivision 12
+        }
+      }
+    ]
+    name IS name
+    boundingObject USE TOKEN_GEOMETRY
+    physics Physics {
+      density -1
+      mass 0.300
+      damping Damping {
+        linear 0.4
+        angular 0.4
+      }
+    }
+  }
+}

simulator/protos/props/Marker.proto

@@ -0,0 +1,99 @@
+#VRML_SIM R2022b utf8
+# template language: javascript
+# tags: nonDeterministic
+
+PROTO Marker [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 1 0 0
+  field SFVec2f {0.08 0.08, 0.1 0.1, 0.15 0.15, 0.2 0.2} size 0.08 0.08
+  field SFString name ""
+  field SFString model ""
+  field MFString texture_url []
+  field SFBool add_recognition FALSE
+  field SFBool upright FALSE
+]
+{
+  %< if (fields.upright.value) { >%
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+  %< } >%
+  Solid {
+    %<
+    import * as wbrandom from 'wbrandom.js';
+    wbrandom.seed(context.id);
+    const uid = wbrandom.integer();
+    >%
+    %< if (fields.upright.value) { >%
+    rotation 1 0 0 1.5708
+    %< } else { >%
+    translation IS translation
+    rotation IS rotation
+    %< } >%
+    children [
+      %<
+      if (fields.add_recognition.value) {
+        for (let corner of [['TL', 1, 1], ['TR', -1, 1], ['BR', -1, -1], ['BL', 1, -1]]) {
+        let corner_name = corner[0];
+        let horiz_sign = corner[1];
+        let vert_sign = corner[2];
+      >%
+      Solid {
+        translation %<= horiz_sign * fields.size.value.x / 2 >% %<= vert_sign * fields.size.value.y / 2 >% 0.001
+        children [
+          Shape {
+            appearance PBRAppearance {
+              transparency 1
+              metalness 0
+            }
+            geometry Plane {
+              # Make the detection corners one marker pixel in size
+              size %<= fields.size.value.x / 8 >% %<= fields.size.value.y / 8 >%
+            }
+            castShadows FALSE
+          }
+        ]
+        model %<= "\"" + uid + "_" + fields.model.value + "_" + corner_name + "\"" >%
+        name %<= "\"" + corner_name + "\"" >%
+        locked TRUE
+        recognitionColors [
+          0 0 1
+        ]
+      }
+      %< }} >%
+      Shape {
+        appearance PBRAppearance {
+          baseColorMap ImageTexture {
+            url IS texture_url
+            repeatS FALSE
+            repeatT FALSE
+          }
+          roughness 1
+          metalness 0
+        }
+        geometry Plane {
+          # The size of the marker including the white border
+          size %<= 1.25 * fields.size.value.x >% %<= 1.25 * fields.size.value.y >%
+        }
+        castShadows FALSE
+      }
+    ]
+    %< if (fields.name.value !== "") { >%
+    name IS name
+    %< } else { >%
+    name IS model
+    %< } >%
+    model %<= "\"" + uid + "_" + fields.model.value + "_base\"" >%
+    locked TRUE
+    %< if (fields.add_recognition.value) { >%
+    recognitionColors [
+      1 1 1
+    ]
+    %< } >%
+  }
+  %< if (fields.upright.value) { >%
+    ]
+  }
+  %< } >%
+}

simulator/protos/robot/BumpSensor.proto

@@ -0,0 +1,28 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO BumpSensor [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+] {
+  TouchSensor {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor 0.5 0 0
+          roughness 0.7
+        }
+        geometry DEF BUMPER Box {
+          size 0.01 0.05 0.01
+        }
+      }
+    ]
+    boundingObject Box {
+      size 0.03 0.05 0.01
+    }
+  }
+}

simulator/protos/robot/Caster.proto

@@ -0,0 +1,52 @@
+#VRML_SIM R2023b utf8
+
+PROTO Caster [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+] {
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Solid {
+        name IS name
+        children [
+          DEF CASTER_BALL Shape {
+            appearance PBRAppearance {
+              baseColor 0 0.0051864 0
+              roughness 0
+              metalness 0
+            }
+            geometry Sphere {
+              radius 0.005
+            }
+            castShadows FALSE
+          }
+        ]
+        boundingObject USE CASTER_BALL
+        physics Physics {
+          density 8000  # steel
+        }
+      }
+      Pose {
+        translation 0 0 0.01075
+        children [
+          DEF CASTER_TOP_CYLINDER Shape {
+            appearance PBRAppearance {
+              baseColor 0 0.0051864 0
+              roughness 0.5
+              metalness 0
+            }
+            geometry Cylinder {
+              height 0.0215
+              radius 0.01
+              subdivision 6
+              top FALSE
+            }
+          }
+        ]
+      }
+    ]
+  }
+}

simulator/protos/robot/Flag.proto

@@ -0,0 +1,51 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO Flag [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFColor flagColour 0.1 0.1 0.7
+] {
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Shape {  # pole
+        appearance PBRAppearance {
+          baseColor 0.0705882 0.0705882 0.0705882
+          roughness 0.4
+          metalness 0
+        }
+        geometry Cylinder {
+          height 0.2
+          radius 0.0075
+          subdivision 8
+          bottom FALSE
+        }
+      }
+      Solid {
+        translation -0.08 0 0.04925
+        children [
+          Shape {
+            appearance PBRAppearance {
+              metalness 0
+              baseColor IS flagColour
+            }
+            geometry Box {
+              size 0.15 0.005 0.1
+            }
+          }
+        ]
+        name %<= "\"" + fields.name.value + "flag\"" >%
+      }
+    ]
+    boundingObject Box {
+      size 0.015 0.015 0.2
+    }
+    physics Physics {
+      density 1000
+    }
+  }
+}

simulator/protos/robot/MotorAssembly.proto

@@ -0,0 +1,85 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO MotorAssembly [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFFloat maxVelocity 25
+  field SFBool reversed FALSE
+] {
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Solid {
+        translation 0 0.04 0
+        rotation -1 0 0 1.5708
+        name %<= "\"" + fields.name.value + " housing\"" >%
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0.36 0.36 0.36
+              roughness 0.3
+              metalness 0
+            }
+            geometry Cylinder {
+              height 0.07
+              radius 0.015
+              subdivision 12
+            }
+          }
+        ]
+        boundingObject Box {
+          size 0.03 0.03 0.07
+        }
+        physics Physics {
+          density 8000  # steel
+        }
+      }
+      HingeJoint {
+        jointParameters HingeJointParameters {
+          position 0
+          %< if (fields.reversed.value) { >%
+          axis 0 -1 0
+          %< } else { >%
+          axis 0 1 0
+          %< } >%
+        }
+        device [
+          RotationalMotor {
+            name IS name
+            maxVelocity IS maxVelocity
+            sound ""
+          }
+          PositionSensor {
+            name %<= "\"" + fields.name.value + " sensor\"" >%
+          }
+        ]
+        endPoint Solid {
+          translation 0 0 0
+          rotation -1 0 0 1.5708
+          children [
+            DEF WHEEL_GEO Shape {
+              appearance PBRAppearance {
+                baseColor 0 0.0051864 0
+                roughness 0.3
+                metalness 0
+              }
+              geometry Cylinder {
+                height 0.021
+                radius 0.05
+                subdivision 24
+              }
+            }
+          ]
+          name IS name
+          boundingObject USE WHEEL_GEO
+          physics Physics {
+            density 2000
+          }
+        }
+      }
+    ]
+  }
+}

simulator/protos/robot/RGBLed.proto

@@ -0,0 +1,37 @@
+#VRML_SIM R2023b utf8
+
+PROTO RGBLed [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+] {
+  LED {
+    name IS name
+    translation IS translation
+    rotation IS rotation
+    children [
+      Shape {
+        appearance DEF APP_LED PBRAppearance {
+          baseColor 0.6 0.4 0.4
+          roughness 1
+          emissiveIntensity 100
+        }
+        geometry Box {
+          size 0.02 0.04 0.005
+        }
+        castShadows FALSE
+      }
+    ]
+    color [
+      1 0 0  # RED
+      1 1 0  # YELLOW
+      0 1 0  # GREEN
+      0 1 1  # CYAN
+      0 0 1  # BLUE
+      1 0 1  # MAGENTA
+      1 1 1  # WHITE
+    ]
+  }
+}
+
+

simulator/protos/robot/ReflectanceSensor.proto

@@ -0,0 +1,40 @@
+#VRML_SIM R2023b utf8
+
+PROTO ReflectanceSensor [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+] {
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      DistanceSensor {
+        name IS name
+        rotation 0 1 0 1.5708
+        type "infra-red"
+        lookupTable [
+          # 2% standard deviation
+          0 0 0.02
+          0.1 1023 0.02
+        ]
+        children [
+          Pose {
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.1 0.1 1
+                  metalness 0
+                }
+                geometry Box {
+                  size 0.002 0.045 0.015
+                }
+                castShadows FALSE
+              }
+            ]
+          }
+        ]
+      }
+    ]
+  }
+}

simulator/protos/robot/RobotCamera.proto

@@ -0,0 +1,56 @@
+#VRML_SIM R2023b utf8
+
+PROTO RobotCamera [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFInt32 width 800
+  field SFInt32 height 450
+] {
+  Camera {
+    name IS name
+    translation IS translation
+    rotation IS rotation
+    children [
+      Pose {
+        translation 0 0 0
+        rotation 0 1 0 1.5708
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0 0 0
+            }
+            geometry Cylinder {
+              height 0.01
+              radius 0.01
+            }
+            castShadows FALSE
+          }
+        ]
+        translationStep 0.001
+      }
+      Pose {
+        translation -0.02 0 0
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0.4 0.4 0.4
+              metalness 0
+            }
+            geometry Box {
+              size 0.03 0.03 0.03
+            }
+          }
+        ]
+      }
+    ]
+    # In radians, ~45 degrees
+    fieldOfView 0.82
+    width IS width
+    height IS height
+    recognition Recognition {
+      frameThickness 2
+      maxRange 6
+    }
+  }
+}

simulator/protos/robot/UltrasoundModule.proto

@@ -0,0 +1,75 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO UltrasoundModule [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFInt32 range 4
+  field SFBool upright FALSE
+] {
+  DistanceSensor {
+    name IS name
+    translation IS translation
+    rotation IS rotation
+    type "sonar"
+    numberOfRays 10
+    aperture 0.3
+    lookupTable [
+      # 1% standard deviation, with no deviation at the limits
+      0 0 0
+      0.01 10 0.01
+      %<= fields.range.value * 0.99 >% %<= fields.range.value * 1000 * 0.99 >% 0.01
+      %<= fields.range.value >% %<= fields.range.value * 1000 >% 0
+      # Return 0 for out of range values
+      %<= fields.range.value + 0.001 >% 0 0
+    ]
+    children [
+      Pose {
+        %< if (fields.upright.value) { >%
+        rotation 0 1 1 3.14159
+        %< } else { >%
+        rotation 0 0 1 3.14159
+        %< } >%
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0.1 0.1 1
+              metalness 0
+            }
+            geometry Box {
+              size 0.002 0.045 0.02
+            }
+          }
+          Pose {
+            translation -0.007 0.013 0
+            rotation 0 -1 0 1.5708
+            children [
+              DEF SONAR_TRX Shape {
+                appearance PBRAppearance {
+                  baseColor 0.92 0.92 0.92
+                  roughness 0.3
+                }
+                geometry Cylinder {
+                  radius 0.008
+                  height 0.012
+                  subdivision 12
+                  bottom FALSE
+                }
+                castShadows FALSE
+              }
+            ]
+          }
+          Pose {
+            translation -0.007 -0.013 0
+            rotation 0 -1 0 1.5708
+            children [
+              USE SONAR_TRX
+            ]
+          }
+        ]
+      }
+    ]
+  }
+}
+

simulator/protos/robot/VacuumSucker.proto

@@ -0,0 +1,201 @@
+#VRML_SIM R2023b utf8
+# template language: javascript
+
+PROTO VacuumSucker [
+  field SFString name ""
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFFloat max_height 0.35
+  field SFFloat forward_reach 0.12
+] {
+  Pose {
+    translation IS translation
+    rotation IS rotation
+    children [
+      Pose {
+        translation 0 0 %<= fields.max_height.value / 4 >%
+        children [
+          DEF BASE Solid {
+            children [
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.7 0.7 0.7
+                  roughness 1
+                  metalness 0
+                }
+                geometry DEF BASE_GEO Box {
+                  size 0.06 0.06 %<= fields.max_height.value / 2 >%
+                }
+              }
+            ]
+            name "Tower Base"
+            boundingObject USE BASE_GEO
+            physics Physics {
+              density 500  # Hollow Aluminium
+            }
+          }
+          SliderJoint {
+            jointParameters JointParameters {
+              axis 0 0 1
+              minStop 0
+              maxStop IS max_height
+              position 0.01
+            }
+            device [
+              LinearMotor {
+                name %<= "\"" + fields.name.value + " motor::main\"" >%
+                minPosition 0
+                maxPosition IS max_height
+                maxVelocity 2
+                sound ""
+              }
+              PositionSensor {
+                name %<= "\"" + fields.name.value + " position sensor\"" >%
+              }
+            ]
+            endPoint Solid {
+              translation %<= (fields.forward_reach.value + 0.055) - 0.05 >% 0 %<= fields.max_height.value / 4 - 0.05 >%
+              children [
+                Solid {
+                  children [
+                    Pose {
+                      translation %<= 0.025 - (fields.forward_reach.value + 0.055) / 2 >% 0 0.08
+                      children [
+                        Shape {  # Horizontal arm
+                          appearance PBRAppearance {
+                            baseColor 0.7 0.7 0.7
+                            roughness 1
+                            metalness 0
+                          }
+                          geometry Box {
+                            size %<= fields.forward_reach.value + 0.055 >% 0.05 0.02
+                          }
+                        }
+                      ]
+                    }
+                    Pose {
+                      translation %<= 0.05 - (fields.forward_reach.value + 0.055) >% 0 %<= 0.06 - (fields.max_height.value / 4) >%
+                      children [
+                        Shape {  # Column
+                          appearance PBRAppearance {
+                            baseColor 0.7 0.7 0.7
+                            roughness 1
+                            metalness 0
+                          }
+                          geometry DEF COLUMN_GEO Box {
+                            size 0.05 0.05 %<= fields.max_height.value / 2 + 0.02 >%
+                          }
+                        }
+                      ]
+                    }
+                    Pose {
+                      translation 0 0 0.045
+                      children [
+                        Shape {
+                          appearance PBRAppearance {
+                            baseColor 0.7 0.7 0.7
+                            roughness 1
+                            metalness 0
+                          }
+                          geometry Box {
+                            size 0.05 0.05 0.05
+                          }
+                        }
+                      ]
+                    }
+                  ]
+                  name "Armature"
+                  boundingObject Pose {
+                    translation %<= 0.05 - (fields.forward_reach.value + 0.055) >% 0 %<= 0.06 - (fields.max_height.value / 4) >%
+                    children [USE COLUMN_GEO]
+                  }
+                  physics Physics {
+                    density 200  # Hollow Aluminium
+                  }
+                }
+                DEF SUCKER Shape {
+                  appearance PBRAppearance {
+                    baseColor 0 0 0
+                    roughness 1
+                    metalness 1
+                  }
+                  geometry DEF hook_geo Box {
+                    size 0.05 0.05 0.04
+                  }
+                  castShadows FALSE
+                }
+                Connector {
+                  # Shift origin to near the lower face of the hook shape
+                  translation 0 0 -0.01
+                  rotation 0 1 0 3.1416
+                  type "active"
+                  distanceTolerance 0.094  # (0.14 / 2) * sqrt(2)
+                  axisTolerance 3.1415
+                  rotationTolerance 0
+                  numberOfRotations 0
+                  tensileStrength 35
+                  shearStrength 20
+                  snap FALSE
+                  autoLock TRUE
+                  name IS name
+                  unilateralUnlock TRUE
+                  unilateralLock TRUE
+                }
+              ]
+              boundingObject Pose {
+                translation 0 0 0.005
+                children [
+                  Box {
+                    size 0.05 0.05 0.03
+                  }
+                ]
+              }
+              physics Physics {
+                density 1500  # rubber
+              }
+            }
+          }
+          # Secondary slider joint for telescoping column
+          SliderJoint {
+            jointParameters JointParameters {
+              axis 0 0 1
+              minStop 0
+              maxStop %<= fields.max_height.value / 2 >%
+              position 0.005
+            }
+            device [
+              LinearMotor {
+                name %<= "\"" + fields.name.value + " motor::extension\"" >%
+                minPosition 0
+                maxPosition %<= fields.max_height.value / 2 >%
+                maxVelocity 1
+                sound ""
+                multiplier 0.5
+              }
+            ]
+            endPoint Solid {
+              translation 0 0 0.005
+              children [
+                Shape {
+                  appearance PBRAppearance {
+                    baseColor 0.7 0.7 0.7
+                    roughness 1
+                    metalness 0
+                  }
+                  geometry DEF COLUMN_GEO Box {
+                    size 0.055 0.055 %<= fields.max_height.value / 2 + 0.01 >%
+                  }
+                }
+              ]
+              name "Extension"
+              boundingObject USE COLUMN_GEO
+              physics Physics {
+                density 200  # Hollow Aluminium
+              }
+            }
+          }
+        ]
+      }
+    ]
+  }
+}

simulator/worlds/.arena.wbproj

@@ -0,0 +1,10 @@
+Webots Project File version R2025a
+perspectives: 000000ff00000000fd00000002000000010000011c0000036dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000036d0000003f00ffffff00000003000007800000009dfc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000006620000036d00000001000000020000000100000008fc00000000
+simulationViewPerspectives: 000000ff000000010000000200000100000001a20100000002010000000100
+sceneTreePerspectives: 000000ff00000001000000030000001f000002f4000000fa0100000002010000000200
+maximizedDockId: -1
+centralWidgetVisible: 1
+orthographicViewHeight: 1
+textFiles: -1
+consoles: Console:All:All
+renderingDevicePerspectives: robot0:camera;1;0.497778;0;0

simulator/worlds/arena.wbt

@@ -0,0 +1,289 @@
+#VRML_SIM R2023b utf8
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "../protos/arena/Arena.proto"
+EXTERNPROTO "../protos/arena/Deck.proto"
+EXTERNPROTO "../protos/arena/TriangleDeck.proto"
+EXTERNPROTO "../protos/props/Can.proto"
+EXTERNPROTO "../protos/props/Marker.proto"
+EXTERNPROTO "../protos/SRObot.proto"
+
+WorldInfo {
+  basicTimeStep 8
+}
+Viewpoint {
+  orientation 0.43231 0.43231 -0.79134 1.80272
+  position 0 9.1 13.8
+}
+DEF AMBIENT Background {
+  skyColor [
+    0.4 0.4 0.4
+  ]
+  luminosity 1.6
+}
+TexturedBackgroundLight {
+}
+
+DEF ROBOT0 SRObot {
+  name "robot0"
+  translation 0.45 1.95 0
+  rotation 0 0 1 3.1415
+  flagColour 0 1 0
+  controllerArgs ["0"]
+  controller "usercode_runner"
+  customData "start"
+}
+
+DEF ROBOT1 SRObot {
+  name "robot1"
+  translation -0.45 -1.95 0
+  rotation 0 0 1 0
+  flagColour 1 0.4 0
+  controllerArgs ["1"]
+  controller "usercode_runner"
+  customData "start"
+}
+
+Robot {
+  name "competition_supervisor"
+  controller "competition_supervisor"
+  supervisor TRUE
+}
+
+Arena {
+  size 5.4 5.4
+  locked TRUE
+  floorTexture ["arena_floor.png"]
+}
+Deck {
+  name "TL deck"
+  size 1.2192 1.2192
+  translation 2.0904 -2.0904 0.085
+  locked TRUE
+}
+Deck {
+  name "BR deck"
+  size 1.2192 1.2192
+  translation -2.0904 2.0904 0.085
+  locked TRUE
+}
+Deck {
+  name "CR deck"
+  size 1.2192 1.2192
+  translation -0.6096 -0.6096 0.085
+  locked TRUE
+}
+Deck {
+  name "CL deck"
+  size 1.2192 1.2192
+  translation 0.6096 0.6096 0.085
+  locked TRUE
+}
+TriangleDeck {
+  name "TR deck"
+  size 1.2192 1.2192
+  translation -2.0904 -2.0904 0.085
+  locked TRUE
+}
+TriangleDeck {
+  name "BL deck"
+  size 1.2192 1.2192
+  translation 2.0904 2.0904 0.085
+  rotation 0 0 1 3.14159
+  locked TRUE
+}
+
+DEF CANS Pose {
+  translation 0 0 0.05
+  children [
+    Can {
+      name "can1"
+      translation -0.46 0.60 0
+    }
+    Can {
+      name "can2"
+      translation -1.48 1.1 0
+    }
+    Can {
+      name "can3"
+      translation -1.5892 0.06 0
+    }
+    Can {
+      name "can4"
+      translation -2.33 -0.06 0
+    }
+    Can {
+      name "can5"
+      translation -1.48 -1.95 0
+    }
+
+    Can {
+      name "can6"
+      translation 0.46 -0.6 0
+    }
+    Can {
+      name "can7"
+      translation 1.48 -1.1 0
+    }
+    Can {
+      name "can8"
+      translation 1.5892 -0.06 0
+    }
+    Can {
+      name "can9"
+      translation 2.33 0.06 0
+    }
+    Can {
+      name "can10"
+      translation 1.48 1.95 0
+    }
+  ]
+}
+
+Marker {
+  name "A0"
+  model "100"
+  size 0.1 0.1
+  translation -1.4798 2.165 0.07
+  rotation 0 0 1 1.5708
+  texture_url ["sim_markers/100.png"]
+  upright TRUE
+}
+Marker {
+  name "A1"
+  model "101"
+  size 0.1 0.1
+  translation -1.4798 2.015 0.07
+  rotation 0 0 1 1.5708
+  texture_url ["sim_markers/101.png"]
+  upright TRUE
+}
+Marker {
+  name "A2"
+  model "102"
+  size 0.1 0.1
+  translation -0.535 0.001 0.07
+  rotation 0 0 1 3.1415
+  texture_url ["sim_markers/102.png"]
+  upright TRUE
+}
+Marker {
+  name "A3"
+  model "103"
+  size 0.1 0.1
+  translation -0.685 0.001 0.07
+  rotation 0 0 1 3.1415
+  texture_url ["sim_markers/103.png"]
+  upright TRUE
+}
+Marker {
+  name "A4"
+  model "104"
+  size 0.1 0.1
+  translation -2.699 0.815 0.07
+  rotation 0 0 1 1.5708
+  texture_url ["sim_markers/104.png"]
+  upright TRUE
+}
+Marker {
+  name "A5"
+  model "105"
+  size 0.1 0.1
+  translation -2.699 0.665 0.07
+  rotation 0 0 1 1.5708
+  texture_url ["sim_markers/105.png"]
+  upright TRUE
+}
+Marker {
+  name "A6"
+  model "106"
+  size 0.1 0.1
+  translation -2.14243 -2.03637 0.07
+  rotation 0 0 1 2.3561
+  texture_url ["sim_markers/106.png"]
+  upright TRUE
+}
+Marker {
+  name "A7"
+  model "107"
+  size 0.1 0.1
+  translation -2.03637 -2.14243 0.07
+  rotation 0 0 1 2.3561
+  texture_url ["sim_markers/107.png"]
+  upright TRUE
+}
+
+Marker {
+  name "B0"
+  model "100"
+  size 0.1 0.1
+  translation 1.4798 -2.165 0.07
+  rotation 0 0 1 -1.5708
+  texture_url ["sim_markers/100.png"]
+  upright TRUE
+}
+Marker {
+  name "B1"
+  model "101"
+  size 0.1 0.1
+  translation 1.4798 -2.015 0.07
+  rotation 0 0 1 -1.5708
+  texture_url ["sim_markers/101.png"]
+  upright TRUE
+}
+Marker {
+  name "B2"
+  model "102"
+  size 0.1 0.1
+  translation 0.535 -0.001 0.07
+  rotation 0 0 1 0
+  texture_url ["sim_markers/102.png"]
+  upright TRUE
+}
+Marker {
+  name "B3"
+  model "103"
+  size 0.1 0.1
+  translation 0.685 -0.001 0.07
+  rotation 0 0 1 0
+  texture_url ["sim_markers/103.png"]
+  upright TRUE
+}
+Marker {
+  name "B4"
+  model "104"
+  size 0.1 0.1
+  translation 2.699 -0.815 0.07
+  rotation 0 0 1 -1.5708
+  texture_url ["sim_markers/104.png"]
+  upright TRUE
+}
+Marker {
+  name "B5"
+  model "105"
+  size 0.1 0.1
+  translation 2.699 -0.665 0.07
+  rotation 0 0 1 -1.5708
+  texture_url ["sim_markers/105.png"]
+  upright TRUE
+}
+Marker {
+  name "B6"
+  model "106"
+  size 0.1 0.1
+  translation 2.14243 2.03637 0.07
+  rotation 0 0 1 -0.7853
+  texture_url ["sim_markers/106.png"]
+  upright TRUE
+}
+Marker {
+  name "B7"
+  model "107"
+  size 0.1 0.1
+  translation 2.03637 2.14243 0.07
+  rotation 0 0 1 -0.7853
+  texture_url ["sim_markers/107.png"]
+  upright TRUE
+}
+
+# 5400/2 - 1219.2/2 ± 150/2/sqrt(2)
+# 2.14343, 2.03737