diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 697d0273a5..d696566ba5 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -56,6 +56,9 @@ def update_v_cruise(self, CS, enabled, is_metric): if CS.cruiseState.speed == 0: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET + elif CS.cruiseState.speed == -1: + self.v_cruise_kph = -1 + self.v_cruise_cluster_kph = -1 else: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -120,12 +123,13 @@ def update_button_timers(self, CS, enabled): self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control: bool) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + initial_experimental_mode = experimental_mode and not dynamic_experimental_control + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: self.v_cruise_kph = self.v_cruise_kph_last diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index 74d034d505..4f6b855c17 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -18,7 +18,7 @@ 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', + self.axes_map = {'b': 'gb', 's': 'gb', 'a': 'steer', 'd': 'steer'} self.axes_values = {'gb': 0., 'steer': 0.} self.axes_order = ['gb', 'steer'] @@ -33,12 +33,67 @@ def update(self): 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 + incr = self.axis_increment if key in ['b', 'a'] else -self.axis_increment self.axes_values[axis] = float(np.clip(self.axes_values[axis] + incr, -1, 1)) else: return False return True + +class SteeringGUI: + def __init__(self, steer_slider): + # Refers to the acceleration and steering inputs + self.accel_axis = 'GUI_ACCEL' + self.steer_axis = 'GUI_STEER' + # Acceleration and steering both range from -1.0 to 1.0 + self.min_axis_value = {self.accel_axis: -1.0, self.steer_axis: -1.0} + self.max_axis_value = {self.accel_axis: 1.0, self.steer_axis: 1.0} + + # Initially, acceleration and steering are set to 0 + self.axes_values = {self.accel_axis: 0., self.steer_axis: 0.} + + # Defines the order in which to read/process the axes + self.axes_order = [self.accel_axis, self.steer_axis] + + # Tracks whether the "cancel" button (or an emergency stop control) has been pressed in the GUI + self.cancel = False + + # Tracks the previous state of the cancel button + self._cancel_prev = False + + def update(self): + # Read input values directly from the GUI + try: + accel_raw = get_throttle_value() + steer_raw = get_slider_value() + cancel_now = is_cancel_pressed() + + except Exception: + # If GUI is unavailable or throws error, set neutral state + self.axes_values = {ax: 0. for ax in self.axes_values} + return False + + # Update cancel logic joystick-style, detect when a button is pressed or released + if not self._cancel_prev and cancel_now: + self.cancel = True # rising edge + elif self._cancel_prev and not cancel_now: + self.cancel = False # falling edge + + # Was the cancel button not pressed last update, but is pressed now? + self._cancel_prev = cancel_now + + # Normalizing accel/steer input + for axis, raw_value in [(self.accel_axis, accel_raw), (self.steer_axis, steer_raw)]: + norm = -float(np.interp(raw_value, [self.min_axis_value[axis], self.max_axis_value[axis]], [-1., 1.])) + norm = norm if abs(norm) > 0.03 else 0. # deadzone + self.axes_values[axis] = EXPO * norm ** 3 + (1 - EXPO) * norm + + return True + + + + + class Joystick: def __init__(self): @@ -121,11 +176,12 @@ def main(): if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + + parser = argparse.ArgumentParser(description='Publishes events from your GUI, joystick to control your car.\n' + 'openpilot must be offroad before starting joystick_control. This tool supports ' + 'a PlayStation 5 DualSense controller on the comma 3X.', formatter_class=argparse.ArgumentDefaultsHelpFormatter) - parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') + parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a keyboard joystick') + parser.add_argument('--gui', action='store_true', help='Use your GUI instead of a joystick') args = parser.parse_args() if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: @@ -133,8 +189,10 @@ def main(): exit() print() - if args.keyboard: - print('Gas/brake control: `W` and `S` keys') + if args.gui: + print('Using GUI for control (slider + throttle inputs).') + elif args.keyboard: + print('Gas/brake control: `B` and `S` keys') print('Steering control: `A` and `D` keys') print('Buttons') print('- `R`: Resets axes') @@ -143,5 +201,12 @@ def main(): print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') print('If not running on a comma device, the mapping may need to be adjusted.') - joystick = Keyboard() if args.keyboard else Joystick() - joystick_control_thread(joystick) + # Controller selection logic + if args.gui: + # Replace gui with the module that has these functions + from your_module import get_slider_value, get_throttle_value, is_cancel_pressed + control = SteeringGUI(get_slider_value, get_throttle_value, is_cancel_pressed) + steering_control_thread(control) + else: + joystick = Keyboard() if args.keyboard else Joystick() + joystick_control_thread(joystick) diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 0a17f0ee85..014a4f9466 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -28,7 +28,7 @@ | r | Reset Simulation | | i | Toggle Ignition | | q | Exit all | - | wasd | Control manually | + | basd | Control manually | """ @@ -66,8 +66,8 @@ def keyboard_poll_thread(q: 'Queue[QueueMessage]'): q.put(control_cmd_gen("cruise_down")) elif c == '3': q.put(control_cmd_gen("cruise_cancel")) - elif c == 'w': - q.put(control_cmd_gen(f"throttle_{1.0}")) + elif c == 'b': + q.put(control_cmd_gen(f"throttle_{100.0}")) elif c == 'a': q.put(control_cmd_gen(f"steer_{-0.15}")) elif c == 's':