diff --git a/common/params.cc b/common/params.cc index c75a09e28b..ba90826c2c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -158,7 +158,7 @@ std::unordered_map keys = { {"LiveParameters", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, {"LocationFilterInitialState", PERSISTENT}, - {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"LongitudinalManeuverMode", PERSISTENT}, {"LongitudinalPersonality", PERSISTENT}, {"NetworkMetered", PERSISTENT}, {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, diff --git a/system/manager/process_config.py b/system/manager/process_config.py index fff9ec7a02..95f7a9a5c0 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -77,7 +77,7 @@ procs = [ PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", not_joystick), PythonProcess("joystickd", "tools.joystick.joystickd", joystick), - PythonProcess("longitudinal_profilesd", "examples.longitudinal_profiles", long_maneuver), + PythonProcess("maneuversd", "examples.maneuversd", long_maneuver), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py new file mode 100644 index 0000000000..354d3bab34 --- /dev/null +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -0,0 +1,61 @@ +import io +import os +import time +import base64 +import argparse +import numpy as np +import matplotlib.pyplot as plt +from collections import defaultdict +from dataclasses import dataclass, asdict +from pathlib import Path + + +def report(args, logs, fp): + output_path = Path(__file__).resolve().parent / "longitudinal_reports" + output_fn = args.output or output_path / f"{fp}_{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") + f.write(f"

{fp}

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

{args.desc}

") + for description, runs in logs.items(): + f.write("
\n") + f.write(f"

{description}

\n") + for run, log in runs.items(): + f.write(f"

Run #{int(run)+1}

\n") + plt.rcParams['font.size'] = 40 + fig = plt.figure(figsize=(30, 25)) + ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'hspace': 0, 'height_ratios': [5, 3, 1, 1]}) + + ax[0].grid(linewidth=4) + ax[0].plot(log["t"], log["carControl.actuators.accel"], label='accel command', linewidth=6) + ax[0].plot(log["t"], log["carState.aEgo"], label='aEgo', linewidth=6) + ax[0].set_ylabel('Acceleration (m/s^2)') + #ax[0].set_ylim(-6.5, 6.5) + ax[0].legend() + + ax[1].grid(linewidth=4) + ax[1].plot(log["t"], log["carState.vEgo"], 'g', label='vEgo', linewidth=6) + ax[1].set_ylabel('Velocity (m/s)') + ax[1].legend() + + ax[2].plot(log["t"], log["carControl.enabled"], label='enabled', linewidth=6) + ax[3].plot(log["t"], log["carState.gasPressed"], label='gasPressed', linewidth=6) + ax[3].plot(log["t"], log["carState.brakePressed"], label='brakePressed', linewidth=6) + for i in (2, 3): + 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") + + import json + f.write(f"

{json.dumps(logs)}

") + print(f"\nReport written to {output_fn}\n") diff --git a/examples/longitudinal_profiles.py b/tools/longitudinal_maneuvers/maneuversd.py similarity index 51% rename from examples/longitudinal_profiles.py rename to tools/longitudinal_maneuvers/maneuversd.py index c0f0910d3e..680589e0d5 100755 --- a/examples/longitudinal_profiles.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -1,27 +1,11 @@ #!/usr/bin/env python3 -import io -import os -import time -import base64 -import argparse -import numpy as np -import matplotlib.pyplot as plt -from collections import defaultdict -from dataclasses import dataclass, asdict -from pathlib import Path +from dataclasses import dataclass from cereal import messaging, car -from opendbc.car.structs import CarControl from opendbc.car.common.conversions import Conversions -from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper +from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan - -# TODOs -# - support lateral maneuvers -# - setup: show countdown? @dataclass @@ -63,7 +47,7 @@ class Maneuver: # next action if self._action_index < len(self.actions) - 1: self._action_index += 1 - self._action_frames = 0 # TODO rename action frames? + self._action_frames = 0 # repeat maneuver elif self._repeated < self.repeat: self._repeated += 1 @@ -123,57 +107,6 @@ MANEUVERS = [ ] -def report(args, logs, fp): - output_path = Path(__file__).resolve().parent / "longitudinal_reports" - output_fn = args.output or output_path / f"{fp}_{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") - f.write(f"

{fp}

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

{args.desc}

") - for description, runs in logs.items(): - f.write("
\n") - f.write(f"

{description}

\n") - for run, log in runs.items(): - f.write(f"

Run #{int(run)+1}

\n") - plt.rcParams['font.size'] = 40 - fig = plt.figure(figsize=(30, 25)) - ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'hspace': 0, 'height_ratios': [5, 3, 1, 1]}) - - ax[0].grid(linewidth=4) - ax[0].plot(log["t"], log["carControl.actuators.accel"], label='accel command', linewidth=6) - ax[0].plot(log["t"], log["carState.aEgo"], label='aEgo', linewidth=6) - ax[0].set_ylabel('Acceleration (m/s^2)') - #ax[0].set_ylim(-6.5, 6.5) - ax[0].legend() - - ax[1].grid(linewidth=4) - ax[1].plot(log["t"], log["carState.vEgo"], 'g', label='vEgo', linewidth=6) - ax[1].set_ylabel('Velocity (m/s)') - ax[1].legend() - - ax[2].plot(log["t"], log["carControl.enabled"], label='enabled', linewidth=6) - ax[3].plot(log["t"], log["carState.gasPressed"], label='gasPressed', linewidth=6) - ax[3].plot(log["t"], log["carState.brakePressed"], label='brakePressed', linewidth=6) - for i in (2, 3): - 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") - - import json - f.write(f"

{json.dumps(logs)}

") - print(f"\nReport written to {output_fn}\n") - - def main(): params = Params() cloudlog.info("joystickd is waiting for CarParams") @@ -226,34 +159,9 @@ def main(): assistance_send.valid = True pm.send('driverAssistance', assistance_send) - print('finished?', maneuver is not None and maneuver.finished) - print('aTarget:', longitudinalPlan.aTarget) - print('shouldStop:', longitudinalPlan.shouldStop) + # print('finished?', maneuver is not None and maneuver.finished) + # print('aTarget:', longitudinalPlan.aTarget) + # print('shouldStop:', longitudinalPlan.shouldStop) if maneuver is not None and maneuver.finished: maneuver = None - - -if __name__ == "__main__": - main() - exit() - - maneuver_help = "\n".join([f"{i+1}. {m.description}" for i, m in enumerate(MANEUVERS)]) - parser = argparse.ArgumentParser(description="A tool for longitudinal control testing.", - formatter_class=argparse.RawTextHelpFormatter) - parser.add_argument('--desc', help="Extra description to include in report.") - parser.add_argument('--output', help="Write out report to this file.", default=None) - parser.add_argument('maneuvers', nargs='*', type=int, default=None, help=f'Deafult is all.\n{maneuver_help}') - args = parser.parse_args() - print(args) - - if "REPORT_TEST" in os.environ: - with open(os.environ["REPORT_TEST"]) as f: - import json - logs = json.loads(f.read().split("none'>")[1].split('

')[0]) - report(args, logs, "testing") - exit() - - assert args.output is None or args.output.endswith(".html"), "Output filename must end with '.html'" - - main(args)