From 346b9b8c6f2536766a9a4fd66e9f0d9f6c87df45 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 29 Aug 2024 16:48:29 -0700 Subject: [PATCH 01/11] long profiles --- .gitignore | 2 + examples/longitudinal-profiles.py | 179 ++++++++++++++++++++++++++++++ pyproject.toml | 1 + 3 files changed, 182 insertions(+) create mode 100755 examples/longitudinal-profiles.py diff --git a/.gitignore b/.gitignore index 6c6810db60..767434eb72 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,8 @@ *.egg-info/ uv.lock +/examples/longitudinal_reports/ + opendbc/can/*.so opendbc/can/*.a opendbc/can/build/ diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py new file mode 100755 index 0000000000..b38a248fbd --- /dev/null +++ b/examples/longitudinal-profiles.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 +import io +import time +import base64 +import argparse +import matplotlib.pyplot as plt +from enum import Enum +from collections import defaultdict +from dataclasses import dataclass, asdict +from pathlib import Path + +from opendbc.car.structs import CarControl, CarState +from opendbc.car.can_definitions import CanData +from opendbc.car.panda_runner import PandaRunner + +DT = 0.01 # step time (s) +OUTPUT_PATH = Path(__file__).resolve().parent / "longitudinal_reports" + +class Setup(Enum): + STOPPED = 0 + STEADY_STATE_SPEED = 1 + +@dataclass +class Maneuver: + description: str + setup: Setup # initial state + + _log = defaultdict(list) + + def get_cc(self, t: float) -> CarControl: + CC = CarControl( + enabled=True, + latActive=False, + longActive=True, + + actuators=CarControl.Actuators( + gas=0., + brake=0., + speed=0., + accel=0., + longControlState=CarControl.Actuators.LongControlState.off, + ), + + cruiseControl=CarControl.CruiseControl( + cancel=False, + resume=False, + override=False, + ), + ) + return CC + + def get_msgs(self): + for t in range(0, int(1./DT)): + yield t, self.get_cc(t) + + def log(self, t, cc: CarControl, cs: CarState) -> None: + self._log["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(): + self._log[f"{k}.{k2}"].append(v2) + +MANEUVERS = [ + Maneuver( + "start from stop", + Setup.STOPPED, + ), + Maneuver( + "brake step response: 20mph steady state -> -1m/ss", + Setup.STEADY_STATE_SPEED, + ), + Maneuver( + "brake step response: 20mph steady state -> max brake", + Setup.STEADY_STATE_SPEED, + ), + Maneuver( + "gas step response: 20mph steady state -> +1m/ss", + Setup.STEADY_STATE_SPEED, + ), + Maneuver( + "gas step response: 20mph steady state -> max gas", + Setup.STEADY_STATE_SPEED, + ), + Maneuver( + "creeping: alternate between +1m/ss and -1m/ss", + Setup.STOPPED, + ), +] + +def main(args): + with PandaRunner() as (p, CI): + print("\n\n") + + for i, m in enumerate(MANEUVERS): + print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'") + + # cleanup and get into a good state + print("- setting up") + cs = None + for _ in range(int(3./DT)): + cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] + cs = CI.update([0, cd]) + _, can_sends = CI.apply(CarControl(enabled=False)) + p.can_send_many(can_sends, timeout=1000) + time.sleep(DT) + #assert not cs.cruiseState.enabled, "Cruise control not disabled" + + # run the maneuver + print("- executing maneuver") + for t, msg in m.get_msgs(): + cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] + cs = CI.update([0, cd]) + #assert cs.canValid, f"CAN went invalid, check connections" + + _, can_sends = CI.apply(msg) + #p.can_send_many(can_sends, timeout=20) + + m.log(t, msg, cs) + time.sleep(DT) + + if len(m._log["t"]) > 100: + break + + # ***** write out report ***** + + def plt2html(): + plt.legend() + plt.tight_layout(pad=0) + + buffer = io.BytesIO() + plt.savefig(buffer, format='png') + buffer.seek(0) + return f"\n" + + output_fn = OUTPUT_PATH / f"{CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" + OUTPUT_PATH.mkdir(exist_ok=True) + with open(output_fn, "w") as f: + f.write(f"

Longitudinal maneuver report

\n") + f.write(f"

{CI.CP.carFingerprint}

\n") + if args.desc: + f.write(f"

{args.desc}

") + for m in MANEUVERS: + f.write(f"
\n") + f.write(f"

{m.description}

\n") + + # accel plot + plt.figure(figsize=(12, 4)) + plt.plot(m._log["t"], m._log["carState.aEgo"], label='aEgo') + plt.plot(m._log["t"], m._log["carControl.actuators.accel"], label='actuators.accel') + plt.xlabel('Time (s)') + plt.ylabel('Acceleration (m/s^2)') + plt.ylim(-2.2, 2.2) + plt.title('Acceleration Profile') + plt.grid(True) + f.write(plt2html()) + plt.close() + + # secondary plot + for k in ("carControl.enabled", "carState.cruiseState.enabled"): + plt.rcParams['lines.linewidth'] = 2 + plt.figure(figsize=(12, 1)) + plt.plot(m._log["t"], m._log[k], label=k) + plt.ylim(0.1, 1.1) + # plt.grid(False) + # plt.axis('off') + plt.ylabel(' ') # for alignment + f.write(plt2html()) + plt.close() + + print(f"\nReport written to {output_fn.relative_to(Path(__file__).parent)}\n") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="A tool for longitudinal control testing.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--desc', help="Extra description to include in report.") + args = parser.parse_args() + + main(args) \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 494635e729..945ee631e1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -35,6 +35,7 @@ docs = [ ] examples = [ "inputs", + "matplotlib", ] [tool.pytest.ini_options] From 7cb4b47da350218233d4e31989a4c3854c0c94c5 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 29 Aug 2024 19:14:24 -0700 Subject: [PATCH 02/11] start with creep --- examples/longitudinal-profiles.py | 167 +++++++++++++++++------------- 1 file changed, 96 insertions(+), 71 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index b38a248fbd..27c84d9d59 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -3,150 +3,175 @@ import time import base64 import argparse +import numpy as np import matplotlib.pyplot as plt from enum import Enum from collections import defaultdict from dataclasses import dataclass, asdict from pathlib import Path -from opendbc.car.structs import CarControl, CarState +from opendbc.car.structs import CarControl from opendbc.car.can_definitions import CanData from opendbc.car.panda_runner import PandaRunner DT = 0.01 # step time (s) -OUTPUT_PATH = Path(__file__).resolve().parent / "longitudinal_reports" class Setup(Enum): STOPPED = 0 STEADY_STATE_SPEED = 1 +@dataclass +class Action: + accel: float # m/s^2 + duration: float # seconds + longControlState: CarControl.Actuators.LongControlState + + def get_msgs(self): + return [ + (t, CarControl( + enabled=True, + longActive=True, + actuators=CarControl.Actuators( + accel=self.accel, + longControlState=self.longControlState, + ), + )) + for t in np.linspace(0, self.duration, int(self.duration/DT)) + ] + @dataclass class Maneuver: description: str setup: Setup # initial state - - _log = defaultdict(list) - - def get_cc(self, t: float) -> CarControl: - CC = CarControl( - enabled=True, - latActive=False, - longActive=True, - - actuators=CarControl.Actuators( - gas=0., - brake=0., - speed=0., - accel=0., - longControlState=CarControl.Actuators.LongControlState.off, - ), - - cruiseControl=CarControl.CruiseControl( - cancel=False, - resume=False, - override=False, - ), - ) - return CC + actions: list[Action] def get_msgs(self): - for t in range(0, int(1./DT)): - yield t, self.get_cc(t) - - def log(self, t, cc: CarControl, cs: CarState) -> None: - self._log["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(): - self._log[f"{k}.{k2}"].append(v2) + t = 0 + for action in self.actions: + for lt, msg in action.get_msgs(): + t += lt + yield t, msg MANEUVERS = [ - Maneuver( - "start from stop", - Setup.STOPPED, - ), - Maneuver( - "brake step response: 20mph steady state -> -1m/ss", - Setup.STEADY_STATE_SPEED, - ), - Maneuver( - "brake step response: 20mph steady state -> max brake", - Setup.STEADY_STATE_SPEED, - ), - Maneuver( - "gas step response: 20mph steady state -> +1m/ss", - Setup.STEADY_STATE_SPEED, - ), - Maneuver( - "gas step response: 20mph steady state -> max gas", - Setup.STEADY_STATE_SPEED, - ), Maneuver( "creeping: alternate between +1m/ss and -1m/ss", Setup.STOPPED, + [ + Action(1, 1, CarControl.Actuators.LongControlState.pid), + Action(-1, 1, CarControl.Actuators.LongControlState.pid), + Action(1, 1, CarControl.Actuators.LongControlState.pid), + Action(-1, 1, CarControl.Actuators.LongControlState.pid), + Action(1, 1, CarControl.Actuators.LongControlState.pid), + Action(-1, 1, CarControl.Actuators.LongControlState.pid), + ], ), + # Maneuver( + # "brake step response: 20mph steady state -> -1m/ss", + # Setup.STEADY_STATE_SPEED, + # ), + # Maneuver( + # "brake step response: 20mph steady state -> max brake", + # Setup.STEADY_STATE_SPEED, + # ), + # Maneuver( + # "gas step response: 20mph steady state -> +1m/ss", + # Setup.STEADY_STATE_SPEED, + # ), + # Maneuver( + # "gas step response: 20mph steady state -> max gas", + # Setup.STEADY_STATE_SPEED, + # ), ] def main(args): with PandaRunner() as (p, CI): print("\n\n") + logs = {} for i, m in enumerate(MANEUVERS): print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'") + log = defaultdict(list) + logs[m.description] = log + # cleanup and get into a good state print("- setting up") - cs = None - for _ in range(int(3./DT)): + good_cnt = 0 + for _ in range(int(30./DT)): cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] cs = CI.update([0, cd]) + + cc = CarControl(enabled=True) + if m.setup == Setup.STOPPED: + cc.longActive = True + cc.actuators.accel = -1.5 + cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping + good_cnt = (good_cnt+1) if cs.vEgo < 0.1 else 0 + elif m.setup == Setup.STEADY_STATE_SPEED: + cc.longActive = True + cc.actuators.accel = -1.5 + cc.actuators.longControlState = CarControl.Actuators.LongControlState.pid + good_cnt = (good_cnt+1) if 9 < cs.vEgo < 11 else 0 + + break + if good_cnt > (2./DT): + break + _, can_sends = CI.apply(CarControl(enabled=False)) p.can_send_many(can_sends, timeout=1000) time.sleep(DT) - #assert not cs.cruiseState.enabled, "Cruise control not disabled" + else: + print("ERROR: failed to setup ***********") + continue # run the maneuver print("- executing maneuver") - for t, msg in m.get_msgs(): + for t, cc in m.get_msgs(): cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] cs = CI.update([0, cd]) #assert cs.canValid, f"CAN went invalid, check connections" - _, can_sends = CI.apply(msg) + _, can_sends = CI.apply(cc) #p.can_send_many(can_sends, timeout=20) - m.log(t, msg, cs) time.sleep(DT) - if len(m._log["t"]) > 100: - break + log["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(): + log[f"{k}.{k2}"].append(v2) # ***** write out report ***** def plt2html(): plt.legend() plt.tight_layout(pad=0) - buffer = io.BytesIO() plt.savefig(buffer, format='png') buffer.seek(0) return f"\n" - output_fn = OUTPUT_PATH / f"{CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" - OUTPUT_PATH.mkdir(exist_ok=True) + output_path = Path(__file__).resolve().parent / "longitudinal_reports" + #output_fn = output_path / f"{CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" + output_fn = output_path / f"{CI.CP.carFingerprint}.html" + output_path.mkdir(exist_ok=True) with open(output_fn, "w") as f: - f.write(f"

Longitudinal maneuver report

\n") + f.write("

Longitudinal maneuver report

\n") f.write(f"

{CI.CP.carFingerprint}

\n") if args.desc: f.write(f"

{args.desc}

") for m in MANEUVERS: - f.write(f"
\n") + f.write("
\n") f.write(f"

{m.description}

\n") + log = logs[m.description] + # accel plot plt.figure(figsize=(12, 4)) - plt.plot(m._log["t"], m._log["carState.aEgo"], label='aEgo') - plt.plot(m._log["t"], m._log["carControl.actuators.accel"], label='actuators.accel') + plt.plot(log["t"], log["carState.aEgo"], label='aEgo') + plt.plot(log["t"], log["carControl.actuators.accel"], label='actuators.accel') plt.xlabel('Time (s)') plt.ylabel('Acceleration (m/s^2)') plt.ylim(-2.2, 2.2) @@ -159,7 +184,7 @@ def plt2html(): for k in ("carControl.enabled", "carState.cruiseState.enabled"): plt.rcParams['lines.linewidth'] = 2 plt.figure(figsize=(12, 1)) - plt.plot(m._log["t"], m._log[k], label=k) + plt.plot(log["t"], log[k], label=k) plt.ylim(0.1, 1.1) # plt.grid(False) # plt.axis('off') From 490038bf55839370dcbcf2e61eb98df3331a96bb Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 29 Aug 2024 19:18:23 -0700 Subject: [PATCH 03/11] lil cleanup --- examples/longitudinal-profiles.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index 27c84d9d59..2dfb5327b8 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -24,7 +24,7 @@ class Setup(Enum): class Action: accel: float # m/s^2 duration: float # seconds - longControlState: CarControl.Actuators.LongControlState + longControlState: CarControl.Actuators.LongControlState = CarControl.Actuators.LongControlState.pid def get_msgs(self): return [ @@ -57,12 +57,9 @@ def get_msgs(self): "creeping: alternate between +1m/ss and -1m/ss", Setup.STOPPED, [ - Action(1, 1, CarControl.Actuators.LongControlState.pid), - Action(-1, 1, CarControl.Actuators.LongControlState.pid), - Action(1, 1, CarControl.Actuators.LongControlState.pid), - Action(-1, 1, CarControl.Actuators.LongControlState.pid), - Action(1, 1, CarControl.Actuators.LongControlState.pid), - Action(-1, 1, CarControl.Actuators.LongControlState.pid), + Action(1, 1), Action(-1, 1), + Action(1, 1), Action(-1, 1), + Action(1, 1), Action(-1, 1), ], ), # Maneuver( @@ -109,7 +106,7 @@ def main(args): good_cnt = (good_cnt+1) if cs.vEgo < 0.1 else 0 elif m.setup == Setup.STEADY_STATE_SPEED: cc.longActive = True - cc.actuators.accel = -1.5 + cc.actuators.accel = 0.5 # TODO: small pid? cc.actuators.longControlState = CarControl.Actuators.LongControlState.pid good_cnt = (good_cnt+1) if 9 < cs.vEgo < 11 else 0 From 96732db37a4c8f48cf31af2342ad044f5a3867da Mon Sep 17 00:00:00 2001 From: Comma Device Date: Thu, 29 Aug 2024 20:57:55 -0700 Subject: [PATCH 04/11] corolla updates --- examples/longitudinal-profiles.py | 27 +++++++++++++-------------- opendbc/car/panda_runner.py | 3 ++- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index 2dfb5327b8..739b644928 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -96,26 +96,24 @@ def main(args): good_cnt = 0 for _ in range(int(30./DT)): cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] - cs = CI.update([0, cd]) + cs = CI.update([int(time.monotonic()*1e9), cd]) cc = CarControl(enabled=True) if m.setup == Setup.STOPPED: cc.longActive = True cc.actuators.accel = -1.5 cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping - good_cnt = (good_cnt+1) if cs.vEgo < 0.1 else 0 - elif m.setup == Setup.STEADY_STATE_SPEED: - cc.longActive = True - cc.actuators.accel = 0.5 # TODO: small pid? - cc.actuators.longControlState = CarControl.Actuators.LongControlState.pid - good_cnt = (good_cnt+1) if 9 < cs.vEgo < 11 else 0 + good_cnt = (good_cnt+1) if cs.vEgo < 0.1 and cs.cruiseState.enabled and not cs.cruiseState.standstill else 0 + + if not p.health()['controls_allowed']: + cc = CarControl(enabled=False) - break if good_cnt > (2./DT): break - _, can_sends = CI.apply(CarControl(enabled=False)) - p.can_send_many(can_sends, timeout=1000) + _, can_sends = CI.apply(cc) + p.can_send_many(can_sends, timeout=20) + p.send_heartbeat() time.sleep(DT) else: print("ERROR: failed to setup ***********") @@ -125,11 +123,12 @@ def main(args): print("- executing maneuver") for t, cc in m.get_msgs(): cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] - cs = CI.update([0, cd]) - #assert cs.canValid, f"CAN went invalid, check connections" + cs = CI.update([int(time.monotonic()*1e9), cd]) + assert cs.canValid, f"CAN went invalid, check connections" _, can_sends = CI.apply(cc) - #p.can_send_many(can_sends, timeout=20) + p.can_send_many(can_sends, timeout=20) + p.send_heartbeat() time.sleep(DT) @@ -198,4 +197,4 @@ def plt2html(): parser.add_argument('--desc', help="Extra description to include in report.") args = parser.parse_args() - main(args) \ No newline at end of file + main(args) diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index 02fd3ff224..ae78b2f656 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -7,6 +7,7 @@ @contextmanager def PandaRunner(): p = Panda() + p.reset() def _can_recv(wait_for_one: bool = False) -> list[list[CanData]]: recv = p.can_recv() @@ -32,4 +33,4 @@ def _can_recv(wait_for_one: bool = False) -> list[list[CanData]]: if __name__ == "__main__": with PandaRunner() as (p, CI): - print(p.can_recv()) \ No newline at end of file + print(p.can_recv()) From 7c7aaaf50868728a1d9653bdd1ee70f40f28f03e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 29 Aug 2024 21:26:58 -0700 Subject: [PATCH 05/11] cleanup --- examples/longitudinal-profiles.py | 34 ++++++----------- opendbc/car/panda_runner.py | 62 ++++++++++++++++++++----------- 2 files changed, 52 insertions(+), 44 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index 739b644928..93381babc6 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -11,7 +11,6 @@ from pathlib import Path from opendbc.car.structs import CarControl -from opendbc.car.can_definitions import CanData from opendbc.car.panda_runner import PandaRunner DT = 0.01 # step time (s) @@ -81,7 +80,7 @@ def get_msgs(self): ] def main(args): - with PandaRunner() as (p, CI): + with PandaRunner() as p: print("\n\n") logs = {} @@ -95,8 +94,7 @@ def main(args): print("- setting up") good_cnt = 0 for _ in range(int(30./DT)): - cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] - cs = CI.update([int(time.monotonic()*1e9), cd]) + cs = p.read() cc = CarControl(enabled=True) if m.setup == Setup.STOPPED: @@ -105,32 +103,22 @@ def main(args): cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping good_cnt = (good_cnt+1) if cs.vEgo < 0.1 and cs.cruiseState.enabled and not cs.cruiseState.standstill else 0 - if not p.health()['controls_allowed']: + if not p.panda.health()['controls_allowed']: cc = CarControl(enabled=False) + p.write(cc) if good_cnt > (2./DT): break - - _, can_sends = CI.apply(cc) - p.can_send_many(can_sends, timeout=20) - p.send_heartbeat() time.sleep(DT) else: - print("ERROR: failed to setup ***********") + print("ERROR: failed to setup") continue # run the maneuver print("- executing maneuver") for t, cc in m.get_msgs(): - cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] - cs = CI.update([int(time.monotonic()*1e9), cd]) - assert cs.canValid, f"CAN went invalid, check connections" - - _, can_sends = CI.apply(cc) - p.can_send_many(can_sends, timeout=20) - p.send_heartbeat() - - time.sleep(DT) + cs = p.read() + p.send(cc) log["t"].append(t) to_log = {"carControl": cc, "carState": cs, "carControl.actuators": cc.actuators, @@ -139,6 +127,8 @@ def main(args): for k2, v2 in asdict(v).items(): log[f"{k}.{k2}"].append(v2) + time.sleep(DT) + # ***** write out report ***** def plt2html(): @@ -150,12 +140,12 @@ def plt2html(): return f"\n" output_path = Path(__file__).resolve().parent / "longitudinal_reports" - #output_fn = output_path / f"{CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" - output_fn = output_path / f"{CI.CP.carFingerprint}.html" + #output_fn = output_path / f"{p.CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" + output_fn = output_path / f"{p.CI.CP.carFingerprint}.html" output_path.mkdir(exist_ok=True) with open(output_fn, "w") as f: f.write("

Longitudinal maneuver report

\n") - f.write(f"

{CI.CP.carFingerprint}

\n") + f.write(f"

{p.CI.CP.carFingerprint}

\n") if args.desc: f.write(f"

{args.desc}

") for m in MANEUVERS: diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index ae78b2f656..c86b1099bd 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -1,36 +1,54 @@ -from contextlib import contextmanager +import time +from contextlib import AbstractContextManager +from opendbc_repo.opendbc.car.structs import CarState, CarControl from panda import Panda from opendbc.car.car_helpers import get_car from opendbc.car.can_definitions import CanData -@contextmanager -def PandaRunner(): - p = Panda() - p.reset() +class PandaRunner(AbstractContextManager): + def __enter__(self): + self.p = Panda() + self.p.reset() - def _can_recv(wait_for_one: bool = False) -> list[list[CanData]]: - recv = p.can_recv() - while len(recv) == 0 and wait_for_one: - recv = p.can_recv() - return [[CanData(addr, dat, bus) for addr, dat, bus in recv], ] - - try: # setup + fingerprinting p.set_safety_mode(Panda.SAFETY_ELM327, 1) - CI = get_car(_can_recv, p.can_send_many, p.set_obd, True) - print("fingerprinted", CI.CP.carName) - assert CI.CP.carFingerprint != "mock", "Unable to identify car. Check connections and ensure car is supported." + self.CI = get_car(self._can_recv, p.can_send_many, p.set_obd, True) + print("fingerprinted", self.CI.CP.carName) + assert self.CI.CP.carFingerprint != "mock", "Unable to identify car. Check connections and ensure car is supported." p.set_safety_mode(Panda.SAFETY_ELM327, 1) - CI.init(CI.CP, _can_recv, p.can_send_many) - p.set_safety_mode(Panda.SAFETY_TOYOTA, CI.CP.safetyConfigs[0].safetyParam) + self.CI.init(self.CI.CP, self._can_recv, p.can_send_many) + p.set_safety_mode(Panda.SAFETY_TOYOTA, self.CI.CP.safetyConfigs[0].safetyParam) + + return self + + def __exit__(self, exc_type, exc_value, traceback): + self.p.set_safety_mode(Panda.SAFETY_NOOUTPUT) + self.p.reset() # avoid siren + return super().__exit__(exc_type, exc_value, traceback) + + @property + def panda(self) -> Panda: + return self.p + + def _can_recv(self, wait_for_one: bool = False) -> list[list[CanData]]: + recv = self.p.can_recv() + while len(recv) == 0 and wait_for_one: + recv = self.p.can_recv() + return [[CanData(addr, dat, bus) for addr, dat, bus in recv], ] - yield p, CI - finally: - p.set_safety_mode(Panda.SAFETY_NOOUTPUT) + def read(self) -> CarState: + cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] + cs = self.CI.update([int(time.monotonic()*1e9), cd]) + assert cs.canValid, "CAN went invalid, check connections" + return cs + def write(self, cc: CarControl) -> None: + _, can_sends = self.CI.apply(cc) + p.can_send_many(can_sends, timeout=20) + p.send_heartbeat() if __name__ == "__main__": - with PandaRunner() as (p, CI): - print(p.can_recv()) + with PandaRunner() as p: + print(p.read()) \ No newline at end of file From ddd38d84bf1eb20ceb10d1f4443c62a83bb805d9 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 29 Aug 2024 21:32:43 -0700 Subject: [PATCH 06/11] 2s --- examples/longitudinal-profiles.py | 22 +++------------------- 1 file changed, 3 insertions(+), 19 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index 93381babc6..cdd5fabe1d 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -56,27 +56,11 @@ def get_msgs(self): "creeping: alternate between +1m/ss and -1m/ss", Setup.STOPPED, [ - Action(1, 1), Action(-1, 1), - Action(1, 1), Action(-1, 1), - Action(1, 1), Action(-1, 1), + Action(1, 2), Action(-1, 2), + Action(1, 2), Action(-1, 2), + Action(1, 2), Action(-1, 2), ], ), - # Maneuver( - # "brake step response: 20mph steady state -> -1m/ss", - # Setup.STEADY_STATE_SPEED, - # ), - # Maneuver( - # "brake step response: 20mph steady state -> max brake", - # Setup.STEADY_STATE_SPEED, - # ), - # Maneuver( - # "gas step response: 20mph steady state -> +1m/ss", - # Setup.STEADY_STATE_SPEED, - # ), - # Maneuver( - # "gas step response: 20mph steady state -> max gas", - # Setup.STEADY_STATE_SPEED, - # ), ] def main(args): From e8c135c975212805ac077b1cbd532a48ef9e779d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 Aug 2024 12:03:06 -0700 Subject: [PATCH 07/11] plot is a little nicer --- .gitignore | 3 +- examples/longitudinal-profiles.py | 74 +++++++++++++++---------------- opendbc/car/panda_runner.py | 21 +++++---- 3 files changed, 46 insertions(+), 52 deletions(-) diff --git a/.gitignore b/.gitignore index 767434eb72..d66bd7dcb5 100644 --- a/.gitignore +++ b/.gitignore @@ -10,10 +10,9 @@ .sconsign.dblite .hypothesis *.egg-info/ +*.html uv.lock -/examples/longitudinal_reports/ - opendbc/can/*.so opendbc/can/*.a opendbc/can/build/ diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index cdd5fabe1d..f2f0775d5b 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -45,11 +45,11 @@ class Maneuver: actions: list[Action] def get_msgs(self): - t = 0 + t0 = 0 for action in self.actions: for lt, msg in action.get_msgs(): - t += lt - yield t, msg + yield lt + t0, msg + t0 += lt MANEUVERS = [ Maneuver( @@ -102,7 +102,7 @@ def main(args): print("- executing maneuver") for t, cc in m.get_msgs(): cs = p.read() - p.send(cc) + p.write(cc) log["t"].append(t) to_log = {"carControl": cc, "carState": cs, "carControl.actuators": cc.actuators, @@ -115,17 +115,8 @@ def main(args): # ***** write out report ***** - def plt2html(): - plt.legend() - plt.tight_layout(pad=0) - buffer = io.BytesIO() - plt.savefig(buffer, format='png') - buffer.seek(0) - return f"\n" - output_path = Path(__file__).resolve().parent / "longitudinal_reports" - #output_fn = output_path / f"{p.CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" - output_fn = output_path / f"{p.CI.CP.carFingerprint}.html" + output_fn = args.output or output_path / f"{p.CI.CP.carFingerprint}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" output_path.mkdir(exist_ok=True) with open(output_fn, "w") as f: f.write("

Longitudinal maneuver report

\n") @@ -138,37 +129,42 @@ def plt2html(): log = logs[m.description] - # accel plot - plt.figure(figsize=(12, 4)) - plt.plot(log["t"], log["carState.aEgo"], label='aEgo') - plt.plot(log["t"], log["carControl.actuators.accel"], label='actuators.accel') - plt.xlabel('Time (s)') - plt.ylabel('Acceleration (m/s^2)') - plt.ylim(-2.2, 2.2) - plt.title('Acceleration Profile') - plt.grid(True) - f.write(plt2html()) - plt.close() - - # secondary plot - for k in ("carControl.enabled", "carState.cruiseState.enabled"): - plt.rcParams['lines.linewidth'] = 2 - plt.figure(figsize=(12, 1)) - plt.plot(log["t"], log[k], label=k) - plt.ylim(0.1, 1.1) - # plt.grid(False) - # plt.axis('off') - plt.ylabel(' ') # for alignment - f.write(plt2html()) - plt.close() - - print(f"\nReport written to {output_fn.relative_to(Path(__file__).parent)}\n") + plt.rcParams['font.size'] = 40 + fig = plt.figure(figsize=(30, 20)) + ax = fig.subplots(3, 1, sharex=True, gridspec_kw={'hspace': 0, 'height_ratios': [5, 1, 1]}) + + ax[0].grid(linewidth=4) + ax[0].plot(log["t"], log["carState.aEgo"], label='aEgo', linewidth=6) + ax[0].plot(log["t"], log["carControl.actuators.accel"], label='accel command', linewidth=6) + ax[0].set_ylabel('Acceleration (m/s^2)') + ax[0].set_ylim(-4.5, 4.5) + ax[0].legend() + + ax[1].plot(log["t"], log["carControl.enabled"], label='enabled', linewidth=6) + ax[2].plot(log["t"], log["carState.gasPressed"], label='gasPressed', linewidth=6) + for i in (1, 2): + ax[i].set_yticks([0, 1], minor=False) + ax[i].set_ylim(-1, 2) + ax[i].legend() + + ax[-1].set_xlabel("Time (s)") + fig.tight_layout() + + buffer = io.BytesIO() + fig.savefig(buffer, format='png') + buffer.seek(0) + f.write(f"\n") + + print(f"\nReport written to {output_fn}\n") if __name__ == "__main__": parser = argparse.ArgumentParser(description="A tool for longitudinal control testing.", formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--desc', help="Extra description to include in report.") + parser.add_argument('--output', help="Write out report to this file.", default=None) args = parser.parse_args() + assert args.output is None or args.output.endswith(".html"), f"Output filename must end with '.html'" + main(args) diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index c86b1099bd..fb52c0f91f 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -1,10 +1,10 @@ import time from contextlib import AbstractContextManager -from opendbc_repo.opendbc.car.structs import CarState, CarControl from panda import Panda from opendbc.car.car_helpers import get_car from opendbc.car.can_definitions import CanData +from opendbc.car.structs import CarState, CarControl class PandaRunner(AbstractContextManager): def __enter__(self): @@ -12,14 +12,14 @@ def __enter__(self): self.p.reset() # setup + fingerprinting - p.set_safety_mode(Panda.SAFETY_ELM327, 1) - self.CI = get_car(self._can_recv, p.can_send_many, p.set_obd, True) + self.p.set_safety_mode(Panda.SAFETY_ELM327, 1) + self.CI = get_car(self._can_recv, self.p.can_send_many, self.p.set_obd, True) print("fingerprinted", self.CI.CP.carName) assert self.CI.CP.carFingerprint != "mock", "Unable to identify car. Check connections and ensure car is supported." - p.set_safety_mode(Panda.SAFETY_ELM327, 1) - self.CI.init(self.CI.CP, self._can_recv, p.can_send_many) - p.set_safety_mode(Panda.SAFETY_TOYOTA, self.CI.CP.safetyConfigs[0].safetyParam) + self.p.set_safety_mode(Panda.SAFETY_ELM327, 1) + self.CI.init(self.CI.CP, self._can_recv, self.p.can_send_many) + self.p.set_safety_mode(Panda.SAFETY_TOYOTA, self.CI.CP.safetyConfigs[0].safetyParam) return self @@ -39,15 +39,14 @@ def _can_recv(self, wait_for_one: bool = False) -> list[list[CanData]]: return [[CanData(addr, dat, bus) for addr, dat, bus in recv], ] def read(self) -> CarState: - cd = [CanData(addr, dat, bus) for addr, dat, bus in p.can_recv()] - cs = self.CI.update([int(time.monotonic()*1e9), cd]) - assert cs.canValid, "CAN went invalid, check connections" + cs = self.CI.update([int(time.monotonic()*1e9), self._can_recv()[0]]) + #assert cs.canValid, "CAN went invalid, check connections" return cs def write(self, cc: CarControl) -> None: _, can_sends = self.CI.apply(cc) - p.can_send_many(can_sends, timeout=20) - p.send_heartbeat() + self.p.can_send_many(can_sends, timeout=25) + self.p.send_heartbeat() if __name__ == "__main__": with PandaRunner() as p: From 21ce078a112ce6322a3c586adadde1b28461f368 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Fri, 30 Aug 2024 13:26:41 -0700 Subject: [PATCH 08/11] strict mode --- examples/longitudinal-profiles.py | 2 +- opendbc/car/panda_runner.py | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index f2f0775d5b..3a9634eb86 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -78,7 +78,7 @@ def main(args): print("- setting up") good_cnt = 0 for _ in range(int(30./DT)): - cs = p.read() + cs = p.read(strict=False) cc = CarControl(enabled=True) if m.setup == Setup.STOPPED: diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index fb52c0f91f..1ccf331856 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -38,9 +38,10 @@ def _can_recv(self, wait_for_one: bool = False) -> list[list[CanData]]: recv = self.p.can_recv() return [[CanData(addr, dat, bus) for addr, dat, bus in recv], ] - def read(self) -> CarState: + def read(self, strict: bool = True) -> CarState: cs = self.CI.update([int(time.monotonic()*1e9), self._can_recv()[0]]) - #assert cs.canValid, "CAN went invalid, check connections" + if strict: + assert cs.canValid, "CAN went invalid, check connections" return cs def write(self, cc: CarControl) -> None: @@ -50,4 +51,4 @@ def write(self, cc: CarControl) -> None: if __name__ == "__main__": with PandaRunner() as p: - print(p.read()) \ No newline at end of file + print(p.read()) From d44247ff4c5220ba95a0e42d55e3bf01a1f2d327 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 Aug 2024 13:35:50 -0700 Subject: [PATCH 09/11] cleanup --- examples/longitudinal-profiles.py | 36 ++++++++++--------------------- opendbc/car/panda_runner.py | 5 ++++- 2 files changed, 15 insertions(+), 26 deletions(-) diff --git a/examples/longitudinal-profiles.py b/examples/longitudinal-profiles.py index 3a9634eb86..2e7337441d 100755 --- a/examples/longitudinal-profiles.py +++ b/examples/longitudinal-profiles.py @@ -5,7 +5,6 @@ import argparse import numpy as np import matplotlib.pyplot as plt -from enum import Enum from collections import defaultdict from dataclasses import dataclass, asdict from pathlib import Path @@ -15,10 +14,6 @@ DT = 0.01 # step time (s) -class Setup(Enum): - STOPPED = 0 - STEADY_STATE_SPEED = 1 - @dataclass class Action: accel: float # m/s^2 @@ -41,7 +36,6 @@ def get_msgs(self): @dataclass class Maneuver: description: str - setup: Setup # initial state actions: list[Action] def get_msgs(self): @@ -54,7 +48,6 @@ def get_msgs(self): MANEUVERS = [ Maneuver( "creeping: alternate between +1m/ss and -1m/ss", - Setup.STOPPED, [ Action(1, 2), Action(-1, 2), Action(1, 2), Action(-1, 2), @@ -71,26 +64,19 @@ def main(args): for i, m in enumerate(MANEUVERS): print(f"Running {i+1}/{len(MANEUVERS)} '{m.description}'") - log = defaultdict(list) - logs[m.description] = log - - # cleanup and get into a good state print("- setting up") good_cnt = 0 for _ in range(int(30./DT)): cs = p.read(strict=False) - cc = CarControl(enabled=True) - if m.setup == Setup.STOPPED: - cc.longActive = True - cc.actuators.accel = -1.5 - cc.actuators.longControlState = CarControl.Actuators.LongControlState.stopping - good_cnt = (good_cnt+1) if cs.vEgo < 0.1 and cs.cruiseState.enabled and not cs.cruiseState.standstill else 0 - - if not p.panda.health()['controls_allowed']: - cc = CarControl(enabled=False) + cc = CarControl( + enabled=True, + longActive=True, + actuators=CarControl.Actuators(accel=-1.5, longControlState=CarControl.Actuators.LongControlState.stopping), + ) p.write(cc) + good_cnt = (good_cnt+1) if cs.vEgo < 0.1 and cs.cruiseState.enabled and not cs.cruiseState.standstill else 0 if good_cnt > (2./DT): break time.sleep(DT) @@ -98,18 +84,18 @@ def main(args): print("ERROR: failed to setup") continue - # run the maneuver print("- executing maneuver") + logs[m.description] = defaultdict(list) for t, cc in m.get_msgs(): cs = p.read() p.write(cc) - log["t"].append(t) + logs[m.description]["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(): - log[f"{k}.{k2}"].append(v2) + logs[m.description][f"{k}.{k2}"].append(v2) time.sleep(DT) @@ -165,6 +151,6 @@ def main(args): parser.add_argument('--output', help="Write out report to this file.", default=None) args = parser.parse_args() - assert args.output is None or args.output.endswith(".html"), f"Output filename must end with '.html'" + assert args.output is None or args.output.endswith(".html"), "Output filename must end with '.html'" - main(args) + main(args) \ No newline at end of file diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index 1ccf331856..ff4216c957 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -38,13 +38,16 @@ def _can_recv(self, wait_for_one: bool = False) -> list[list[CanData]]: recv = self.p.can_recv() return [[CanData(addr, dat, bus) for addr, dat, bus in recv], ] - def read(self, strict: bool = True) -> CarState: + def read(self, strict: bool = True): cs = self.CI.update([int(time.monotonic()*1e9), self._can_recv()[0]]) if strict: assert cs.canValid, "CAN went invalid, check connections" return cs def write(self, cc: CarControl) -> None: + if cc.enabled and not p.panda.health()['controls_allowed']: + # prevent the car from faulting. print a warning? + cc = CarControl(enabled=False) _, can_sends = self.CI.apply(cc) self.p.can_send_many(can_sends, timeout=25) self.p.send_heartbeat() From 8d89aa68e973ac45e584cfba288f3b03c136ad74 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 Aug 2024 13:36:43 -0700 Subject: [PATCH 10/11] unused --- opendbc/car/panda_runner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index ff4216c957..a34bf32fd2 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -4,7 +4,7 @@ from panda import Panda from opendbc.car.car_helpers import get_car from opendbc.car.can_definitions import CanData -from opendbc.car.structs import CarState, CarControl +from opendbc.car.structs import CarControl class PandaRunner(AbstractContextManager): def __enter__(self): From 136b4fb80e1e9e7262b9d96b2a1488dc597ab90a Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 Aug 2024 13:46:40 -0700 Subject: [PATCH 11/11] fix that --- opendbc/car/panda_runner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/panda_runner.py b/opendbc/car/panda_runner.py index a34bf32fd2..2e6221decb 100644 --- a/opendbc/car/panda_runner.py +++ b/opendbc/car/panda_runner.py @@ -45,7 +45,7 @@ def read(self, strict: bool = True): return cs def write(self, cc: CarControl) -> None: - if cc.enabled and not p.panda.health()['controls_allowed']: + if cc.enabled and not self.p.health()['controls_allowed']: # prevent the car from faulting. print a warning? cc = CarControl(enabled=False) _, can_sends = self.CI.apply(cc)