stash

examples/longitudinal-profiles.py

@@ -10,34 +10,16 @@ from collections import defaultdict
 from dataclasses import dataclass, asdict
 from pathlib import Path
 
+from cereal import messaging
 from opendbc.car.structs import CarControl
-from opendbc.car.panda_runner import PandaRunner
 from opendbc.car.common.conversions import Conversions
-
-DT = 0.01  # step time (s)
+from openpilot.common.realtime import DT_CTRL, Ratekeeper
 
 # TODOs
 # - support lateral maneuvers
 # - setup: show countdown?
 
 
-class Ratekeeper:
-  def __init__(self, rate: float) -> None:
-    self.interval = 1. / rate
-    self.next_frame_time = time.monotonic() + self.interval
-
-  def keep_time(self) -> bool:
-    lagged = False
-    remaining = self.next_frame_time - time.monotonic()
-    self.next_frame_time += self.interval
-    if remaining < -0.1:
-      print(f"lagging by {-remaining * 1000:.2f} ms")
-      lagged = True
-
-    if remaining > 0:
-      time.sleep(remaining)
-    return lagged
-
 @dataclass
 class Action:
   accel: float      # m/s^2
@@ -54,7 +36,7 @@ class Action:
           longControlState=self.longControlState,
         ),
       ))
-      for t in np.linspace(0, self.duration, int(self.duration/DT))
+      for t in np.linspace(0, self.duration, int(self.duration/DT_CTRL))
     ]
 
 @dataclass
@@ -159,7 +141,43 @@ def report(args, logs, fp):
   print(f"\nReport written to {output_fn}\n")
 
 def main(args):
-  with PandaRunner() as p:
+  sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
+
+  maneuvers = iter(MANEUVERS)
+
+  while True:
+    sm.update()
+
+    maneuver = next(maneuvers, None)
+
+    if maneuver is not None:
+      pass
+
+    plan_send = messaging.new_message('longitudinalPlan')
+    plan_send.valid = sm.all_checks()
+
+    longitudinalPlan = plan_send.longitudinalPlan
+
+    longitudinalPlan.speeds = []
+    longitudinalPlan.accels = []
+    longitudinalPlan.jerks = []
+
+    longitudinalPlan.aTarget = 0.
+    longitudinalPlan.shouldStop = False
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = True
+
+    longitudinalPlan.hasLead = True
+
+    pm.send('longitudinalPlan', plan_send)
+
+
+    assistance_send = messaging.new_message('driverAssistance')
+    assistance_send.valid = True
+    pm.send('driverAssistance', assistance_send)
+
+
   print("\n\n")
 
   maneuvers = MANEUVERS
@@ -167,7 +185,7 @@ def main(args):
     maneuvers = [MANEUVERS[i-1] for i in set(args.maneuvers)]
 
   logs = {}
-    rk = Ratekeeper(int(1./DT))
+  rk = Ratekeeper(int(1./DT_CTRL))
   for i, m in enumerate(maneuvers):
     logs[m.description] = {}
     print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'")
@@ -175,7 +193,7 @@ def main(args):
       print(f"- run #{run}")
       print("- setting up, engage cruise")
       ready_cnt = 0
-        for _ in range(int(2*60./DT)):
+      for _ in range(int(2*60./DT_CTRL)):
         cs = p.read(strict=False)
         cc = CarControl(
           enabled=True,
@@ -192,7 +210,7 @@ def main(args):
 
         ready = cs.cruiseState.enabled and not cs.cruiseState.standstill and ((m.initial_speed - 0.6) < cs.vEgo < (m.initial_speed + 0.6))
         ready_cnt = (ready_cnt+1) if ready else 0
-          if ready_cnt > (2./DT):
+        if ready_cnt > (2./DT_CTRL):
           break
         rk.keep_time()
       else:
@@ -214,11 +232,11 @@ def main(args):
 
         rk.keep_time()
 
-  print("writing out report")
-  with open('/tmp/logs.json', 'w') as f:
-    import json
-    json.dump(logs, f, indent=2)
-  report(args, logs, p.CI.CP.carFingerprint)
+  # print("writing out report")
+  # with open('/tmp/logs.json', 'w') as f:
+  #   import json
+  #   json.dump(logs, f, indent=2)
+  # report(args, logs, p.CI.CP.carFingerprint)
 
 
 if __name__ == "__main__":

selfdrive/controls/controlsd.py

@@ -18,6 +18,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+from opendbc.car.fingerprints import MIGRATION
 
 
 State = log.SelfdriveState.OpenpilotState
@@ -30,7 +31,9 @@ class Controls:
   def __init__(self) -> None:
     self.params = Params()
     cloudlog.info("controlsd is waiting for CarParams")
-    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
+    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams).as_builder()
+    self.CP.carFingerprint = MIGRATION.get(self.CP.carFingerprint, self.CP.carFingerprint)
+    self.CP = self.CP.as_reader()
     cloudlog.info("controlsd got CarParams")
 
     self.CI = get_car_interface(self.CP)
@@ -170,7 +173,7 @@ class Controls:
 
     # controlsState
     dat = messaging.new_message('controlsState')
-    dat.valid = CS.canValid
+    dat.valid = CS.canValid and self.sm.all_checks(['modelV2', 'longitudinalPlan',])
     cs = dat.controlsState
 
     lp = self.sm['liveParameters']