diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index d9c922bb90..9128f1acd8 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -320,10 +320,11 @@ 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: - 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) + # 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", "") 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 d0e5317124..b866e015ac 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -78eb5f832bb1a53b33d952edc586b986a8e31089 \ No newline at end of file +eac137f456f25bf138677315b7c4907e2fe9971b \ No newline at end of file diff --git a/system/manager/process_config.py b/system/manager/process_config.py index a85bfd1f46..8af376c624 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -29,12 +29,6 @@ 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() @@ -69,8 +63,7 @@ procs = [ 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", not_joystick), - PythonProcess("joystickd", "tools.joystick.joystickd", joystick), + PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), 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 52460031a5..09cc16d337 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -3,19 +3,15 @@ import os import argparse import threading from inputs import get_gamepad -import math -from cereal import messaging, car -from openpilot.common.realtime import DT_CTRL, Ratekeeper +import cereal.messaging as messaging +from openpilot.common.realtime import 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 -EXPO = 0.4 -MAX_LAT_ACCEL = 2.5 +JS_EXPO = 0.4 class Keyboard: @@ -84,56 +80,21 @@ class Joystick: 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]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control + self.axes_values[event[0]] = JS_EXPO * norm ** 3 + (1 - JS_EXPO) * norm # less action near center for fine control else: return False return True def send_thread(joystick): - 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']) - + joystick_sock = messaging.pub_sock('testJoystick') rk = Ratekeeper(100, print_delay_threshold=None) while 1: - 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) - + 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())) rk.keep_time() @@ -144,10 +105,6 @@ 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 ' +