From 14bb8b0b1f7bc39815b18efbe14847320305221f Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 14 Sep 2026 20:26:14 -0400 Subject: [PATCH] joystick: keep manager from replacing manual input publisher --- openpilot/tools/joystick/README.md | 2 + openpilot/tools/joystick/joystick_control.py | 35 +++-- .../joystick/tests/test_joystick_publisher.py | 135 ++++++++++++++++++ 3 files changed, 164 insertions(+), 8 deletions(-) create mode 100644 openpilot/tools/joystick/tests/test_joystick_publisher.py diff --git a/openpilot/tools/joystick/README.md b/openpilot/tools/joystick/README.md index a58c3ac024..8e16aec94d 100644 --- a/openpilot/tools/joystick/README.md +++ b/openpilot/tools/joystick/README.md @@ -19,6 +19,8 @@ openpilot/tools/joystick/joystick_control.py --keyboard The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments. +Keep this terminal running when you start the car. The automatic onroad gamepad process waits while this local input tool is running; a second manual instance exits without replacing it. Stop the tool with `Ctrl+C` before starting another instance. Starting the tool enables `JoystickDebugMode` automatically. To disable the mode, go offroad, stop the tool, and turn off **Joystick Debug Mode** in Settings. + ### Ford C0 / C1 independently Use the existing **Settings → Developer → Joystick Debug Mode** toggle, while offroad. On a CAN FD Ford, start the existing keyboard tool with an explicit channel: diff --git a/openpilot/tools/joystick/joystick_control.py b/openpilot/tools/joystick/joystick_control.py index 7ce3914329..f3290026be 100755 --- a/openpilot/tools/joystick/joystick_control.py +++ b/openpilot/tools/joystick/joystick_control.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import argparse +import fcntl import threading import numpy as np from inputs import UnpluggedError, get_gamepad @@ -9,6 +10,7 @@ from openpilot.cereal import messaging from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.common.hardware import HARDWARE +from openpilot.common.hardware.hw import Paths from openpilot.tools.lib.kbhit import KBHit EXPO = 0.4 @@ -97,12 +99,13 @@ class Joystick: return True -def send_thread(joystick): +def send_thread(joystick, stop_event=None): + stop_event = stop_event or threading.Event() pm = messaging.PubMaster(['testJoystick']) rk = Ratekeeper(100, print_delay_threshold=None) - while True: + while not stop_event.is_set(): if rk.frame % 20 == 0: print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) if joystick.ford_channel != 'standard': @@ -118,15 +121,31 @@ def send_thread(joystick): rk.keep_time() -def joystick_control_thread(joystick): - Params().put_bool('JoystickDebugMode', True, block=True) - threading.Thread(target=send_thread, args=(joystick,), daemon=True).start() - while True: - joystick.update() +def joystick_control_thread(joystick, wait_for_lock=False): + # The manager starts this module again onroad. Keep that gamepad publisher + # from replacing an offroad-started keyboard publisher on the same device. + lock_path = os.path.join(Paths.shm_path(), 'joystick_control' + os.environ.get('OPENPILOT_PREFIX', '') + '.lock') + with open(lock_path, 'a') as lock: + try: + fcntl.flock(lock, fcntl.LOCK_EX | (0 if wait_for_lock else fcntl.LOCK_NB)) + except BlockingIOError: + print('Joystick input is already running. Stop the other joystick_control process first.') + return + + Params().put_bool('JoystickDebugMode', True, block=True) + stop_event = threading.Event() + sender = threading.Thread(target=send_thread, args=(joystick, stop_event), daemon=True) + sender.start() + try: + while sender.is_alive(): + joystick.update() + finally: + stop_event.set() + sender.join() # Release the publisher before another input process can acquire the lock. def main(): - joystick_control_thread(Joystick()) + joystick_control_thread(Joystick(), wait_for_lock=True) if __name__ == '__main__': diff --git a/openpilot/tools/joystick/tests/test_joystick_publisher.py b/openpilot/tools/joystick/tests/test_joystick_publisher.py new file mode 100644 index 0000000000..db8bbc1cff --- /dev/null +++ b/openpilot/tools/joystick/tests/test_joystick_publisher.py @@ -0,0 +1,135 @@ +"""Exercise manual input and the manager entrypoint against real IPC sockets.""" +import multiprocessing +import shutil +import threading +import time +import uuid +from contextlib import contextmanager +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from openpilot.cereal import messaging +from openpilot.common.hardware.hw import Paths +from openpilot.tools.joystick import joystick_control as frontend + + +def run_frontend(role, pipe): + class Input: + axes_values = {'gb': 0., 'steer': .05 if role == 'manual' else 0.} + axes_order = ['gb', 'steer'] + ford_channel = 'c0' if role == 'manual' else 'standard' + + def update(self): + command = pipe.recv() + if command == 'stop': + raise SystemExit + self.axes_values['steer'] = command + + frontend.Params = lambda: SimpleNamespace(put_bool=lambda *a, **kw: pipe.send('enabled')) + frontend.Joystick = Input + threading.excepthook = lambda args: pipe.send(type(args.exc_value).__name__) + pipe.send('ready') + if role == 'manager': + frontend.main() + else: + frontend.joystick_control_thread(Input()) + + +@contextmanager +def publisher(role): + context = multiprocessing.get_context('spawn') + parent, child = context.Pipe() + process = context.Process(target=run_frontend, args=(role, child)) + process.start() + child.close() + try: + assert parent.poll(10), 'frontend failed to start' + assert parent.recv() == 'ready' + yield process, parent + finally: + if process.is_alive(): + process.terminate() + process.join(5) + if process.is_alive(): + process.kill() + process.join(5) + parent.close() + process.close() + + +@pytest.fixture +def joystick_socket(monkeypatch): + prefix = 'joystick_test_' + uuid.uuid4().hex + monkeypatch.setenv('OPENPILOT_PREFIX', prefix) + monkeypatch.delenv('ZMQ', raising=False) + directory = Path(Paths.shm_path()) / ('msgq_' + prefix) + directory.mkdir() + messaging.reset_context() + socket = messaging.sub_sock('testJoystick', conflate=True, timeout=1000) + try: + yield socket + finally: + del socket + shutil.rmtree(directory) + (Path(Paths.shm_path()) / ('joystick_control' + prefix + '.lock')).unlink(missing_ok=True) + messaging.reset_context() + + +def assert_stream(socket, channel, steer, duration=.3): + deadline = time.monotonic() + duration + while time.monotonic() < deadline: + message = messaging.recv_one(socket) + assert message is not None, 'joystick publisher stopped transmitting' + assert message.testJoystick.fordChannel == channel, 'another input source stole testJoystick' + assert list(message.testJoystick.axes) == pytest.approx([0., steer]) + + +def test_onroad_manager_does_not_replace_keyboard_publisher(joystick_socket): + with publisher('manual') as (_, manual): + assert manual.poll(5) and manual.recv() == 'enabled' + assert_stream(joystick_socket, 'c0', .05) + with publisher('manager'): + assert_stream(joystick_socket, 'c0', .05) + assert not manual.poll(), 'keyboard sender raised an exception' + + +@pytest.mark.parametrize('abrupt', [False, True]) +def test_manager_can_publish_after_keyboard_exits(joystick_socket, abrupt): + with publisher('manual') as (manual_process, manual): + assert manual.poll(5) and manual.recv() == 'enabled' + assert_stream(joystick_socket, 'c0', .05) + with publisher('manager') as (_, manager): + assert_stream(joystick_socket, 'c0', .05) + assert not manager.poll(), 'manager enabled a competing publisher' + if abrupt: + manual_process.kill() + else: + manual.send('stop') + manual_process.join(5) + assert not manual_process.is_alive() + if not abrupt: + assert manual_process.exitcode == 0 + assert manager.poll(5) and manager.recv() == 'enabled' + messaging.drain_sock(joystick_socket) + assert_stream(joystick_socket, 'standard', 0.) + + +def test_second_manual_session_exits_without_replacing_first(joystick_socket): + with publisher('manual') as (_, manual): + assert manual.poll(5) and manual.recv() == 'enabled' + assert_stream(joystick_socket, 'c0', .05) + with publisher('manual') as (duplicate_process, duplicate): + duplicate_process.join(5) + assert duplicate_process.exitcode == 0, 'duplicate frontend did not exit' + with pytest.raises(EOFError): + duplicate.recv() # A rejected instance must not enable the mode or start a sender. + assert_stream(joystick_socket, 'c0', .05) + assert not manual.poll(), 'original keyboard sender raised an exception' + + +def test_automatic_gamepad_publishes_normally_without_manual_session(joystick_socket): + with publisher('manager') as (_, manager): + assert manager.poll(5) and manager.recv() == 'enabled' + assert_stream(joystick_socket, 'standard', 0.)