utils.sim.sdl2_controller_bridge

  1"""
  2SDL2 controller bridge for macOS WPILib simulation.
  3
  4Problem
  5-------
  6The WPILib sim GUI uses GLFW for joystick input.  On macOS, Apple's Game
  7Controller framework claims Xbox and PlayStation controllers at the OS level,
  8preventing GLFW from reading their axes or buttons (it reports 0 for both).
  9This makes physical controllers unusable in the simulator on Mac.
 10
 11Solution
 12--------
 13SDL2 has native support for macOS's Game Controller framework and can read
 14these controllers correctly.  This module spawns a daemon thread that:
 15
 161. Initializes SDL2 and discovers connected game controllers.
 172. Maps each controller to a WPILib joystick port (first → port 0, second → port 1).
 183. Polls controller state at ~100 Hz and writes it to the HAL sim via
 19   ``hal.simulation`` functions.
 20
 21The bridge writes directly to the HAL joystick data, bypassing the sim GUI's
 22GLFW-based joystick handling entirely.
 23
 24Important
 25---------
 26**Do NOT assign the controller in the sim GUI's System Joystick panel.**
 27If a controller is assigned there, the sim GUI will overwrite the HAL data
 28with GLFW's empty values each tick, cancelling out the bridge's writes.
 29
 30If no physical controllers are detected, the bridge exits silently and the
 31sim GUI's virtual joystick (keyboard-driven) panel continues to work normally.
 32
 33Requirements
 34------------
 35macOS only.  Install with::
 36
 37    pip install pysdl2 pysdl2-dll
 38
 39These are listed in ``requirements.txt`` with ``sys_platform == "darwin"``
 40markers so they only install on Mac.
 41
 42Not needed on Windows or Linux where GLFW reads controllers correctly.
 43"""
 44
 45import sys
 46import threading
 47import time
 48
 49import hal.simulation
 50import wpilib
 51
 52
 53def start_controller_bridge() -> None:
 54    """Start the SDL2 controller bridge if running on macOS.
 55
 56    Safe to call on any platform — returns immediately on non-Mac systems.
 57    Should be called once during ``PhysicsEngine.__init__``.
 58    """
 59    if sys.platform != "darwin":
 60        return
 61
 62    try:
 63        import sdl2
 64    except ImportError:
 65        wpilib.reportWarning(
 66            "pysdl2 not installed — Xbox controller input in sim requires "
 67            "'pip install pysdl2 pysdl2-dll' on macOS"
 68        )
 69        return
 70
 71    # --- Mapping tables: SDL2 game controller → WPILib XboxController ---
 72
 73    # SDL2 axis constant → WPILib axis index
 74    axis_map = {
 75        sdl2.SDL_CONTROLLER_AXIS_LEFTX: 0,        # kLeftX
 76        sdl2.SDL_CONTROLLER_AXIS_LEFTY: 1,         # kLeftY
 77        sdl2.SDL_CONTROLLER_AXIS_TRIGGERLEFT: 2,   # kLeftTrigger
 78        sdl2.SDL_CONTROLLER_AXIS_TRIGGERRIGHT: 3,   # kRightTrigger
 79        sdl2.SDL_CONTROLLER_AXIS_RIGHTX: 4,        # kRightX
 80        sdl2.SDL_CONTROLLER_AXIS_RIGHTY: 5,        # kRightY
 81    }
 82
 83    # SDL2 button constant → bit index in HAL button bitmask.
 84    # WPILib buttons are 1-based: bit 0 = button 1 (A), bit 1 = button 2 (B), etc.
 85    button_map = {
 86        sdl2.SDL_CONTROLLER_BUTTON_A: 0,
 87        sdl2.SDL_CONTROLLER_BUTTON_B: 1,
 88        sdl2.SDL_CONTROLLER_BUTTON_X: 2,
 89        sdl2.SDL_CONTROLLER_BUTTON_Y: 3,
 90        sdl2.SDL_CONTROLLER_BUTTON_LEFTSHOULDER: 4,
 91        sdl2.SDL_CONTROLLER_BUTTON_RIGHTSHOULDER: 5,
 92        sdl2.SDL_CONTROLLER_BUTTON_BACK: 6,
 93        sdl2.SDL_CONTROLLER_BUTTON_START: 7,
 94        sdl2.SDL_CONTROLLER_BUTTON_LEFTSTICK: 8,
 95        sdl2.SDL_CONTROLLER_BUTTON_RIGHTSTICK: 9,
 96    }
 97
 98    # D-pad buttons for POV conversion (up, down, left, right order)
 99    dpad_buttons = [
100        sdl2.SDL_CONTROLLER_BUTTON_DPAD_UP,
101        sdl2.SDL_CONTROLLER_BUTTON_DPAD_DOWN,
102        sdl2.SDL_CONTROLLER_BUTTON_DPAD_LEFT,
103        sdl2.SDL_CONTROLLER_BUTTON_DPAD_RIGHT,
104    ]
105
106    def _dpad_to_pov(up, down, left, right):
107        """Convert four d-pad booleans to a WPILib POV angle (-1 if released)."""
108        if up and right:
109            return 45
110        if right and down:
111            return 135
112        if down and left:
113            return 225
114        if left and up:
115            return 315
116        if up:
117            return 0
118        if right:
119            return 90
120        if down:
121            return 180
122        if left:
123            return 270
124        return -1
125
126    def _bridge_thread():
127        sdl2.SDL_SetHint(sdl2.SDL_HINT_JOYSTICK_MFI, b"1")
128        sdl2.SDL_Init(sdl2.SDL_INIT_GAMECONTROLLER)
129
130        # Allow time for controller discovery
131        for _ in range(20):
132            sdl2.SDL_PumpEvents()
133            time.sleep(0.05)
134
135        # Open all game-controller-capable devices.
136        # First controller → port 0 (driver), second → port 1 (operator).
137        controllers = []  # list of (sdl2_gc_handle, wpilib_port)
138        for idx in range(sdl2.SDL_NumJoysticks()):
139            if not sdl2.SDL_IsGameController(idx):
140                continue
141            gc = sdl2.SDL_GameControllerOpen(idx)
142            if not gc:
143                continue
144            port = len(controllers)
145            gc_name = sdl2.SDL_GameControllerName(gc)
146            if gc_name and isinstance(gc_name, bytes):
147                gc_name = gc_name.decode()
148            wpilib.reportWarning(
149                f"SDL2 bridge: '{gc_name}' → HAL joystick port {port}. "
150                f"Do NOT assign this controller in the sim GUI System Joystick panel."
151            )
152            hal.simulation.setJoystickAxisCount(port, 6)
153            hal.simulation.setJoystickButtonCount(port, 10)
154            hal.simulation.setJoystickPOVCount(port, 1)
155            hal.simulation.setJoystickIsXbox(port, True)
156            hal.simulation.setJoystickName(port, gc_name or "SDL2 Controller")
157            controllers.append((gc, port))
158
159        if not controllers:
160            # No physical controllers found — exit silently so the sim GUI's
161            # virtual joystick panel continues to work.
162            sdl2.SDL_Quit()
163            return
164
165        # Poll loop at ~100 Hz
166        while True:
167            sdl2.SDL_PumpEvents()
168
169            for gc, port in controllers:
170                # Axes (SDL2: -32768..32767 → WPILib: -1.0..1.0)
171                for sdl_axis, wpi_axis in axis_map.items():
172                    raw = sdl2.SDL_GameControllerGetAxis(gc, sdl_axis)
173                    hal.simulation.setJoystickAxis(port, wpi_axis, raw / 32767.0)
174
175                # Buttons → packed bitmask
176                buttons = 0
177                for sdl_btn, bit_index in button_map.items():
178                    if sdl2.SDL_GameControllerGetButton(gc, sdl_btn):
179                        buttons |= (1 << bit_index)
180                hal.simulation.setJoystickButtonsValue(port, buttons)
181
182                # D-pad → POV angle
183                dpad = [
184                    bool(sdl2.SDL_GameControllerGetButton(gc, b))
185                    for b in dpad_buttons
186                ]
187                hal.simulation.setJoystickPOV(port, 0, _dpad_to_pov(*dpad))
188
189            hal.simulation.notifyDriverStationNewData()
190            time.sleep(0.01)
191
192    t = threading.Thread(
193        target=_bridge_thread, daemon=True, name="sdl2-controller-bridge"
194    )
195    t.start()