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
-
+from openpilot.common.realtime import DT_CTRL, Ratekeeper
-DT = 0.01  # step time (s)
 
 # 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,66 +141,102 @@ 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')
-    print("\n\n")
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
-
+
-    maneuvers = MANEUVERS
+  maneuvers = iter(MANEUVERS)
-    if len(args.maneuvers):
+
-      maneuvers = [MANEUVERS[i-1] for i in set(args.maneuvers)]
+  while True:
-
+    sm.update()
-    logs = {}
+
-    rk = Ratekeeper(int(1./DT))
+    maneuver = next(maneuvers, None)
-    for i, m in enumerate(maneuvers):
+
-      logs[m.description] = {}
+    if maneuver is not None:
-      print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'")
+      pass
-      for run in range(m.repeat):
+
-        print(f"- run #{run}")
+    plan_send = messaging.new_message('longitudinalPlan')
-        print("- setting up, engage cruise")
+    plan_send.valid = sm.all_checks()
-        ready_cnt = 0
+
-        for _ in range(int(2*60./DT)):
+    longitudinalPlan = plan_send.longitudinalPlan
-          cs = p.read(strict=False)
+
-          cc = CarControl(
+    longitudinalPlan.speeds = []
-            enabled=True,
+    longitudinalPlan.accels = []
-            longActive=True,
+    longitudinalPlan.jerks = []
-            actuators=CarControl.Actuators(
+
-              accel=(m.initial_speed - cs.vEgo)*0.8,
+    longitudinalPlan.aTarget = 0.
-              longControlState=CarControl.Actuators.LongControlState.pid,
+    longitudinalPlan.shouldStop = False
-            ),
+    longitudinalPlan.allowBrake = True
-          )
+    longitudinalPlan.allowThrottle = True
-          if m.initial_speed < 0.1:
+
-            cc.actuators.accel = -2
+    longitudinalPlan.hasLead = True
-            cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping
+
-          p.write(cc)
+    pm.send('longitudinalPlan', plan_send)
-
+
-          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
+    assistance_send = messaging.new_message('driverAssistance')
-          if ready_cnt > (2./DT):
+    assistance_send.valid = True
-            break
+    pm.send('driverAssistance', assistance_send)
-          rk.keep_time()
+
-        else:
+
-          print("ERROR: failed to setup")
+  print("\n\n")
-          continue
+
-
+  maneuvers = MANEUVERS
-        print("- executing maneuver")
+  if len(args.maneuvers):
-        logs[m.description][run] = defaultdict(list)
+    maneuvers = [MANEUVERS[i-1] for i in set(args.maneuvers)]
-        for t, cc in m.get_msgs():
+
-          cs = p.read()
+  logs = {}
-          p.write(cc)
+  rk = Ratekeeper(int(1./DT_CTRL))
-
+  for i, m in enumerate(maneuvers):
-          logs[m.description][run]["t"].append(t)
+    logs[m.description] = {}
-          to_log = {"carControl": cc, "carState": cs, "carControl.actuators": cc.actuators,
+    print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'")
-                    "carControl.cruiseControl": cc.cruiseControl, "carState.cruiseState": cs.cruiseState}
+    for run in range(m.repeat):
-          for k, v in to_log.items():
+      print(f"- run #{run}")
-            for k2, v2 in asdict(v).items():
+      print("- setting up, engage cruise")
-              logs[m.description][run][f"{k}.{k2}"].append(v2)
+      ready_cnt = 0
-
+      for _ in range(int(2*60./DT_CTRL)):
-          rk.keep_time()
+        cs = p.read(strict=False)
-
+        cc = CarControl(
-  print("writing out report")
+          enabled=True,
-  with open('/tmp/logs.json', 'w') as f:
+          longActive=True,
-    import json
+          actuators=CarControl.Actuators(
-    json.dump(logs, f, indent=2)
+            accel=(m.initial_speed - cs.vEgo)*0.8,
-  report(args, logs, p.CI.CP.carFingerprint)
+            longControlState=CarControl.Actuators.LongControlState.pid,
+          ),
+        )
+        if m.initial_speed < 0.1:
+          cc.actuators.accel = -2
+          cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping
+        p.write(cc)
+
+        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_CTRL):
+          break
+        rk.keep_time()
+      else:
+        print("ERROR: failed to setup")
+        continue
+
+      print("- executing maneuver")
+      logs[m.description][run] = defaultdict(list)
+      for t, cc in m.get_msgs():
+        cs = p.read()
+        p.write(cc)
+
+        logs[m.description][run]["t"].append(t)
+        to_log = {"carControl": cc, "carState": cs, "carControl.actuators": cc.actuators,
+                  "carControl.cruiseControl": cc.cruiseControl, "carState.cruiseState": cs.cruiseState}
+        for k, v in to_log.items():
+          for k2, v2 in asdict(v).items():
+            logs[m.description][run][f"{k}.{k2}"].append(v2)
+
+        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)
 
 
 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']