Merge Aaron's branch

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

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,20 +176,23 @@ 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:
     print("The car must be off before running joystick_control.")
     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)

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':