diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 9128f1acd81b50..d9c922bb901a04 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -320,11 +320,10 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - # TODO: add some info back - #axes = sm['testJoystick'].axes - #gb, steer = list(axes)[:2] if len(axes) else (0., 0.) - #vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" - return NormalPermanentAlert("Joystick Mode", "") + gb = sm['carControl'].actuators.accel / 4. + steer = sm['carControl'].actuators.steer + vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" + return NormalPermanentAlert("Joystick Mode", vals) def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: personality = str(personality).title() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index be74e5cfc92488..cfdddb09656b1c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fa642b559758928289d8092f30bb9787eb9ebcf2 +849fcc369f7afa78606cba636fe7c73e540e6df8 \ No newline at end of file diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 8af376c624b4bc..a85bfd1f46acf9 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -29,6 +29,12 @@ def ublox(started, params, CP: car.CarParams) -> bool: params.put_bool("UbloxAvailable", use_ublox) return started and use_ublox +def joystick(started: bool, params, CP: car.CarParams) -> bool: + return started and params.get_bool("JoystickDebugMode") + +def not_joystick(started: bool, params, CP: car.CarParams) -> bool: + return started and not params.get_bool("JoystickDebugMode") + def qcomgps(started, params, CP: car.CarParams) -> bool: return started and not ublox_available() @@ -63,7 +69,8 @@ def only_offroad(started, params, CP: car.CarParams) -> bool: NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), - PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), + PythonProcess("controlsd", "selfdrive.controls.controlsd", not_joystick), + PythonProcess("joystickd", "tools.joystick.joystickd", joystick), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 09cc16d337d03b..f39dec7d4bcd56 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -2,16 +2,20 @@ import os import argparse import threading -from inputs import get_gamepad +from inputs import UnpluggedError, get_gamepad +import math -import cereal.messaging as messaging -from openpilot.common.realtime import Ratekeeper +from cereal import messaging, car +from openpilot.common.realtime import DT_CTRL, Ratekeeper from openpilot.common.numpy_fast import interp, clip from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE from openpilot.tools.lib.kbhit import KBHit -JS_EXPO = 0.4 +EXPO = 0.4 +MAX_LAT_ACCEL = 2.5 class Keyboard: @@ -62,7 +66,12 @@ def __init__(self): self.cancel = False def update(self): - joystick_event = get_gamepad()[0] + try: + joystick_event = get_gamepad()[0] + except (OSError, UnpluggedError): + self.axes_values = {ax: 0. for ax in self.axes_values} + return False + event = (joystick_event.code, joystick_event.state) # flip left trigger to negative accel @@ -80,21 +89,56 @@ def update(self): norm = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]) norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3% - self.axes_values[event[0]] = JS_EXPO * norm ** 3 + (1 - JS_EXPO) * norm # less action near center for fine control + self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control else: return False return True def send_thread(joystick): - joystick_sock = messaging.pub_sock('testJoystick') + params = Params() + cloudlog.info("joystickd is waiting for CarParams") + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + VM = VehicleModel(CP) + + sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState'], frequency=1. / DT_CTRL) + pm = messaging.PubMaster(['carControl', 'controlsState']) + rk = Ratekeeper(100, print_delay_threshold=None) while 1: - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_order] - 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())) + sm.update(0) + + joystick_axes = [joystick.axes_values[a] for a in joystick.axes_order] + if rk.frame % 20 == 0: + print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) + + cc_msg = messaging.new_message('carControl') + cc_msg.valid = True + CC = cc_msg.carControl + CC.enabled = sm['selfdriveState'].enabled + CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent + CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl + + actuators = CC.actuators + + if CC.longActive: + actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1) + + if CC.latActive: + max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5) + max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll)) + + actuators.steer = clip(joystick_axes[1], -1, 1) + actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature + + pm.send('carControl', cc_msg) + + cs_msg = messaging.new_message('controlsState') + cs_msg.valid = True + controlsState = cs_msg.controlsState + controlsState.lateralControlState.init('debugState') + pm.send('controlsState', cs_msg) + rk.keep_time() @@ -105,6 +149,10 @@ def joystick_thread(joystick): joystick.update() +def main(): + joystick_thread(Joystick()) + + if __name__ == '__main__': parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + 'openpilot must be offroad before starting joystickd. This tool supports ' + diff --git a/tools/lib/kbhit.py b/tools/lib/kbhit.py index 3881ee7d18bc59..c6365cdfecc42a 100755 --- a/tools/lib/kbhit.py +++ b/tools/lib/kbhit.py @@ -4,13 +4,13 @@ import atexit from select import select -STDIN_FD = sys.stdin.fileno() class KBHit: def __init__(self) -> None: ''' Creates a KBHit object that you can call to do various keyboard things. ''' + self.stdin_fd = sys.stdin.fileno() self.set_kbhit_terminal() def set_kbhit_terminal(self) -> None: @@ -18,12 +18,12 @@ def set_kbhit_terminal(self) -> None: ''' # Save the terminal settings - self.old_term = termios.tcgetattr(STDIN_FD) + self.old_term = termios.tcgetattr(self.stdin_fd) self.new_term = self.old_term.copy() # New terminal setting unbuffered self.new_term[3] &= ~(termios.ICANON | termios.ECHO) - termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, self.new_term) + termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.new_term) # Support normal-terminal reset at exit atexit.register(self.set_normal_term) @@ -32,7 +32,7 @@ def set_normal_term(self) -> None: ''' Resets to normal terminal. On Windows this is a no-op. ''' - termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, self.old_term) + termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.old_term) @staticmethod def getch() -> str: