diff --git a/BuildInstructions.md b/BuildInstructions.md index 5105335..c4e7b2c 100644 --- a/BuildInstructions.md +++ b/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 diff --git a/README.md b/README.md index 1a0dcf0..00cd807 100644 --- a/README.md +++ b/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. diff --git a/waybionic_teleop/setup.py b/waybionic_teleop/setup.py index 76495e0..ffbcfcf 100644 --- a/waybionic_teleop/setup.py +++ b/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', diff --git a/waybionic_teleop/test/test_sdl_bridge.py b/waybionic_teleop/test/test_sdl_bridge.py new file mode 100644 index 0000000..ec521b8 --- /dev/null +++ b/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'] diff --git a/waybionic_teleop/waybionic_teleop/sdl_bridge.py b/waybionic_teleop/waybionic_teleop/sdl_bridge.py new file mode 100644 index 0000000..7db4502 --- /dev/null +++ b/waybionic_teleop/waybionic_teleop/sdl_bridge.py @@ -0,0 +1,206 @@ +""" +Send a PS5 DualSense controller from Windows to the ROS teleop UDP receiver. + +SDL maps USB and Bluetooth controllers to one layout. It maps DualSense +Cross to A, Circle to B, and Triangle to Y, as teleop expects. +Pygame is needed only on the Windows host running this bridge. +""" + +import argparse +import os +import socket +import time + +from waybionic_teleop.gamepad import AXES, BUTTON, BUTTONS, pack +from waybionic_teleop.xinput_bridge import read_rumble, RUMBLE_S + + +BUTTON_NAMES = { + 'a': 'A', 'b': 'B', 'x': 'X', 'y': 'Y', + 'back': 'BACK', 'guide': 'GUIDE', 'start': 'START', + 'left_stick': 'LEFTSTICK', 'right_stick': 'RIGHTSTICK', + 'left_bumper': 'LEFTSHOULDER', 'right_bumper': 'RIGHTSHOULDER', + 'dpad_up': 'DPAD_UP', 'dpad_down': 'DPAD_DOWN', + 'dpad_left': 'DPAD_LEFT', 'dpad_right': 'DPAD_RIGHT', +} +AXIS_NAMES = ('LEFTX', 'LEFTY', 'RIGHTX', 'RIGHTY', + 'TRIGGERLEFT', 'TRIGGERRIGHT') +NEUTRAL = ([0.0] * len(AXES), [0] * len(BUTTONS)) + + +def to_joy(controller, sdl): + """Translate SDL's DualSense layout to the existing /joy packet layout.""" + raw = [controller.get_axis(getattr(sdl, 'CONTROLLER_AXIS_' + name)) + for name in AXIS_NAMES] + # SDL's stick Y axes grow downwards. The ROS layout is positive left/up. + axes = [-raw[0] / 32768.0, -raw[1] / 32768.0, + -raw[2] / 32768.0, -raw[3] / 32768.0, + -max(0, raw[4]) / 32768.0, -max(0, raw[5]) / 32768.0] + axes = [max(-1.0, min(1.0, value)) for value in axes] + buttons = [0] * len(BUTTONS) + for name, suffix in BUTTON_NAMES.items(): + buttons[BUTTON[name]] = int(bool(controller.get_button( + getattr(sdl, 'CONTROLLER_BUTTON_' + suffix)))) + return axes, buttons + + +def neutral(axes, buttons): + """Require the operator to release the controls after a new connection.""" + return (all(abs(value) <= 0.1 for value in axes[:4]) + and all(abs(value) <= 0.05 for value in axes[4:]) + and not any(buttons)) + + +class SDLBridge: + """Poll one SDL controller and enforce a neutral reset on reconnection.""" + + def __init__(self, pygame, sdl, index=None): + """Keep the SDL API and optional controller index for polling.""" + self.pygame = pygame + self.sdl = sdl + self.index = index + self.controller = None + self.next_scan = 0.0 + self.ready = False + + def close(self): + """Release the current SDL controller, if one is open.""" + if self.controller is not None: + self.controller.quit() + self.controller = None + self.ready = False + + def rumble(self, intensity): + """Rumble the open controller like the Xbox bridge; zero stops it.""" + if self.controller is None: + return + if intensity > 0: + self.controller.rumble(intensity, intensity, + round(RUMBLE_S * 1000)) + else: + self.controller.stop_rumble() + + def poll(self, now): + """Return axes, buttons, connection state, and a status message.""" + self.pygame.event.pump() + if self.controller is not None and not self.controller.attached(): + self.close() + self.next_scan = now + 1.0 + return *NEUTRAL, False, 'Controller disconnected; teleop disabled' + + message = None + if self.controller is None and now >= self.next_scan: + self.next_scan = now + 1.0 + indices = ([self.index] if self.index is not None + else range(self.sdl.get_count())) + for index in indices: + if (index < self.sdl.get_count() + and self.sdl.is_controller(index)): + try: + self.controller = self.sdl.Controller(index) + except (OSError, RuntimeError): + continue + name = self.sdl.name_forindex(index) + message = f'Controller connected: {name}' + break + if self.controller is None: + message = ('No controller detected; connect a PS5 DualSense ' + 'controller') + if self.controller is None: + return *NEUTRAL, False, message + + try: + axes, buttons = to_joy(self.controller, self.pygame) + except (OSError, RuntimeError): + self.close() + self.next_scan = now + 1.0 + return *NEUTRAL, False, 'Controller disconnected; teleop disabled' + if not self.ready: + if neutral(axes, buttons): + self.ready = True + message = 'Controller ready; press Options to enable teleop' + else: + message = 'Release all controls before enabling teleop' + return *NEUTRAL, False, message + return axes, buttons, True, message + + +def main(): + """Run the Windows SDL controller bridge until Ctrl+C.""" + parser = argparse.ArgumentParser( + description='Send a PS5 controller to the arm.') + parser.add_argument('--host', default='127.0.0.1', + help='ground station address') + parser.add_argument('--port', type=int, default=47300, + help='ground station joy_udp_port') + parser.add_argument('--rate', type=float, default=120.0, + help='packets per second') + parser.add_argument('--index', type=int, + help='SDL controller index (default: first)') + args = parser.parse_args() + if (args.rate <= 0 or not 1 <= args.port <= 65535 + or (args.index is not None and args.index < 0)): + parser.error( + 'rate must be positive, port 1..65535, and index nonnegative') + + # SDL must keep updating input while the operator is looking at RViz. + os.environ.setdefault('SDL_JOYSTICK_ALLOW_BACKGROUND_EVENTS', '1') + try: + import pygame + from pygame._sdl2 import controller as sdl + except ImportError as error: + raise SystemExit( + 'Install Pygame on Windows: python -m pip install pygame==2.6.1' + ) from error + + pygame.display.init() + pygame.display.set_mode((1, 1), pygame.HIDDEN) + sdl.init() + bridge = SDLBridge(pygame, sdl, args.index) + # Replies come from the address packets go to; nothing else may rumble + # the controller. + receiver = (socket.gethostbyname(args.host), args.port) + sender = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sender.setblocking(False) + sequence, period, last_message, shown = 0, 1.0 / args.rate, None, '' + print(f'Sending controller state to {args.host}:{args.port} ' + '(Ctrl+C to stop)') + try: + while True: + started = time.monotonic() + axes, buttons, connected, message = bridge.poll(started) + if message and message != last_message: + print('\n' + message) + last_message = message + sequence += 1 + try: + sender.sendto(pack(sequence, axes, buttons, connected), + receiver) + except BlockingIOError: + pass + # The ground station answers when a move stops at a limit. + intensity = read_rumble(sender, receiver) + if intensity is not None: + bridge.rumble(intensity) + if connected and sequence % 30 == 0: + held = ' '.join(name for name, pressed in zip(BUTTONS, buttons) + if pressed) + line = ('axes ' + ' '.join(f'{value:+.2f}' for value in axes) + + f' {held:<40}') + if line != shown: + print('\r' + line, end='', flush=True) + shown = line + time.sleep(max(0.0, period - (time.monotonic() - started))) + except KeyboardInterrupt: + pass + finally: + # Never leave the controller vibrating after the bridge stops. + bridge.rumble(0.0) + bridge.close() + sender.close() + sdl.quit() + pygame.display.quit() + + +if __name__ == '__main__': + main()