process replay: initialize controlsState with original route (#25461)

* push

* do process replay

* commit

* update refs

* clean up

* clean up controlsd

* clean up controlsd

* Add assert

* debubuggier param name

* can be peristent

* Revert "can be peristent"

This reverts commit 6e6d3f6423c26a202623cef728e038259e9e46cd.

* Update selfdrive/test/process_replay/process_replay.py

common/params.cc

@@ -151,6 +151,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFront", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReleaseNotes", PERSISTENT},
+    {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"ShouldDoUpdate", CLEAR_ON_MANAGER_START},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"SshEnabled", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -203,6 +203,16 @@ class Controls:
     self.rk = Ratekeeper(100, print_delay_threshold=None)
     self.prof = Profiler(False)  # off by default
 
+  def set_initial_state(self):
+    if REPLAY:
+      controls_state = Params().get("ReplayControlsState")
+      if controls_state is not None:
+        controls_state = log.ControlsState.from_bytes(controls_state)
+        self.v_cruise_kph = controls_state.vCruise
+
+      if self.sm['pandaStates'][0].controlsAllowed:
+        self.state = State.enabled
+
   def update_events(self, CS):
     """Compute carEvents from carState"""
 
@@ -417,11 +427,9 @@ class Controls:
       if all_valid or timed_out or SIMULATION:
         if not self.read_only:
           self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+
         self.initialized = True
-
-        if REPLAY and self.sm['pandaStates'][0].controlsAllowed:
-          self.state = State.enabled
-
+        self.set_initial_state()
         Params().put_bool("ControlsReady", True)
 
     # Check for CAN timeout

selfdrive/test/process_replay/process_replay.py

@@ -385,7 +385,7 @@ def replay_process(cfg, lr, fingerprint=None):
       return cpp_replay_process(cfg, lr, fingerprint)
 
 
-def setup_env(simulation=False, CP=None, cfg=None):
+def setup_env(simulation=False, CP=None, cfg=None, controlsState=None):
   params = Params()
   params.clear_all()
   params.put_bool("OpenpilotEnabledToggle", True)
@@ -414,6 +414,12 @@ def setup_env(simulation=False, CP=None, cfg=None):
   elif "SIMULATION" in os.environ:
     del os.environ["SIMULATION"]
 
+  # Initialize controlsd with a controlsState packet
+  if controlsState is not None:
+    params.put("ReplayControlsState", controlsState.as_builder().to_bytes())
+  else:
+    params.delete("ReplayControlsState")
+
   # Regen or python process
   if CP is not None:
     if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS:
@@ -440,13 +446,25 @@ def python_replay_process(cfg, lr, fingerprint=None):
   all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
   pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
 
+  controlsState = None
+  initialized = False
+  for msg in lr:
+    if msg.which() == 'controlsState':
+      controlsState = msg.controlsState
+      if initialized:
+        break
+    elif msg.which() == 'carEvents':
+      initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents]
+
+  assert controlsState is not None and initialized, "controlsState never initialized"
+
   if fingerprint is not None:
     os.environ['SKIP_FW_QUERY'] = "1"
     os.environ['FINGERPRINT'] = fingerprint
-    setup_env(cfg=cfg)
+    setup_env(cfg=cfg, controlsState=controlsState)
   else:
     CP = [m for m in lr if m.which() == 'carParams'][0].carParams
-    setup_env(CP=CP, cfg=cfg)
+    setup_env(CP=CP, cfg=cfg, controlsState=controlsState)
 
   assert(type(managed_processes[cfg.proc_name]) is PythonProcess)
   managed_processes[cfg.proc_name].prepare()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-118d78e2040103c00b4bfcc875fcdcd6a15e2211
+375f581030fe8efeb9dacd63875b3f046d3b420f

selfdrive/test/process_replay/regen.py

@@ -203,9 +203,10 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False):
 
   # Get and setup initial state
   CP = [m for m in lr if m.which() == 'carParams'][0].carParams
+  controlsState = [m for m in lr if m.which() == 'controlsState'][0].controlsState
   liveCalibration = [m for m in lr if m.which() == 'liveCalibration'][0]
 
-  setup_env(CP=CP)
+  setup_env(CP=CP, controlsState=controlsState)
   params.put("CalibrationParams", liveCalibration.as_builder().to_bytes())
 
   vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm)

selfdrive/test/process_replay/test_processes.py

@@ -49,7 +49,7 @@ segments = [
   ("CHRYSLER", "regen38346FB33D0|2022-07-14--18-05-26--0"),
   ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"),
   ("SUBARU", "regen54A1E2BE5AA|2022-07-14--18-07-50--0"),
-  ("GM", "regen01D09D915B5|2022-07-06--14-36-20--0"),
+  ("GM", "regen76027B408B7|2022-08-16--19-56-58--0"),
   ("NISSAN", "regenCA0B0DC946E|2022-07-14--18-10-17--0"),
   ("VOLKSWAGEN", "regen007098CA0EF|2022-07-06--15-01-26--0"),
   ("MAZDA", "regen61BA413D53B|2022-07-06--14-39-42--0"),
@@ -248,12 +248,8 @@ if __name__ == "__main__":
       print("TEST SUCCEEDED")
 
   else:
-    if failed:
-      print(diff1)
-      print("FAILED TO UPDATE REFS")
-    else:
-      with open(REF_COMMIT_FN, "w") as f:
-        f.write(cur_commit)
-      print(f"\n\nUpdated reference logs for commit: {cur_commit}")
+    with open(REF_COMMIT_FN, "w") as f:
+      f.write(cur_commit)
+    print(f"\n\nUpdated reference logs for commit: {cur_commit}")
 
   sys.exit(int(failed))