From db839e887dd241187667cc33afd3f214c1a70896 Mon Sep 17 00:00:00 2001
From: Justin Newberry <justin@comma.ai>
Date: Fri, 29 Sep 2023 16:32:45 -0700
Subject: [PATCH] Simulator: blinkers and lane changes (#30107)

blinkers and lane changes
old-commit-hash: ba90bdf7d9c2d436be33ac7def0ab02d535efe73
---
 tools/sim/bridge/common.py     | 8 ++++++++
 tools/sim/lib/common.py        | 4 ++++
 tools/sim/lib/keyboard_ctrl.py | 4 ++++
 tools/sim/lib/simulated_car.py | 9 +++++++--
 4 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py
index 3ca5cc5f5a..a0e6538b2c 100644
--- a/tools/sim/bridge/common.py
+++ b/tools/sim/bridge/common.py
@@ -109,6 +109,8 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
       throttle_op = steer_op = brake_op = 0.0
 
       self.simulator_state.cruise_button = 0
+      self.simulator_state.left_blinker = False
+      self.simulator_state.right_blinker = False
 
       throttle_manual = steer_manual = brake_manual = 0.
 
@@ -131,6 +133,11 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
             self.simulator_state.cruise_button = CruiseButtons.CANCEL
           elif m[1] == "main":
             self.simulator_state.cruise_button = CruiseButtons.MAIN
+        elif m[0] == "blinker":
+          if m[1] == "left":
+            self.simulator_state.left_blinker = True
+          elif m[1] == "right":
+            self.simulator_state.right_blinker = True
         elif m[0] == "ignition":
           self.simulator_state.ignition = not self.simulator_state.ignition
         elif m[0] == "reset":
@@ -140,6 +147,7 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
 
       self.simulator_state.user_brake = brake_manual
       self.simulator_state.user_gas = throttle_manual
+      self.simulator_state.user_torque = steer_manual * 10000
 
       steer_manual = steer_manual * -40
 
diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py
index 19012b1c2d..b7aa66f5c5 100644
--- a/tools/sim/lib/common.py
+++ b/tools/sim/lib/common.py
@@ -49,9 +49,13 @@ class SimulatorState:
 
     self.user_gas: float = 0
     self.user_brake: float = 0
+    self.user_torque: float = 0
 
     self.cruise_button = 0
 
+    self.left_blinker = False
+    self.right_blinker = False
+
   @property
   def speed(self):
     return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py
index a9d22c9757..c37f6ea479 100644
--- a/tools/sim/lib/keyboard_ctrl.py
+++ b/tools/sim/lib/keyboard_ctrl.py
@@ -67,6 +67,10 @@ def keyboard_poll_thread(q: 'Queue[str]'):
       q.put("brake_%f" % 1.0)
     elif c == 'd':
       q.put("steer_%f" % -0.15)
+    elif c == 'z':
+      q.put("blinker_left")
+    elif c == 'x':
+      q.put("blinker_right")
     elif c == 'i':
       q.put("ignition")
     elif c == 'r':
diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py
index 41b58063b9..ab60fad8d3 100644
--- a/tools/sim/lib/simulated_car.py
+++ b/tools/sim/lib/simulated_car.py
@@ -59,7 +59,7 @@ class SimulatedCar:
     msg.append(self.packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}))
     msg.append(self.packer.make_can_msg("GAS_PEDAL_2", 0, {}))
     msg.append(self.packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}))
-    msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {"STEER_TORQUE_SENSOR": simulator_state.user_torque}))
     msg.append(self.packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": simulator_state.steering_angle}))
     msg.append(self.packer.make_can_msg("VSA_STATUS", 0, {}))
     msg.append(self.packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if simulator_state.speed >= 1.0 else 0}))
@@ -68,7 +68,12 @@ class SimulatedCar:
     msg.append(self.packer.make_can_msg("DOORS_STATUS", 0, {}))
     msg.append(self.packer.make_can_msg("CRUISE_PARAMS", 0, {}))
     msg.append(self.packer.make_can_msg("CRUISE", 0, {}))
-    msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}))
+    msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0,
+                                    {
+                                      "MAIN_ON": 1,
+                                      "LEFT_BLINKER": simulator_state.left_blinker,
+                                      "RIGHT_BLINKER": simulator_state.right_blinker
+                                    }))
     msg.append(self.packer.make_can_msg("POWERTRAIN_DATA", 0,
                                     {
                                     "ACC_STATUS": int(simulator_state.is_engaged),