diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 53e7d07bf6..b48ced1955 100644 --- a/tools/sim/bridge/common.py +++ b/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() diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index f8b3517fde..2f27e06969 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/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: