Add PS5 DualSense controller support on Windows

BuildInstructions.md

@@ -292,9 +292,9 @@ Qt may report a default `XDG_RUNTIME_DIR`, and RViz may report that stereo is no
 supported; neither prevented this demo from starting. On Ctrl+C, the joint GUI
 can report exit code `-2`, indicating the requested SIGINT interruption.
 
-## Xbox Controller (Simulated Arm)
+## Xbox and PS5 Controllers (Simulated Arm)
 
-`teleop:=true` drives the arm with an Xbox controller through simulated CAN drives;
+`teleop:=true` drives the arm with a controller through simulated CAN drives;
 nothing is sent to hardware unless you choose a drive bus, as described in
 [Real MKS drives over CAN](#real-mks-drives-over-can). The controller mapping is in
 `waybionic_teleop/config/xbox_teleop.yaml`, and the placeholder joint-to-drive map
@@ -319,6 +319,26 @@ python -m waybionic_teleop.xinput_bridge
 The bridge sends the controller state to the container on `127.0.0.1:47300/udp`.
 Press **Ctrl+C** in each window to stop.
 
+**PS5 DualSense on Windows:** pair the controller in Windows Bluetooth settings or
+connect it by USB, then use the same `wslg-teleop` command above. In the second
+PowerShell window, use Python 3.13 or older, install Pygame once, and start the SDL
+bridge:
+
+```powershell
+python -m pip install pygame==2.6.1
+cd waybionic_teleop
+python -m waybionic_teleop.sdl_bridge
+```
+
+Run only one Windows bridge at a time; the
+[self-running demo](#self-running-demo) opens the Xbox bridge, so close its window
+before starting the SDL bridge. The SDL bridge uses the first recognized
+controller by default; use `--index 1` (or another SDL index) if multiple controllers
+are connected. It waits for all controls to be released when the controller first
+connects or reconnects. Windows must show the controller as connected, and the bridge
+must print `Controller ready` before Options can enable teleoperation. If Pygame
+cannot see the controller over Bluetooth, try USB and report which connection worked.
+
 **Linux (native ROS):** with the controller plugged in, run
 `ros2 launch waybionic_bringup ground_station.launch.py teleop:=true`.
 
@@ -351,6 +371,15 @@ Bluetooth controllers are not listed in `/dev/input/by-id`; connect the controll
 | D-pad up/down | Speed: 10, 25, 50 or 100% of 60 deg/s, or of 50 mm/s and 30 deg/s of tilt in the Cartesian and incision groups |
 | A (hold) | Return to the zero pose (not in the incision group) |
 
+On PS5 DualSense, **Options** replaces Start/Menu, **Circle** replaces B,
+**Triangle** replaces Y, **Cross** replaces A, and the left/right triggers are
+L2/R2. The sticks, D-pad, and shoulder buttons keep the same positions. After
+connecting, test each row above with those names, and check that the controller
+rumbles when a move stops at a limit. Turn the controller off while
+teleoperation is enabled: the arm should hold its pose and show disabled. After
+reconnecting, teleoperation must remain disabled until you release the controls
+and press Options again.
+
 Start is refused until the sticks are centred and the triggers and motion buttons released;
 after Y changes groups with motion held, the new group waits until the controls are neutral.
 Stick and trigger movement inside the 15% deadzone is ignored, so a trigger that does not
@@ -367,7 +396,7 @@ RViz label also turns red and names the cause, for example `STOPPED-joint_3` or
 `OUT-OF-REACH`. If the drives stop the arm while teleop is still enabled, for example after
 rejecting a target or losing encoder feedback, the label shows `DRIVES-STOPPED` and the
 drives' reason, and the controller rumbles once; press B, then Start, to continue. Rumble
-works with a controller plugged into the ground station and through the Windows bridge.
+works with a controller plugged into the ground station and through either Windows bridge.
 
 When the bridge reports that the controller disconnected, every control is released once,
 so the arm slows to a stop. After 0.5 s without controller input, teleop disables and holds
@@ -442,8 +471,8 @@ updating the controller. The bridge keeps sending the last state it read, so the
 controller cannot stop the arm, not even with B. Press **Ctrl+C** in the bridge
 window; with no controller input, teleop disables and holds the arm after the
 0.5 s input timeout. Turn the controller off and on (or unplug and replug it),
-start the bridge again and press Start. Real drives need a stop that does not
-depend on the controller.
+start the bridge again and press Start/Options. Real drives need a stop that does
+not depend on the controller.
 
 ### Self-Running Demo
 

README.md

@@ -11,7 +11,7 @@ Currently, this repository contains the clean foundation and the SolidWorks-expo
 - **`waybionic_bringup`**
   Launch files and RViz configurations to bring up the robot state and visualization.
 - **`waybionic_teleop`**
-  Xbox controller teleoperation of the arm through placeholder MKS SERVO CAN drives, simulated until the real drives are connected.
+  Xbox and PS5 DualSense controller teleoperation of the arm through placeholder MKS SERVO CAN drives, simulated until the real drives are connected.
 - **`waybionic_rviz_plugins`**
   Engineer diagnostics panel, mock/live diagnostics sources, and a temporary diagnostics publisher.
 

waybionic_teleop/setup.py

@@ -19,7 +19,7 @@
     maintainer='Yassin Soliman',
     maintainer_email='solimanyassin@gmail.com',
     description=(
-        'Xbox controller teleoperation and MKS CAN joint drives, simulated or real, for the '
+        'Controller teleoperation and MKS CAN joint drives, simulated or real, for the '
         'WayBionic arm.'
     ),
     license='Apache-2.0',

waybionic_teleop/test/test_sdl_bridge.py

@@ -0,0 +1,227 @@
+"""DualSense input must match the teleop layout and fail safe on reconnect."""
+
+import socket
+import sys
+import time
+from types import SimpleNamespace
+
+import pytest
+
+from waybionic_teleop import gamepad, sdl_bridge
+
+
+class FakeController:
+    """An SDL controller whose input can change during a test."""
+
+    def __init__(self):
+        """Start with neutral controls and a connected device."""
+        self.axes = {}
+        self.buttons = {}
+        self.connected = True
+        self.rumbles = []
+
+    def get_axis(self, index):
+        """Return a simulated SDL axis."""
+        return self.axes.get(index, 0)
+
+    def get_button(self, index):
+        """Return a simulated SDL button."""
+        return self.buttons.get(index, False)
+
+    def attached(self):
+        """Report whether the simulated device is connected."""
+        return self.connected
+
+    def rumble(self, low, high, duration):
+        """Record a simulated SDL rumble request."""
+        self.rumbles.append((low, high, duration))
+        return True
+
+    def stop_rumble(self):
+        """Record a simulated SDL rumble stop."""
+        self.rumbles.append('stop')
+
+    def quit(self):  # noqa: A003
+        """Match the SDL controller cleanup API."""
+        pass
+
+
+class FakeSDL:
+    """Expose the controller constants used by the production bridge."""
+
+    def __init__(self):
+        """Assign test constants and start without devices."""
+        self.devices = []
+        for index, name in enumerate(sdl_bridge.AXIS_NAMES):
+            setattr(self, 'CONTROLLER_AXIS_' + name, index)
+        for index, name in enumerate(sdl_bridge.BUTTON_NAMES.values()):
+            setattr(self, 'CONTROLLER_BUTTON_' + name, index)
+
+    def get_count(self):
+        """Return the current SDL device count."""
+        return len(self.devices)
+
+    def is_controller(self, index):
+        """Mark every test device as an SDL controller."""
+        return True
+
+    def Controller(self, index):
+        """Open one simulated SDL controller."""
+        return self.devices[index]
+
+    def name_forindex(self, index):
+        """Name the test controller like a DualSense."""
+        return 'DualSense Wireless Controller'
+
+
+def pressed(buttons):
+    """List normalized names of pressed buttons."""
+    return [name for name, value in zip(gamepad.BUTTONS, buttons) if value]
+
+
+def fake_pygame(sdl):
+    """Expose SDL constants where Pygame 2.6 publishes them."""
+    pygame = SimpleNamespace(event=SimpleNamespace(pump=lambda: None))
+    for name in sdl_bridge.AXIS_NAMES:
+        setattr(pygame, 'CONTROLLER_AXIS_' + name,
+                getattr(sdl, 'CONTROLLER_AXIS_' + name))
+    for name in sdl_bridge.BUTTON_NAMES.values():
+        setattr(pygame, 'CONTROLLER_BUTTON_' + name,
+                getattr(sdl, 'CONTROLLER_BUTTON_' + name))
+    return pygame
+
+
+def test_dualsense_controls_match_existing_teleop_layout():
+    """Keep DualSense axes and buttons compatible with teleop packets."""
+    sdl = FakeSDL()
+    pad = FakeController()
+    pad.axes = {sdl.CONTROLLER_AXIS_LEFTX: -32768,
+                sdl.CONTROLLER_AXIS_LEFTY: -32768,
+                sdl.CONTROLLER_AXIS_RIGHTX: 16384,
+                sdl.CONTROLLER_AXIS_RIGHTY: 32767,
+                sdl.CONTROLLER_AXIS_TRIGGERLEFT: 32768}
+    pad.buttons = {sdl.CONTROLLER_BUTTON_A: True,  # Cross
+                   sdl.CONTROLLER_BUTTON_B: True,  # Circle
+                   sdl.CONTROLLER_BUTTON_Y: True,  # Triangle
+                   sdl.CONTROLLER_BUTTON_START: True,  # Options
+                   sdl.CONTROLLER_BUTTON_DPAD_UP: True}
+    axes, buttons = sdl_bridge.to_joy(pad, sdl)
+    assert axes == pytest.approx([1.0, 1.0, -0.5, -32767 / 32768, -1.0, 0.0])
+    assert pressed(buttons) == ['a', 'b', 'y', 'start', 'dpad_up']
+    _, connected, unpacked_axes, unpacked_buttons = gamepad.unpack(
+        gamepad.pack(1, axes, buttons))
+    assert connected
+    assert unpacked_axes == pytest.approx(axes)
+    assert unpacked_buttons == buttons
+
+
+def test_disconnect_sends_neutral_and_reconnect_requires_release():
+    """Do not expose controls until a new device has been released."""
+    sdl = FakeSDL()
+    first = FakeController()
+    sdl.devices = [first]
+    bridge = sdl_bridge.SDLBridge(fake_pygame(sdl), sdl)
+    assert bridge.poll(0.0)[2] is False  # neutral reset
+    assert bridge.poll(0.01)[2] is True
+    first.buttons[sdl.CONTROLLER_BUTTON_START] = True
+    assert 'start' in pressed(bridge.poll(0.02)[1])
+
+    first.connected = False
+    axes, buttons, connected, _ = bridge.poll(0.03)
+    assert not connected and (axes, buttons) == sdl_bridge.NEUTRAL
+
+    second = FakeController()
+    second.buttons[sdl.CONTROLLER_BUTTON_START] = True
+    sdl.devices = [second]
+    # Held Options cannot enable teleop on reconnect.
+    assert bridge.poll(1.04)[2] is False
+    second.buttons.clear()
+    assert bridge.poll(1.05)[2] is False  # wait for a fresh Options press
+    assert bridge.poll(1.06)[2] is True
+    second.buttons[sdl.CONTROLLER_BUTTON_START] = True
+    assert 'start' in pressed(bridge.poll(1.07)[1])
+
+
+def test_trigger_or_stick_held_blocks_connection():
+    """A held control must block the reset after connection."""
+    sdl = FakeSDL()
+    pad = FakeController()
+    pad.axes[sdl.CONTROLLER_AXIS_TRIGGERLEFT] = 20000
+    sdl.devices = [pad]
+    bridge = sdl_bridge.SDLBridge(fake_pygame(sdl), sdl)
+    assert bridge.poll(0.0)[2] is False
+    pad.axes.clear()
+    pad.axes[sdl.CONTROLLER_AXIS_LEFTX] = 20000
+    assert bridge.poll(0.01)[2] is False
+    pad.axes.clear()
+    assert bridge.poll(0.02)[2] is False
+    assert bridge.poll(0.03)[2] is True
+
+
+def test_missing_controller_reports_a_useful_status():
+    """Tell the operator when the bridge is running without a controller."""
+    sdl = FakeSDL()
+    bridge = sdl_bridge.SDLBridge(fake_pygame(sdl), sdl)
+
+    axes, buttons, connected, message = bridge.poll(0.0)
+
+    assert not connected and (axes, buttons) == sdl_bridge.NEUTRAL
+    assert message == ('No controller detected; connect a PS5 DualSense '
+                       'controller')
+    assert bridge.poll(0.5)[3] is None
+
+
+def test_rumble_reaches_only_an_open_controller():
+    """Rumble for a second per request like the Xbox bridge; zero stops it."""
+    sdl = FakeSDL()
+    bridge = sdl_bridge.SDLBridge(fake_pygame(sdl), sdl)
+    bridge.rumble(1.0)
+    pad = FakeController()
+    sdl.devices = [pad]
+    bridge.poll(0.0)
+    bridge.rumble(0.5)
+    bridge.rumble(0.0)
+    assert pad.rumbles == [(0.5, 0.5, 1000), 'stop']
+
+
+def test_rumble_replies_do_not_interrupt_the_controller_packets(monkeypatch):
+    """Only the ground station's replies rumble; the bridge keeps sending."""
+    sdl = FakeSDL()
+    pad = FakeController()
+    sdl.devices = [pad]
+    sdl.init = sdl.quit = lambda: None
+    pygame = fake_pygame(sdl)
+    pygame.HIDDEN = 0
+    pygame.display = SimpleNamespace(
+        init=lambda: None, set_mode=lambda size, flags: None,
+        quit=lambda: None)
+    monkeypatch.setitem(sys.modules, 'pygame', pygame)
+    monkeypatch.setitem(sys.modules, 'pygame._sdl2',
+                        SimpleNamespace(controller=sdl))
+    monkeypatch.setenv('SDL_JOYSTICK_ALLOW_BACKGROUND_EVENTS', '1')
+    station = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    stranger = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    station.bind(('127.0.0.1', 0))
+    station.settimeout(2.0)
+    packets = []
+
+    def sleep(seconds):
+        data, address = station.recvfrom(64)
+        packets.append(gamepad.unpack(data))
+        if len(packets) == 3:
+            stranger.sendto(gamepad.pack_rumble(1.0), address)
+            station.sendto(gamepad.pack_rumble(0.5), address)
+        elif len(packets) == 6:
+            raise KeyboardInterrupt
+
+    monkeypatch.setattr(sdl_bridge, 'time', SimpleNamespace(
+        monotonic=time.monotonic, sleep=sleep))
+    monkeypatch.setattr(sys, 'argv', [
+        'sdl_bridge', '--port', str(station.getsockname()[1])])
+    try:
+        sdl_bridge.main()
+    finally:
+        station.close()
+        stranger.close()
+    assert [packet[1] for packet in packets] == [False] + [True] * 5
+    assert pad.rumbles == [(0.5, 0.5, 1000), 'stop']