From ae77f693a26527ffbee044f852cc4185718c2433 Mon Sep 17 00:00:00 2001 From: ShaneSmiskol Date: Fri, 11 Jun 2021 14:33:17 -0700 Subject: [PATCH] Fixup joystick (#21129) * some common changes * rename to joystick * add alert and update controlsd to work with joystick * update joystickd * Update Joystick readme * assume we have inputs * only send gb or steer when engaged_toggle is true * Update instructions * fix --ip * Easier to understand at a glance * much better * -a * receive events and send msg in same loop * always import * Update selfdrive/controls/lib/events.py Co-authored-by: Adeeb Shihadeh * Update selfdrive/controls/lib/events.py Co-authored-by: Adeeb Shihadeh * combine logic and clean up * use argparse * outdated, and use normal class * rm * bit of a refactor * refactor part 2 / 3 * much better (3 / 3) * Simplify * bump cereal and update readme * capitalize * Update tools/joystick/joystickd.py Co-authored-by: Adeeb Shihadeh * Update tools/joystick/joystickd.py Co-authored-by: Adeeb Shihadeh * make joystick abstraction class clearer * use interp, clearer without comments * no need to use apply_deadzone * more explicit * define btns and axes once * split function by use_keyboard again, but simpler * we can use handle_button as a reset function * need to flip sign * remove * invert axes map for kb, easier to read the button mapping * apply changes from review * new lateral log for debug mode * bump * add saturated * static alert * joystick_mode * conditionally subscribe * Update selfdrive/controls/controlsd.py Co-authored-by: Adeeb Shihadeh * move params instantiation * Spoof active and enabled * Only allow car to engage * no startup alert if joystick * Update controlsd.py * Should be orange not enabled, green enabled * no more button states * should work * blue * submaster conflates, so only send when we have an update * final change * remove msg * clean up a bit sort of clean up clean up a bit remove msg * this was right * Apply suggestions from code review Co-authored-by: Adeeb Shihadeh * lowercase * suggestions from code review * forgot laptop * bump to latest * fixes Co-authored-by: Adeeb Shihadeh Co-authored-by: vanillagorillaa <31773928+vanillagorillaa@users.noreply.github.com> --- selfdrive/common/params.cc | 1 + selfdrive/controls/controlsd.py | 40 +++++-- selfdrive/controls/lib/events.py | 17 ++- selfdrive/ui/qt/onroad.cc | 2 +- tools/README.md | 2 +- tools/carcontrols/README.md | 26 ----- tools/carcontrols/__init__.py | 0 tools/carcontrols/debug_controls.py | 90 ---------------- tools/carcontrols/joystick_test.py | 122 ---------------------- tools/carcontrols/joystickd.py | 65 ------------ tools/joystick/README.md | 45 ++++++++ tools/joystick/joystickd.py | 86 +++++++++++++++ tools/{carcontrols => joystick}/steer.gif | Bin 13 files changed, 181 insertions(+), 315 deletions(-) delete mode 100644 tools/carcontrols/README.md delete mode 100644 tools/carcontrols/__init__.py delete mode 100755 tools/carcontrols/debug_controls.py delete mode 100755 tools/carcontrols/joystick_test.py delete mode 100755 tools/carcontrols/joystickd.py create mode 100644 tools/joystick/README.md create mode 100755 tools/joystick/joystickd.py rename tools/{carcontrols => joystick}/steer.gif (100%) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index a0c3d3b516..86a81cd929 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -218,6 +218,7 @@ std::unordered_map keys = { {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_NvmeMissing", CLEAR_ON_MANAGER_START}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, + {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, }; } // namespace diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6608ce3c2e..2071b40997 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -57,12 +57,16 @@ class Controls: if TICI: self.camera_packets.append("wideRoadCameraState") + params = Params() + self.joystick_mode = params.get_bool("JoystickDebugMode") + joystick_packet = ['testJoystick'] if self.joystick_mode else [] + self.sm = sm if self.sm is None: ignore = ['driverCameraState', 'managerState'] if SIMULATION else None self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'managerState', 'liveParameters', 'radarState'] + self.camera_packets, + 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) self.can_sock = can_sock @@ -80,7 +84,6 @@ class Controls: self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) # read params - params = Params() self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") self.enable_lte_onroad = params.get_bool("EnableLteOnroad") @@ -156,6 +159,9 @@ class Controls: self.events.add(EventName.carUnrecognized, static=True) elif self.read_only: self.events.add(EventName.dashcamMode, static=True) + elif self.joystick_mode: + self.events.add(EventName.joystickDebug, static=True) + self.startup_event = None # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -444,11 +450,26 @@ class Controls: a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart) v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0 - # Gas/Brake PID loop - actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP) + if not self.joystick_mode: + # Gas/Brake PID loop + actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP) - # Steering PID loop and lateral MPC - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan) + # Steering PID loop and lateral MPC + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan) + else: + lac_log = log.ControlsState.LateralDebugState.new_message() + if self.sm.rcv_frame['testJoystick'] > 0 and self.active: + gb = clip(self.sm['testJoystick'].axes[0], -1, 1) + actuators.gas, actuators.brake = max(gb, 0), max(-gb, 0) + + steer = clip(self.sm['testJoystick'].axes[1], -1, 1) + # max angle is 45 for angle-based cars + actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. + + lac_log.active = True + lac_log.steeringAngleDeg = CS.steeringAngleDeg + lac_log.output = steer + lac_log.saturated = abs(steer) >= 0.9 # Check for difference between desired angle and angle for angle based control angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \ @@ -483,6 +504,9 @@ class Controls: CC.cruiseControl.override = True CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled) + if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: + CC.cruiseControl.cancel = True + # Some override values for Honda # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) @@ -573,7 +597,9 @@ class Controls: controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + if self.joystick_mode: + controlsState.lateralControlState.debugState = lac_log + elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log elif self.CP.lateralTuning.which() == 'pid': controlsState.lateralControlState.pidState = lac_log diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index feb3b667d0..27782b53ac 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -227,17 +227,28 @@ def startup_fuzzy_fingerprint_alert(CP: car.CarParams, sm: messaging.SubMaster, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.) +def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: + axes = sm['testJoystick'].axes + gb, steer = list(axes)[:2] if len(axes) else (0., 0.) + return Alert( + "Joystick Mode", + f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .1) + + EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = { # ********** events with no alerts ********** # ********** events only containing alerts displayed in all states ********** EventName.joystickDebug: { + ET.WARNING: joystick_alert, ET.PERMANENT: Alert( - "DEBUG ALERT", + "Joystick Mode", "", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1), + AlertStatus.normal, AlertSize.small, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 0.1), }, EventName.controlsInitializing: { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 9eabc84b26..ebea11d382 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -116,7 +116,7 @@ void OnroadAlerts::offroadTransition(bool offroad) { void OnroadAlerts::updateAlert(const QString &t1, const QString &t2, float blink_rate, const std::string &type, cereal::ControlsState::AlertSize size, AudibleAlert sound) { - if (alert_type.compare(type) == 0 && text1.compare(t1) == 0) { + if (alert_type.compare(type) == 0 && text1.compare(t1) == 0 && text2.compare(t2) == 0) { return; } diff --git a/tools/README.md b/tools/README.md index e007123269..35c3f5cf8a 100644 --- a/tools/README.md +++ b/tools/README.md @@ -66,7 +66,7 @@ Test openpilots performance in a simulated environment. The [CARLA simulator](ht Review video and log data from routes and stream CAN messages to your device. -[Debug car controls](carcontrols) +[Debug car controls](joystick) ------------- Use a joystick to control your car. diff --git a/tools/carcontrols/README.md b/tools/carcontrols/README.md deleted file mode 100644 index 624cbd4fcf..0000000000 --- a/tools/carcontrols/README.md +++ /dev/null @@ -1,26 +0,0 @@ -Debug car controls -------------- - -**Hardware needed**: [panda](panda.comma.ai), [giraffe](https://comma.ai/shop/products/giraffe/), joystick - -Use the panda's OBD-II port to connect with your car and a usb cable to connect the panda to your pc. -Also, connect a joystick to your pc. - -`joystickd.py` runs a deamon that reads inputs from a joystick and publishes them over zmq. -`boardd` sends the CAN messages from your pc to the panda. -`debug_controls` is a mocked version of `controlsd.py` and uses input from a joystick to send controls to your car. - -Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. - -Usage: -``` -python carcontrols/joystickd.py - -# In another terminal: -BASEDIR=$(pwd) selfdrive/boardd/boardd - -# In another terminal: -python carcontrols/debug_controls.py - -``` -![Imgur](steer.gif) diff --git a/tools/carcontrols/__init__.py b/tools/carcontrols/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/tools/carcontrols/debug_controls.py b/tools/carcontrols/debug_controls.py deleted file mode 100755 index 9264b2d768..0000000000 --- a/tools/carcontrols/debug_controls.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python -from common.numpy_fast import clip -from common.params import Params -from copy import copy -from cereal import car, log -import cereal.messaging as messaging -from selfdrive.car.car_helpers import get_car, get_one_can -from selfdrive.boardd.boardd import can_list_to_can_capnp - -PandaType = log.HealthData.PandaType - - -def steer_thread(): - poller = messaging.Poller() - - logcan = messaging.sub_sock('can') - joystick_sock = messaging.sub_sock('testJoystick', conflate=True, poller=poller) - - carstate = messaging.pub_sock('carState') - carcontrol = messaging.pub_sock('carControl') - sendcan = messaging.pub_sock('sendcan') - - button_1_last = 0 - enabled = False - - # wait for CAN packets - print("Waiting for CAN messages...") - get_one_can(logcan) - - CI, CP = get_car(logcan, sendcan) - Params().put("CarParams", CP.to_bytes()) - - CC = car.CarControl.new_message() - - while True: - - # send - joystick = messaging.recv_one(joystick_sock) - can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True) - CS = CI.update(CC, can_strs) - - # Usually axis run in pairs, up/down for one, and left/right for - # the other. - actuators = car.CarControl.Actuators.new_message() - - if joystick is not None: - axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1 - actuators.steer = axis_3 - actuators.steeringAngleDeg = axis_3 * 43. # deg - axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1 - actuators.gas = max(axis_1, 0.) - actuators.brake = max(-axis_1, 0.) - - pcm_cancel_cmd = joystick.testJoystick.buttons[0] - button_1 = joystick.testJoystick.buttons[1] - if button_1 and not button_1_last: - enabled = not enabled - - button_1_last = button_1 - - #print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake - - hud_alert = 0 - if joystick.testJoystick.buttons[3]: - hud_alert = "steerRequired" - - CC.actuators.gas = actuators.gas - CC.actuators.brake = actuators.brake - CC.actuators.steer = actuators.steer - CC.actuators.steeringAngleDeg = actuators.steeringAngleDeg - CC.hudControl.visualAlert = hud_alert - CC.hudControl.setSpeed = 20 - CC.cruiseControl.cancel = pcm_cancel_cmd - CC.enabled = enabled - can_sends = CI.apply(CC) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - # broadcast carState - cs_send = messaging.new_message('carState') - cs_send.carState = copy(CS) - carstate.send(cs_send.to_bytes()) - - # broadcast carControl - cc_send = messaging.new_message('carControl') - cc_send.carControl = copy(CC) - carcontrol.send(cc_send.to_bytes()) - - -if __name__ == "__main__": - steer_thread() diff --git a/tools/carcontrols/joystick_test.py b/tools/carcontrols/joystick_test.py deleted file mode 100755 index 0d92c70423..0000000000 --- a/tools/carcontrols/joystick_test.py +++ /dev/null @@ -1,122 +0,0 @@ -#!/usr/bin/env python -import pygame # pylint: disable=import-error - -# Define some colors -BLACK = ( 0, 0, 0) -WHITE = ( 255, 255, 255) - -# This is a simple class that will help us print to the screen -# It has nothing to do with the joysticks, just outputting the -# information. -class TextPrint: - def __init__(self): - self.reset() - self.font = pygame.font.Font(None, 20) - - def printf(self, screen, textString): - textBitmap = self.font.render(textString, True, BLACK) - screen.blit(textBitmap, [self.x, self.y]) - self.y += self.line_height - - def reset(self): - self.x = 10 - self.y = 10 - self.line_height = 15 - - def indent(self): - self.x += 10 - - def unindent(self): - self.x -= 10 - - -pygame.init() - -# Set the width and height of the screen [width,height] -size = [500, 700] -screen = pygame.display.set_mode(size) - -pygame.display.set_caption("My Game") - -#Loop until the user clicks the close button. -done = False - -# Used to manage how fast the screen updates -clock = pygame.time.Clock() - -# Initialize the joysticks -pygame.joystick.init() - -# Get ready to print -textPrint = TextPrint() - -# -------- Main Program Loop ----------- -while not done: - # EVENT PROCESSING STEP - for event in pygame.event.get(): # User did something - if event.type == pygame.QUIT: # If user clicked close - done = True # Flag that we are done so we exit this loop - - # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION - if event.type == pygame.JOYBUTTONDOWN: - print("Joystick button pressed.") - if event.type == pygame.JOYBUTTONUP: - print("Joystick button released.") - - # DRAWING STEP - # First, clear the screen to white. Don't put other drawing commands - # above this, or they will be erased with this command. - screen.fill(WHITE) - textPrint.reset() - - # Get count of joysticks - joystick_count = pygame.joystick.get_count() - - textPrint.printf(screen, "Number of joysticks: {}".format(joystick_count) ) - textPrint.indent() - - # For each joystick: - joystick = pygame.joystick.Joystick(0) - joystick.init() - - textPrint.printf(screen, "Joystick {}".format(0) ) - textPrint.indent() - - # Get the name from the OS for the controller/joystick - name = joystick.get_name() - textPrint.printf(screen, "Joystick name: {}".format(name) ) - - # Usually axis run in pairs, up/down for one, and left/right for - # the other. - axes = joystick.get_numaxes() - textPrint.printf(screen, "Number of axes: {}".format(axes) ) - textPrint.indent() - - for i in range( axes ): - axis = joystick.get_axis( i ) - textPrint.printf(screen, "Axis {} value: {:>6.3f}".format(i, axis) ) - textPrint.unindent() - - buttons = joystick.get_numbuttons() - textPrint.printf(screen, "Number of buttons: {}".format(buttons) ) - textPrint.indent() - - for i in range( buttons ): - button = joystick.get_button( i ) - textPrint.printf(screen, "Button {:>2} value: {}".format(i, button) ) - textPrint.unindent() - - textPrint.unindent() - - # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT - - # Go ahead and update the screen with what we've drawn. - pygame.display.flip() - - # Limit to 20 frames per second - clock.tick(20) - -# Close the window and quit. -# If you forget this line, the program will 'hang' -# on exit if running from IDLE. -pygame.quit() diff --git a/tools/carcontrols/joystickd.py b/tools/carcontrols/joystickd.py deleted file mode 100755 index 9436dcb4d6..0000000000 --- a/tools/carcontrols/joystickd.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python - -# This process publishes joystick events. Such events can be suscribed by -# mocked car controller scripts. - - -### this process needs pygame and can't run on the EON ### - -import pygame # pylint: disable=import-error -import cereal.messaging as messaging - -def joystick_thread(): - joystick_sock = messaging.pub_sock('testJoystick') - - pygame.init() - - # Used to manage how fast the screen updates - clock = pygame.time.Clock() - - # Initialize the joysticks - pygame.joystick.init() - - # Get count of joysticks - joystick_count = pygame.joystick.get_count() - if joystick_count > 1: - raise ValueError("More than one joystick attached") - elif joystick_count < 1: - raise ValueError("No joystick found") - - # -------- Main Program Loop ----------- - while True: - # EVENT PROCESSING STEP - for event in pygame.event.get(): # User did something - if event.type == pygame.QUIT: # If user clicked close - pass - # Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION - if event.type == pygame.JOYBUTTONDOWN: - print("Joystick button pressed.") - if event.type == pygame.JOYBUTTONUP: - print("Joystick button released.") - - joystick = pygame.joystick.Joystick(0) - joystick.init() - - # Usually axis run in pairs, up/down for one, and left/right for - # the other. - axes = [] - buttons = [] - - for a in range(joystick.get_numaxes()): - axes.append(joystick.get_axis(a)) - - for b in range(joystick.get_numbuttons()): - buttons.append(bool(joystick.get_button(b))) - - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = axes - dat.testJoystick.buttons = buttons - joystick_sock.send(dat.to_bytes()) - - # Limit to 100 frames per second - clock.tick(100) - -if __name__ == "__main__": - joystick_thread() diff --git a/tools/joystick/README.md b/tools/joystick/README.md new file mode 100644 index 0000000000..33366d1efd --- /dev/null +++ b/tools/joystick/README.md @@ -0,0 +1,45 @@ +# Joystick + +**Hardware needed**: [comma two devkit](https://comma.ai/shop/products/comma-two-devkit), laptop, joystick (optional) + +With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard, however a joystick is recommended for more precise control. + +Using a keyboard +--- + +To get started, ssh into your comma device and start joystickd with the following command: + +```shell +tools/joystick/joystickd.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. + +Using a joystick +--- + +In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` ZMQ packets over the network to the comma device. First connect a compatible joystick to your PC; joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. + +1. Connect your laptop to your comma device's hotspot and open a new ssh shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: + ```shell + # on your comma device + echo -n "1" > /data/params/d/JoystickDebugMode + ``` +2. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them: + ```shell + # on your comma device + cereal/messaging/bridge {LAPTOP_IP} testJoystick + ``` +3. Finally, start joystickd on your laptop and tell it to publish ZMQ packets over the network: + ```shell + # on your laptop + export ZMQ=1 + tools/joystick/joystickd.py + ``` + +--- +Now start your car and openpilot should go into debug mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell. + +Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. + +![Imgur](steer.gif) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py new file mode 100755 index 0000000000..b4dc532ef8 --- /dev/null +++ b/tools/joystick/joystickd.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +import argparse + +import cereal.messaging as messaging +from common.numpy_fast import interp, clip +from common.params import Params +from inputs import get_gamepad +from tools.lib.kbhit import KBHit + + +class Keyboard: + def __init__(self): + self.kb = KBHit() + self.axis_increment = 0.05 # 5% of full actuation each key press + self.axes_map = {'w': 'gb', 's': 'gb', + 'a': 'steer', 'd': 'steer'} + self.axes_values = {'gb': 0., 'steer': 0.} + + def update(self): + key = self.kb.getch().lower() + self.cancel = False + if key == 'r': + self.axes_values = {ax: 0. for ax in self.axes_values} + elif key == 'c': + self.cancel = True + elif key in self.axes_map: + axis = self.axes_map[key] + incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment + self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1) + else: + return False + return True + + +class Joystick: + def __init__(self): + self.max_axis_value = 255 # tune based on your joystick, 0 to this + self.cancel_button = 'BTN_TRIGGER' + self.axes_values = {'ABS_Y': 0., 'ABS_RZ': 0.} # gb, steer + + def update(self): + joystick_event = get_gamepad()[0] + event = (joystick_event.code, joystick_event.state) + self.cancel = False + if event[0] == self.cancel_button and event[1] == 0: # state 0 is falling edge + self.cancel = True + elif event[0] in self.axes_values: + norm = -interp(event[1], [0, self.max_axis_value], [-1., 1.]) + self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0. # center can be noisy, deadzone of 5% + else: + return False + return True + + +def joystick_thread(use_keyboard): + Params().put_bool('JoystickDebugMode', True) + joystick_sock = messaging.pub_sock('testJoystick') + joystick = Keyboard() if use_keyboard else Joystick() + + while True: + ret = joystick.update() # processes joystick/key events and handles state of axes + if ret: + dat = messaging.new_message('testJoystick') + dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_values] + dat.testJoystick.buttons = [joystick.cancel] + joystick_sock.send(dat.to_bytes()) + print('\n' + ', '.join([f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()])) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description='Publishes events from your joystick to control your car', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') + args = parser.parse_args() + + if args.keyboard: + print('\nGas/brake control: `W` and `S` keys\n' + 'Steering control: `A` and `D` keys') + print('Buttons:\n' + '- `R`: Resets axes\n' + '- `C`: Cancel cruise control') + else: + print('\nUsing joystick, make sure to run bridge on your device if running over the network!') + + joystick_thread(args.keyboard) diff --git a/tools/carcontrols/steer.gif b/tools/joystick/steer.gif similarity index 100% rename from tools/carcontrols/steer.gif rename to tools/joystick/steer.gif