diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index e7a366d1e2..c901eb2dfa 100755 --- a/examples/longitudinal-profiles.py +++ b/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,66 +141,102 @@ def report(args, logs, fp): print(f"\nReport written to {output_fn}\n") def main(args): - with PandaRunner() as p: - print("\n\n") - - maneuvers = MANEUVERS - if len(args.maneuvers): - maneuvers = [MANEUVERS[i-1] for i in set(args.maneuvers)] - - logs = {} - rk = Ratekeeper(int(1./DT)) - for i, m in enumerate(maneuvers): - logs[m.description] = {} - print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'") - for run in range(m.repeat): - print(f"- run #{run}") - print("- setting up, engage cruise") - ready_cnt = 0 - for _ in range(int(2*60./DT)): - cs = p.read(strict=False) - cc = CarControl( - enabled=True, - longActive=True, - actuators=CarControl.Actuators( - accel=(m.initial_speed - cs.vEgo)*0.8, - 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): - 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) + 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 + if len(args.maneuvers): + maneuvers = [MANEUVERS[i-1] for i in set(args.maneuvers)] + + logs = {} + rk = Ratekeeper(int(1./DT_CTRL)) + for i, m in enumerate(maneuvers): + logs[m.description] = {} + print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'") + for run in range(m.repeat): + print(f"- run #{run}") + print("- setting up, engage cruise") + ready_cnt = 0 + for _ in range(int(2*60./DT_CTRL)): + cs = p.read(strict=False) + cc = CarControl( + enabled=True, + longActive=True, + actuators=CarControl.Actuators( + accel=(m.initial_speed - cs.vEgo)*0.8, + 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__": diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4492b21b47..7c549e5baf 100755 --- a/selfdrive/controls/controlsd.py +++ b/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']