Simulation: engage on startup, print help message (#30044)

old-commit-hash: 2792e5f2db33bd955d327545e6fba30ae2826ee0

tools/sim/bridge/common.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.car.honda.values import CruiseButtons
 from openpilot.tools.sim.lib.common import SimulatorState, World
 from openpilot.tools.sim.lib.simulated_car import SimulatedCar
 from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors
+from openpilot.tools.sim.lib.keyboard_ctrl import KEYBOARD_HELP
 
 
 def rk_loop(function, hz, exit_event: threading.Event):
@@ -45,6 +46,8 @@ class SimulatorBridge(ABC):
 
     self.world: Optional[World] = None
 
+    self.past_startup_engaged = False
+
   def _on_shutdown(self, signal, frame):
     self.shutdown()
 
@@ -69,6 +72,16 @@ class SimulatorBridge(ABC):
     bridge_p.start()
     return bridge_p
 
+  def print_status(self):
+    print(
+    f"""
+Keyboard Commands:
+{KEYBOARD_HELP}
+
+State:
+Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_engaged}
+    """)
+
   @abstractmethod
   def spawn_world(self) -> World:
     pass
@@ -129,18 +142,21 @@ class SimulatorBridge(ABC):
       # Update openpilot on current sensor state
       self.simulated_sensors.update(self.simulator_state, self.world)
 
-      is_openpilot_engaged = self.simulated_car.sm['controlsState'].active
-
       self.simulated_car.sm.update(0)
+      self.simulator_state.is_engaged = self.simulated_car.sm['controlsState'].active
 
-      if is_openpilot_engaged:
+      if self.simulator_state.is_engaged:
         throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
         brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
         steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
 
-      throttle_out = throttle_op if is_openpilot_engaged else throttle_manual
-      brake_out = brake_op if is_openpilot_engaged else brake_manual
-      steer_out = steer_op if is_openpilot_engaged else steer_manual
+        self.past_startup_engaged = True
+      elif not self.past_startup_engaged:
+        self.simulator_state.cruise_button = CruiseButtons.DECEL_SET # force engagement on startup
+
+      throttle_out = throttle_op if self.simulator_state.is_engaged else throttle_manual
+      brake_out = brake_op if self.simulator_state.is_engaged else brake_manual
+      steer_out = steer_op if self.simulator_state.is_engaged else steer_manual
 
       self.world.apply_controls(steer_out, throttle_out, brake_out)
       self.world.read_sensors(self.simulator_state)
@@ -149,6 +165,9 @@ class SimulatorBridge(ABC):
         self.world.tick()
         self.world.read_cameras()
 
+      if self.rk.frame % 25 == 0:
+        self.print_status()
+
       self.started = True
 
       self.rk.keep_time()

tools/sim/lib/keyboard_ctrl.py

@@ -17,6 +17,20 @@ CC = 6
 
 STDIN_FD = sys.stdin.fileno()
 
+
+KEYBOARD_HELP = """
+  | key  |   functionality    |
+  -----------------------------
+  |  1   |   Cruise Resume    |
+  |  2   |    Cruise Set      |
+  |  3   |   Cruise Cancel    |
+  |  r   |  Reset Simulation  |
+  |  i   |  Toggle Ignition   |
+  |  q   |      Exit all      |
+  | wasd |  Control manually  |
+"""
+
+
 def getch() -> str:
   old_settings = termios.tcgetattr(STDIN_FD)
   try: