From 1693532d00454b9cacf5833290abce6dbed0d70c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 8 Mar 2026 16:28:12 -0500 Subject: [PATCH] Backport longitudinal maneuvers tooling and runtime hooks --- common/params.cc | 3 + selfdrive/controls/controlsd.py | 70 +++ system/manager/process_config.py | 9 +- tools/longitudinal_maneuvers/.gitignore | 1 + tools/longitudinal_maneuvers/README.md | 56 ++ tools/longitudinal_maneuvers/__init__.py | 0 .../longitudinal_maneuvers/generate_report.py | 309 +++++++++++ .../maneuver_helpers.py | 18 + tools/longitudinal_maneuvers/maneuversd.py | 493 ++++++++++++++++++ .../mpc_longitudinal_tuning_report.py | 292 +++++++++++ 10 files changed, 1250 insertions(+), 1 deletion(-) create mode 100644 tools/longitudinal_maneuvers/.gitignore create mode 100644 tools/longitudinal_maneuvers/README.md create mode 100644 tools/longitudinal_maneuvers/__init__.py create mode 100755 tools/longitudinal_maneuvers/generate_report.py create mode 100644 tools/longitudinal_maneuvers/maneuver_helpers.py create mode 100755 tools/longitudinal_maneuvers/maneuversd.py create mode 100644 tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py diff --git a/common/params.cc b/common/params.cc index a444fd2d4..27cf76d96 100644 --- a/common/params.cc +++ b/common/params.cc @@ -164,6 +164,9 @@ std::unordered_map keys = { {"LiveDelay", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, + {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"LongitudinalManeuverPaddleMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"LongitudinalManeuverStatus", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"LongitudinalPersonality", PERSISTENT}, {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1d9d97779..a6e46d999 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import json import os import math import time @@ -199,6 +200,12 @@ class Controls: self.event_names_to_clear = set() + self.long_maneuver_popup_show = False + self.long_maneuver_popup_text1 = "" + self.long_maneuver_popup_text2 = "" + self.long_maneuver_popup_size = "small" + self.long_maneuver_popup_status = "normal" + self.has_menu = self.CP.carName == "gm" and not (self.CP.flags & GMFlags.NO_CAMERA.value) self.frogpilot_AM = AlertManager() @@ -968,6 +975,8 @@ class Controls: frogpilotControlsState.alertBlinkingRate = current_frogpilot_alert.alert_rate frogpilotControlsState.alertType = current_frogpilot_alert.alert_type frogpilotControlsState.alertSound = current_frogpilot_alert.audible_alert + else: + self.apply_long_maneuver_popup(frogpilotControlsState) self.pm.send('frogpilotControlsState', frogpilot_dat) @@ -999,12 +1008,73 @@ class Controls: except (ValueError, TypeError): return log.LongitudinalPersonality.standard + def update_long_maneuver_popup_state(self): + raw_status = self.params.get("LongitudinalManeuverStatus", encoding="utf-8") or "" + status = {} + if raw_status: + try: + parsed = json.loads(raw_status) + if isinstance(parsed, dict): + status = parsed + except Exception: + status = {} + + ui_text_1 = str(status.get("uiText1") or "").strip() + ui_text_2 = str(status.get("uiText2") or "").strip() + ui_size = str(status.get("uiSize") or "small").strip().lower() + ui_status = str(status.get("uiStatus") or "normal").strip().lower() + mode_enabled = self.params.get_bool("LongitudinalManeuverMode") + state = str(status.get("state") or "").strip().lower() + + updated_at = 0.0 + try: + updated_at = float(status.get("updatedAtSec") or 0.0) + except Exception: + updated_at = 0.0 + status_age = max(0.0, time.time() - updated_at) if updated_at > 0.0 else 9999.0 + + is_recent_finished = state == "finished" and status_age <= 20.0 + ui_show = bool(status.get("uiShow", False)) + show_popup = bool((mode_enabled and ui_show and (ui_text_1 or ui_text_2)) or (is_recent_finished and (ui_text_1 or ui_text_2))) + + self.long_maneuver_popup_show = show_popup + self.long_maneuver_popup_text1 = ui_text_1 or "Long Maneuvers" + self.long_maneuver_popup_text2 = ui_text_2 + self.long_maneuver_popup_size = ui_size if ui_size in ("small", "mid", "full") else "small" + self.long_maneuver_popup_status = ui_status if ui_status in ("normal", "userprompt", "critical", "frogpilot") else "normal" + + def apply_long_maneuver_popup(self, frogpilotControlsState): + if not self.long_maneuver_popup_show: + return False + + size_map = { + "small": custom.FrogPilotControlsState.AlertSize.small, + "mid": custom.FrogPilotControlsState.AlertSize.mid, + "full": custom.FrogPilotControlsState.AlertSize.full, + } + status_map = { + "normal": custom.FrogPilotControlsState.AlertStatus.normal, + "userprompt": custom.FrogPilotControlsState.AlertStatus.userPrompt, + "critical": custom.FrogPilotControlsState.AlertStatus.critical, + "frogpilot": custom.FrogPilotControlsState.AlertStatus.frogpilot, + } + + frogpilotControlsState.alertText1 = self.long_maneuver_popup_text1 + frogpilotControlsState.alertText2 = self.long_maneuver_popup_text2 + frogpilotControlsState.alertSize = size_map.get(self.long_maneuver_popup_size, custom.FrogPilotControlsState.AlertSize.small) + frogpilotControlsState.alertStatus = status_map.get(self.long_maneuver_popup_status, custom.FrogPilotControlsState.AlertStatus.normal) + frogpilotControlsState.alertBlinkingRate = 0. + frogpilotControlsState.alertType = "longitudinalManeuverStatus" + frogpilotControlsState.alertSound = car.CarControl.HUDControl.AudibleAlert.none + return True + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") if not (self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental_mode): self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.personality = self.read_personality_param() + self.update_long_maneuver_popup_state() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 66060cb80..d8d4a1919 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -41,6 +41,12 @@ def only_onroad(started: bool, params, CP: car.CarParams, classic_model, tinygra def only_offroad(started, params, CP: car.CarParams, classic_model, tinygrad_model, frogpilot_toggles) -> bool: return not started +def long_maneuver(started, params, CP: car.CarParams, classic_model, tinygrad_model, frogpilot_toggles) -> bool: + return started and params.get_bool("LongitudinalManeuverMode") + +def not_long_maneuver(started, params, CP: car.CarParams, classic_model, tinygrad_model, frogpilot_toggles) -> bool: + return started and not params.get_bool("LongitudinalManeuverMode") + # FrogPilot functions def allow_logging(started, params, CP: car.CarParams, classic_model, tinygrad_model, frogpilot_toggles) -> bool: return not frogpilot_toggles.no_logging and logging(started, params, CP, classic_model, tinygrad_model, frogpilot_toggles) @@ -96,7 +102,8 @@ procs = [ PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), - PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad), + PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver), + PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver), PythonProcess("radard", "selfdrive.controls.radard", only_onroad), PythonProcess("hardwared", "system.hardware.hardwared", always_run), PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC), diff --git a/tools/longitudinal_maneuvers/.gitignore b/tools/longitudinal_maneuvers/.gitignore new file mode 100644 index 000000000..e819fa626 --- /dev/null +++ b/tools/longitudinal_maneuvers/.gitignore @@ -0,0 +1 @@ +/longitudinal_reports/ diff --git a/tools/longitudinal_maneuvers/README.md b/tools/longitudinal_maneuvers/README.md new file mode 100644 index 000000000..dc88d2727 --- /dev/null +++ b/tools/longitudinal_maneuvers/README.md @@ -0,0 +1,56 @@ +# Longitudinal Maneuvers Testing Tool + +Test your vehicle's longitudinal control tuning with this tool. The tool will test the vehicle's ability to follow a few longitudinal maneuvers and includes a tool to generate a report from the route. + +
Sample snapshot of a report.
+ +## Instructions + +1. Check out a development branch such as `master` on your comma device. +2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage openpilot between maneuvers and turn around if there is not enough space. +3. Arm maneuver mode from The Pond (`Tools -> Long Maneuvers -> Start / Arm`) or by setting this parameter manually: + + ```sh + echo -n 1 > /data/params/d/LongitudinalManeuverMode + ``` + +4. Turn your vehicle back on. This FrogPilot branch now shows on-road popup status for maneuver progress. You can still verify markers from logs with: + + ```sh + python selfdrive/debug/filter_log_message.py --level WARNING + ``` + You should see `LONG_MANEUVER_MODE|state=STARTED` and maneuver start/end markers. + For GM pedal-long cars, you should also see phase markers: + - `LONG_MANEUVER_PHASE|name=pedal_only|paddleMode=off` + - `LONG_MANEUVER_PHASE|name=pedal_plus_paddle|paddleMode=force` + +5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. The tests will run for about 4 minutes. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel. + + **Note:** For GM cars, it is recommended to hold down the resume button for all low-speed tests (starting, stopping and creep) to avoid the car entering standstill. + **Note:** For GM pedal-long cars (pedal interceptor + regen paddle), the maneuver suite automatically runs A/B phases: + - `pedal-only` (`LongitudinalManeuverPaddleMode=off`) + - `pedal+paddle` (`LongitudinalManeuverPaddleMode=force`) + It also uses additional `-2.0` and `-2.5 m/s^2` brake steps as required checks. The `-4.0 m/s^2` step is logged as informational since pedal/regen authority is physically limited without friction brakes. + + ![cog-clip-00 01 11 250-00 01 22 250](https://github.com/user-attachments/assets/c312c1cc-76e8-46e1-a05e-bb9dfb58994f) + +6. When testing is complete, `maneuversd` logs `LONG_MANEUVER_MODE|state=FINISHED` in `logMessage` and automatically disables `LongitudinalManeuverMode` so normal planning resumes. Complete the route by pulling over and turning off the vehicle. + +7. Visit https://connect.comma.ai and locate the route(s). They will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded." + + ![image](https://github.com/user-attachments/assets/cfe4c6d9-752f-4b24-b421-4b90a01933dc) + +8. Gather the route ID and then run the report generator. The file will be exported to the same directory: + + ```sh + $ python tools/longitudinal_maneuvers/generate_report.py 57048cfce01d9625/0000010e--5b26bc3be7 'pcm accel compensation' + + processing report for LEXUS_ES_TSS2 + plotting maneuver: start from stop, runs: 4 + plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2 + plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2 + + Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html + ``` + +You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself. diff --git a/tools/longitudinal_maneuvers/__init__.py b/tools/longitudinal_maneuvers/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py new file mode 100755 index 000000000..0e6dafa22 --- /dev/null +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -0,0 +1,309 @@ +#!/usr/bin/env python3 +import argparse +import base64 +import io +import os +import math +import json +import pprint +import webbrowser +from collections import defaultdict +from pathlib import Path +import matplotlib.pyplot as plt + +from openpilot.tools.lib.logreader import LogReader +from openpilot.system.hardware.hw import Paths + + +def tabulate_html(rows, headers): + builder = ["", "", ""] + for h in headers: + builder.append(f"") + builder += ["", "", ""] + + for row in rows: + builder.append("") + for cell in row: + builder.append(f"") + builder.append("") + + builder += ["", "
{h}
{cell}
"] + return "\n".join(builder) + + +def parse_log_marker(text): + raw = text + try: + payload = json.loads(text) + if isinstance(payload, dict): + raw = payload.get("msg", text) + except json.JSONDecodeError: + pass + + if not isinstance(raw, str) or not raw.startswith("LONG_MANEUVER_"): + return None + + parts = raw.split("|") + event = parts[0].replace("LONG_MANEUVER_", "") + fields = {} + for p in parts[1:]: + if "=" in p: + k, v = p.split("=", 1) + fields[k] = v + return event, fields + + +def extract_maneuver_intervals(msgs): + intervals = [] + current = None + + for msg in msgs: + which = msg.which() + if which not in ("logMessage", "errorLogMessage"): + continue + + text = msg.logMessage if which == "logMessage" else msg.errorLogMessage + marker = parse_log_marker(text) + if marker is None: + continue + + event, fields = marker + desc = fields.get("desc", "") + run = int(fields.get("run", "0")) if fields.get("run", "0").isdigit() else 0 + + if event == "START": + current = {"desc": desc, "run": run, "start": msg.logMonoTime} + elif event == "END" and current is not None: + if not desc or desc == current["desc"]: + intervals.append((current["desc"], current["run"], current["start"], msg.logMonoTime)) + current = None + + if current is not None: + intervals.append((current["desc"], current["run"], current["start"], msgs[-1].logMonoTime)) + + return intervals + + +def format_car_params(CP): + return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2) + + +def is_informational_maneuver(description: str) -> bool: + return "informational" in description.lower() + + +def report(platform, route, _description, CP, ID, maneuvers): + output_path = Path(__file__).resolve().parent / "longitudinal_reports" + output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html" + output_path.mkdir(exist_ok=True) + target_cross_times = defaultdict(list) + + builder = [ + "\n", + "

Longitudinal maneuver report

\n", + f"

{platform}

\n", + f"

{route}

\n", + f"

{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}

\n", + ] + if _description is not None: + builder.append(f"

Description: {_description}

\n") + builder.append(f"

CarParams

{format_car_params(CP)}
\n") + builder.append('{ summary }') # to be replaced below + for description, runs in maneuvers: + print(f'plotting maneuver: {description}, runs: {len(runs)}') + builder.append("
\n") + builder.append(f"

{description}

\n") + for run, msgs in enumerate(runs): + cc_pairs = [(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'] + co_pairs = [(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'] + cs_pairs = [(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'] + lp_pairs = [(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'] + plan_pairs = [(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'] + if not (cc_pairs and co_pairs and cs_pairs and lp_pairs and plan_pairs): + continue + + t_carControl, carControl = zip(*cc_pairs, strict=True) + t_carOutput, carOutput = zip(*co_pairs, strict=True) + t_carState, carState = zip(*cs_pairs, strict=True) + t_livePose, livePose = zip(*lp_pairs, strict=True) + t_longitudinalPlan, longitudinalPlan = zip(*plan_pairs, strict=True) + + # make time relative seconds + t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl] + t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput] + t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState] + t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose] + t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan] + + # maneuver validity + longActive = [m.longActive for m in carControl] + maneuver_valid = all(longActive) and (not any(cs.cruiseState.standstill for cs in carState) or CP.autoResumeSng) + + _open = 'open' if maneuver_valid else '' + title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') + + builder.append(f"

{title}

\n") + + info_only = is_informational_maneuver(description) + + # get first acceleration target and first intersection + aTarget = longitudinalPlan[0].aTarget + target_cross_time = None + initial_speed = carState[0].vEgo + builder.append(f'

Initial aTarget: {round(aTarget, 2)} m/s^2') + + # Localizer is noisy, require two consecutive 20Hz frames above threshold + prev_crossed = False + for t, lp in zip(t_livePose, livePose, strict=True): + crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x) + if crossed and prev_crossed: + builder.append(f', crossed in {t:.3f}s') + target_cross_time = t + if maneuver_valid: + target_cross_times[description].append(t) + break + prev_crossed = crossed + else: + builder.append(', not crossed') + builder.append('

') + + builder.append( + f'

Initial speed: {initial_speed:.2f} m/s ' + f'({initial_speed * 2.23694:.1f} mph)

' + ) + + if abs(aTarget) > 1e-3: + if aTarget < 0: + achieved = min((lp.accelerationDevice.x for lp in livePose), default=0.0) + ratio = achieved / aTarget if aTarget != 0 else 0.0 + est_power_kw = (CP.mass * initial_speed * abs(aTarget)) / 1000.0 + ref = " vs 70 kW reference" if CP.carName == "gm" else "" + builder.append( + f'

Estimated wheel regen demand: ' + f'{est_power_kw:.1f} kW{ref}

' + ) + else: + achieved = max((lp.accelerationDevice.x for lp in livePose), default=0.0) + ratio = achieved / aTarget if aTarget != 0 else 0.0 + + builder.append( + f'

Peak achieved accel: {achieved:.2f} m/s^2, ' + f'Achieved/target ratio: {ratio:.2f}

' + ) + + if info_only: + builder.append('

Result type: informational (pedal/regen authority check)

') + + pitches = [math.degrees(m.orientationNED[1]) for m in carControl] + builder.append(f'

Average pitch: {sum(pitches) / len(pitches):0.2f} degrees

') + + plt.rcParams['font.size'] = 40 + fig = plt.figure(figsize=(30, 26)) + ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]}) + + ax[0].grid(linewidth=4) + ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6) + ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6) + ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6) + ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6) + ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6) + # TODO localizer accel + ax[0].set_ylabel('Acceleration (m/s^2)') + #ax[0].set_ylim(-6.5, 6.5) + ax[0].legend(prop={'size': 30}) + + if target_cross_time is not None: + ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None') + + ax[1].grid(linewidth=4) + ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6) + ax[1].set_ylabel('Velocity (m/s)') + ax[1].legend() + + ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6) + ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6) + ax[3].plot(t_carState, [m.brakePressed for m in carState], 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='webp') + plt.close(fig) + buffer.seek(0) + builder.append(f"\n") + builder.append("
\n") + + summary = ["

Summary

\n"] + cols = ['maneuver', 'type', 'crossed', 'runs', 'mean', 'min', 'max'] + table = [] + for description, runs in maneuvers: + times = target_cross_times[description] + row_type = 'informational' if is_informational_maneuver(description) else 'required' + l = [description, row_type, len(times), len(runs)] + if len(times): + l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)]) + table.append(l) + summary.append(tabulate_html(table, cols) + '\n') + + sum_idx = builder.index('{ summary }') + builder[sum_idx:sum_idx + 1] = summary + + with open(output_fn, "w") as f: + f.write(''.join(builder)) + + print(f"\nOpening report: {output_fn}\n") + webbrowser.open_new_tab(str(output_fn)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route') + parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)') + parser.add_argument('description', type=str, nargs='?') + + args = parser.parse_args() + + if '/' in args.route or '|' in args.route: + lr = LogReader(args.route) + else: + segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg] + lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs]) + + CP = lr.first('carParams') + ID = lr.first('initData') + platform = CP.carFingerprint + print('processing report for', platform) + + maneuvers: list[tuple[str, list[list]]] = [] + msgs = list(lr) + intervals = extract_maneuver_intervals(msgs) + + if intervals: + grouped: dict[str, list[list]] = defaultdict(list) + for desc, _, start, end in intervals: + run_msgs = [m for m in msgs if start <= m.logMonoTime <= end] + if run_msgs: + grouped[desc].append(run_msgs) + maneuvers = list(grouped.items()) + else: + # Fallback for logs generated by the original alertDebug-based implementation. + active_prev = False + description_prev = None + for msg in msgs: + if msg.which() == 'alertDebug': + active = 'Maneuver Active' in msg.alertDebug.alertText1 + if active and not active_prev: + if msg.alertDebug.alertText2 == description_prev: + maneuvers[-1][1].append([]) + else: + maneuvers.append((msg.alertDebug.alertText2, [[]])) + description_prev = maneuvers[-1][0] + active_prev = active + if active_prev: + maneuvers[-1][1][-1].append(msg) + + report(platform, args.route, args.description, CP, ID, maneuvers) diff --git a/tools/longitudinal_maneuvers/maneuver_helpers.py b/tools/longitudinal_maneuvers/maneuver_helpers.py new file mode 100644 index 000000000..9fc65fb9e --- /dev/null +++ b/tools/longitudinal_maneuvers/maneuver_helpers.py @@ -0,0 +1,18 @@ +from enum import IntEnum + +class Axis(IntEnum): + TIME = 0 + EGO_POSITION = 1 + LEAD_DISTANCE= 2 + EGO_V = 3 + LEAD_V = 4 + EGO_A = 5 + D_REL = 6 + +axis_labels = {Axis.TIME: 'Time (s)', + Axis.EGO_POSITION: 'Ego position (m)', + Axis.LEAD_DISTANCE: 'Lead absolute position (m)', + Axis.EGO_V: 'Ego Velocity (m/s)', + Axis.LEAD_V: 'Lead Velocity (m/s)', + Axis.EGO_A: 'Ego acceleration (m/s^2)', + Axis.D_REL: 'Lead distance (m)'} diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py new file mode 100755 index 000000000..2dc588277 --- /dev/null +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -0,0 +1,493 @@ +#!/usr/bin/env python3 +import json +import time +import numpy as np +from dataclasses import dataclass +from collections import defaultdict +from typing import NamedTuple + +from cereal import messaging, car +from openpilot.common.conversions import Conversions as CV +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.modeld.constants import ModelConstants +from openpilot.selfdrive.car.gm.values import GMFlags + + +@dataclass +class Action: + accel_bp: list[float] # m/s^2 + time_bp: list[float] # seconds + + def __post_init__(self): + assert len(self.accel_bp) == len(self.time_bp) + + +@dataclass +class Maneuver: + description: str + actions: list[Action] + repeat: int = 0 + initial_speed: float = 0. # m/s + + _active: bool = False + _finished: bool = False + _action_index: int = 0 + _action_frames: int = 0 + _ready_cnt: int = 0 + _repeated: int = 0 + + def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float: + ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill + if self.initial_speed < 0.01: + ready = ready and standstill + self._ready_cnt = (self._ready_cnt + 1) if ready else 0 + + if self._ready_cnt > (3. / DT_MDL): + self._active = True + + if not self._active: + return min(max(self.initial_speed - v_ego, -2.), 2.) + + action = self.actions[self._action_index] + action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp) + + self._action_frames += 1 + + # reached duration of action + if self._action_frames > (action.time_bp[-1] / DT_MDL): + # next action + if self._action_index < len(self.actions) - 1: + self._action_index += 1 + self._action_frames = 0 + # repeat maneuver + elif self._repeated < self.repeat: + self._repeated += 1 + self._action_index = 0 + self._action_frames = 0 + self._active = False + # finish maneuver + else: + self._finished = True + + return float(action_accel) + + @property + def finished(self): + return self._finished + + @property + def active(self): + return self._active + + +BASE_MANEUVERS = [ + Maneuver( + "come to stop", + [Action([-0.5], [12])], + repeat=2, + initial_speed=5., + ), + Maneuver( + "start from stop", + [Action([1.5], [6])], + repeat=2, + initial_speed=0., + ), + Maneuver( + "creep: alternate between +1m/s^2 and -1m/s^2", + [ + Action([1], [3]), Action([-1], [3]), + Action([1], [3]), Action([-1], [3]), + Action([1], [3]), Action([-1], [3]), + ], + repeat=2, + initial_speed=0., + ), + Maneuver( + "brake step response: -1m/s^2 from 20mph", + [Action([-1], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), + Maneuver( + "gas step response: +1m/s^2 from 20mph", + [Action([1], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), + Maneuver( + "gas step response: +4m/s^2 from 20mph", + [Action([4], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), +] + +STANDARD_BRAKE_STEPS = [ + Maneuver( + "brake step response: -4m/s^2 from 20mph", + [Action([-4], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), +] + +PEDAL_LONG_BRAKE_STEPS = [ + Maneuver( + "brake step response: -2.0m/s^2 from 20mph", + [Action([-2.0], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), + Maneuver( + "brake step response: -2.5m/s^2 from 20mph", + [Action([-2.5], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), + Maneuver( + "brake step response: -4m/s^2 from 20mph (informational, pedal-limited)", + [Action([-4], [3])], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), +] + + +def clone_maneuver(m: Maneuver, label_suffix: str = "") -> Maneuver: + actions = [Action(list(a.accel_bp), list(a.time_bp)) for a in m.actions] + desc = f"{m.description} {label_suffix}".strip() + return Maneuver(desc, actions, repeat=m.repeat, initial_speed=m.initial_speed) + + +def build_maneuvers(CP, label_suffix: str = "") -> list[Maneuver]: + is_gm_pedal_long = (CP.carName == "gm" and CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value)) + maneuvers = [clone_maneuver(m, label_suffix) for m in BASE_MANEUVERS] + brake_steps = PEDAL_LONG_BRAKE_STEPS if is_gm_pedal_long else STANDARD_BRAKE_STEPS + + # Keep brake step maneuvers grouped with other step responses. + insert_idx = next((i for i, m in enumerate(maneuvers) if m.description.startswith("gas step response")), len(maneuvers)) + for step in reversed(brake_steps): + maneuvers.insert(insert_idx, clone_maneuver(step, label_suffix)) + + return maneuvers + + +class ManeuverPhase(NamedTuple): + name: str + paddle_mode: str + maneuvers: list[Maneuver] + + +def build_maneuver_phases(CP) -> list[ManeuverPhase]: + is_gm_pedal_long = (CP.carName == "gm" and CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value)) + if not is_gm_pedal_long: + return [ManeuverPhase("standard", "auto", build_maneuvers(CP))] + + # For pedal-long GM, run paired A/B phases to isolate pedal-only vs pedal+paddle behavior. + return [ + ManeuverPhase("pedal_only", "off", build_maneuvers(CP, "[pedal-only]")), + ManeuverPhase("pedal_plus_paddle", "force", build_maneuvers(CP, "[pedal+paddle]")), + ] + + +def get_phase_step_count(phase: ManeuverPhase) -> int: + return sum(m.repeat + 1 for m in phase.maneuvers) + + +def get_total_step_count(phases: list[ManeuverPhase]) -> int: + return sum(get_phase_step_count(phase) for phase in phases) + + +def write_status(params: Params, status: dict, history_line: str | None = None, **fields) -> None: + if history_line: + history = list(status.get("history", [])) + history.append(history_line) + status["history"] = history[-120:] + + status.update(fields) + status["updatedAtSec"] = time.time() + params.put("LongitudinalManeuverStatus", json.dumps(status, separators=(",", ":"))) + + +def main(): + params = Params() + history = [] + raw_status = params.get("LongitudinalManeuverStatus", encoding="utf-8") or "" + if raw_status: + try: + prior_status = json.loads(raw_status) + if isinstance(prior_status, dict): + raw_history = prior_status.get("history", []) + if isinstance(raw_history, list): + history = [str(line) for line in raw_history if str(line).strip()][-120:] + except Exception: + pass + + status = { + "state": "starting", + "phase": "", + "paddleMode": "auto", + "maneuver": "", + "runIndex": 0, + "runTotal": 0, + "stepIndex": 0, + "stepTotal": 0, + "phaseStepIndex": 0, + "phaseStepTotal": 0, + "uiShow": True, + "uiSize": "mid", + "uiText1": "Long Maneuvers", + "uiText2": "Waiting for CarParams...", + "history": history, + } + write_status(params, status, history_line="Mode armed, waiting for CarParams.") + + cloudlog.info("maneuversd is waiting for CarParams") + cp_bytes = params.get("CarParams", block=True) + try: + with car.CarParams.from_bytes(cp_bytes) as msg: + CP = msg + except Exception: + # Fallback for branches that decode via Event wrapper only. + CP = messaging.log_from_bytes(cp_bytes).carParams + cloudlog.warning("LONG_MANEUVER_MODE|state=STARTED") + params.put("LongitudinalManeuverPaddleMode", "auto") + + sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'modelV2'], poll='modelV2') + pm = messaging.PubMaster(['longitudinalPlan']) + + phases = build_maneuver_phases(CP) + total_step_count = max(get_total_step_count(phases), 1) + phase_idx = 0 + active_phase = phases[phase_idx] + active_phase_step_total = get_phase_step_count(active_phase) + params.put("LongitudinalManeuverPaddleMode", active_phase.paddle_mode) + cloudlog.warning(f"LONG_MANEUVER_PHASE|name={active_phase.name}|paddleMode={active_phase.paddle_mode}") + write_status( + params, + status, + state="phase", + phase=active_phase.name, + paddleMode=active_phase.paddle_mode, + stepTotal=total_step_count, + phaseStepTotal=active_phase_step_total, + uiText1="Long Maneuvers", + uiText2=f"Phase {phase_idx + 1}/{len(phases)}: {active_phase.name} ({active_phase_step_total} steps)", + history_line=f"Phase {phase_idx + 1}/{len(phases)} started: {active_phase.name} ({active_phase_step_total} steps).", + ) + + maneuvers = iter(active_phase.maneuvers) + maneuver = None + maneuver_run_count: defaultdict[str, int] = defaultdict(int) + run_active = False + active_description = "" + active_run_index = 0 + active_run_total = 0 + finished_logged = False + active_phase_name = active_phase.name + active_paddle_mode = active_phase.paddle_mode + step_index = 0 + phase_step_index = 0 + + t_idx = ModelConstants.T_IDXS[:CONTROL_N] + try: + while True: + sm.update() + + if maneuver is None: + maneuver = next(maneuvers, None) + run_active = False + active_description = "" + if maneuver is None: + if phase_idx + 1 < len(phases): + phase_idx += 1 + active_phase = phases[phase_idx] + active_phase_name = active_phase.name + active_paddle_mode = active_phase.paddle_mode + active_phase_step_total = get_phase_step_count(active_phase) + phase_step_index = 0 + params.put("LongitudinalManeuverPaddleMode", active_paddle_mode) + cloudlog.warning(f"LONG_MANEUVER_PHASE|name={active_phase_name}|paddleMode={active_paddle_mode}") + write_status( + params, + status, + state="phase", + phase=active_phase_name, + paddleMode=active_paddle_mode, + phaseStepIndex=phase_step_index, + phaseStepTotal=active_phase_step_total, + uiText1="Long Maneuvers", + uiText2=f"Phase {phase_idx + 1}/{len(phases)}: {active_phase_name} ({active_phase_step_total} steps)", + history_line=f"Phase {phase_idx + 1}/{len(phases)} started: {active_phase_name} ({active_phase_step_total} steps).", + ) + maneuvers = iter(active_phase.maneuvers) + maneuver = next(maneuvers, None) + elif not finished_logged: + cloudlog.warning("LONG_MANEUVER_MODE|state=FINISHED") + write_status( + params, + status, + state="finished", + maneuver="", + runIndex=0, + runTotal=0, + uiShow=True, + uiSize="mid", + uiText1="Long Maneuvers Complete", + uiText2=f"Completed {step_index}/{total_step_count} steps. Returning to normal long control.", + history_line=f"Complete: {step_index}/{total_step_count} steps finished.", + ) + params.put_bool("LongitudinalManeuverMode", False) + finished_logged = True + break + + plan_send = messaging.new_message('longitudinalPlan') + plan_send.valid = sm.all_checks() + + longitudinalPlan = plan_send.longitudinalPlan + accel = 0 + v_ego = max(sm['carState'].vEgo, 0) + + if maneuver is not None: + accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill) + + if maneuver.active and not run_active: + maneuver_run_count[maneuver.description] += 1 + step_index += 1 + phase_step_index += 1 + run_active = True + active_description = maneuver.description + active_run_index = maneuver_run_count[maneuver.description] + active_run_total = maneuver.repeat + 1 + cloudlog.warning( + f"LONG_MANEUVER_START|desc={maneuver.description}|run={maneuver_run_count[maneuver.description]}|" + f"accel={accel:.3f}|phase={active_phase_name}|paddleMode={active_paddle_mode}" + ) + write_status( + params, + status, + state="running", + phase=active_phase_name, + paddleMode=active_paddle_mode, + maneuver=active_description, + runIndex=active_run_index, + runTotal=active_run_total, + stepIndex=step_index, + stepTotal=total_step_count, + phaseStepIndex=phase_step_index, + phaseStepTotal=active_phase_step_total, + uiShow=True, + uiSize="mid", + uiText1="Long Maneuvers", + uiText2=f"Step {step_index}/{total_step_count}: {active_description} ({active_run_index}/{active_run_total})", + history_line=f"Step {step_index}/{total_step_count} started: {active_description} ({active_run_index}/{active_run_total}).", + ) + elif run_active and not maneuver.active: + cloudlog.warning( + f"LONG_MANEUVER_END|desc={active_description}|run={maneuver_run_count[active_description]}|" + f"phase={active_phase_name}|paddleMode={active_paddle_mode}" + ) + write_status( + params, + status, + state="running", + phase=active_phase_name, + paddleMode=active_paddle_mode, + maneuver=active_description, + runIndex=active_run_index, + runTotal=active_run_total, + stepIndex=step_index, + stepTotal=total_step_count, + phaseStepIndex=phase_step_index, + phaseStepTotal=active_phase_step_total, + uiShow=True, + uiSize="small", + uiText1="Long Maneuvers", + uiText2=f"Step {step_index}/{total_step_count} complete", + history_line=f"Step {step_index}/{total_step_count} complete: {active_description} ({active_run_index}/{active_run_total}).", + ) + run_active = False + active_description = "" + + longitudinalPlan.aTarget = accel + longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2 + + longitudinalPlan.allowBrake = True + longitudinalPlan.allowThrottle = True + longitudinalPlan.hasLead = True + + # Build a full horizon trajectory so both old and new long APIs can consume the test plan. + speed_traj = [max(v_ego + accel * t, 0.0) for t in t_idx] + speed_traj[0] = max(speed_traj[0], 0.2) # keeps stock-ACC resume spamming path alive when needed + longitudinalPlan.speeds = speed_traj + longitudinalPlan.accels = [accel] * CONTROL_N + longitudinalPlan.jerks = [0.0] * CONTROL_N + + pm.send('longitudinalPlan', plan_send) + + if maneuver is not None and maneuver.finished: + if run_active: + cloudlog.warning( + f"LONG_MANEUVER_END|desc={active_description}|run={maneuver_run_count[active_description]}|" + f"phase={active_phase_name}|paddleMode={active_paddle_mode}" + ) + write_status( + params, + status, + state="running", + phase=active_phase_name, + paddleMode=active_paddle_mode, + maneuver=active_description, + runIndex=active_run_index, + runTotal=active_run_total, + stepIndex=step_index, + stepTotal=total_step_count, + phaseStepIndex=phase_step_index, + phaseStepTotal=active_phase_step_total, + uiShow=True, + uiSize="small", + uiText1="Long Maneuvers", + uiText2=f"Step {step_index}/{total_step_count} complete", + history_line=f"Step {step_index}/{total_step_count} complete: {active_description} ({active_run_index}/{active_run_total}).", + ) + cloudlog.warning( + f"LONG_MANEUVER_FINISHED|desc={maneuver.description}|runs={maneuver_run_count[maneuver.description]}|" + f"phase={active_phase_name}|paddleMode={active_paddle_mode}" + ) + write_status( + params, + status, + state="running", + phase=active_phase_name, + paddleMode=active_paddle_mode, + maneuver="", + runIndex=0, + runTotal=0, + uiShow=True, + uiSize="small", + uiText1="Long Maneuvers", + uiText2=f"Maneuver complete: {maneuver.description}", + history_line=f"Maneuver complete: {maneuver.description}.", + ) + maneuver = None + run_active = False + active_description = "" + active_run_index = 0 + active_run_total = 0 + finally: + if not finished_logged: + write_status( + params, + status, + state="stopped", + uiShow=True, + uiSize="small", + uiText1="Long Maneuvers Stopped", + uiText2=f"Stopped at step {step_index}/{total_step_count}.", + history_line=f"Stopped at step {step_index}/{total_step_count}.", + ) + params.put("LongitudinalManeuverPaddleMode", "auto") diff --git a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py new file mode 100644 index 000000000..ae3fee735 --- /dev/null +++ b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py @@ -0,0 +1,292 @@ +import io +import sys +import markdown +import numpy as np +import matplotlib.pyplot as plt +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance +from openpilot.tools.longitudinal_maneuvers.maneuver_helpers import Axis, axis_labels +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + + +def get_html_from_results(results, labels, AXIS): + fig, ax = plt.subplots(figsize=(16, 8)) + for idx, key in enumerate(results.keys()): + ax.plot(results[key][:, Axis.TIME], results[key][:, AXIS], label=labels[idx]) + + ax.set_xlabel(axis_labels[Axis.TIME]) + ax.set_ylabel(axis_labels[AXIS]) + ax.legend(bbox_to_anchor=(1.02, 1), loc='upper left', borderaxespad=0) + ax.grid(True, linestyle='--', alpha=0.7) + ax.text(-0.075, 0.5, '.', transform=ax.transAxes, color='none') + + fig_buffer = io.StringIO() + fig.savefig(fig_buffer, format='svg', bbox_inches='tight') + plt.close(fig) + return fig_buffer.getvalue() + '
' + +def generate_mpc_tuning_report(): + htmls = [] + + results = {} + name = 'Resuming behind lead' + labels = [] + for lead_accel in np.linspace(1.0, 4.0, 4): + man = Maneuver( + '', + duration=11, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 10 * lead_accel], + cruise_values=[100, 100], + prob_lead_values=[1.0, 1.0], + breakpoints=[1., 11], + ) + valid, results[lead_accel] = man.evaluate() + labels.append(f'{lead_accel} m/s^2 lead acceleration') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + + results = {} + name = 'Approaching stopped car from 140m' + labels = [] + for speed in np.arange(0,45,5): + man = Maneuver( + name, + duration=30., + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=140., + speed_lead_values=[0.0, 0.], + breakpoints=[0., 30.], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s approach speed') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + + + results = {} + name = 'Following 5s (triangular) oscillating lead' + labels = [] + speed = np.int64(10) + for oscil in np.arange(0, 10, 1): + man = Maneuver( + '', + duration=30., + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed), + speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil], + breakpoints=[0.,2., 5, 8, 15, 18, 25.], + ) + valid, results[oscil] = man.evaluate() + labels.append(f'{oscil} m/s oscillation size') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + + results = {} + name = 'Following 5s (sinusoidal) oscillating lead' + labels = [] + speed = np.int64(10) + duration = float(30) + f_osc = 1. / 5 + for oscil in np.arange(0, 10, 1): + bps = DT_MDL * np.arange(int(duration / DT_MDL)) + lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps) + man = Maneuver( + '', + duration=duration, + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed), + speed_lead_values=lead_speeds, + breakpoints=bps, + ) + valid, results[oscil] = man.evaluate() + labels.append(f'{oscil} m/s oscillation size') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + + results = {} + name = 'Speed profile when converging to steady state lead at 30m/s' + labels = [] + for distance in np.arange(20, 140, 10): + man = Maneuver( + '', + duration=50, + initial_speed=30.0, + lead_relevancy=True, + initial_distance_lead=distance, + speed_lead_values=[30.0], + breakpoints=[0.], + ) + valid, results[distance] = man.evaluate() + labels.append(f'{distance} m initial distance') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + + + results = {} + name = 'Speed profile when converging to steady state lead at 20m/s' + labels = [] + for distance in np.arange(20, 140, 10): + man = Maneuver( + '', + duration=50, + initial_speed=20.0, + lead_relevancy=True, + initial_distance_lead=distance, + speed_lead_values=[20.0], + breakpoints=[0.], + ) + valid, results[distance] = man.evaluate() + labels.append(f'{distance} m initial distance') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + + + results = {} + name = 'Following car at 30m/s that comes to a stop' + labels = [] + for stop_time in np.arange(4, 14, 1): + man = Maneuver( + '', + duration=30, + initial_speed=30.0, + cruise_values=[30.0, 30.0, 30.0], + lead_relevancy=True, + initial_distance_lead=60.0, + speed_lead_values=[30.0, 30.0, 0.0], + breakpoints=[0., 5., 5 + stop_time], + ) + valid, results[stop_time] = man.evaluate() + labels.append(f'{stop_time} seconds stop time') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + + + results = {} + name = 'Response to cut-in at half follow distance' + labels = [] + for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=20, + initial_speed=float(speed), + cruise_values=[speed, speed, speed], + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed)/2, + speed_lead_values=[speed, speed, speed], + prob_lead_values=[0.0, 0.0, 1.0], + breakpoints=[0., 5.0, 5.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + htmls.append(get_html_from_results(results, labels, Axis.D_REL)) + + + results = {} + name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed' + labels = [] + for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=60, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0, speed], + prob_lead_values=[1.0, 1.0, 1.0], + breakpoints=[0., 1.0, speed/2], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + + results = {} + name = 'From stop to cruise' + labels = [] + for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=50, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0], + cruise_values=[0.0, speed], + prob_lead_values=[0.0, 0.0], + breakpoints=[1., 1.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + + results = {} + name = 'From cruise to min' + labels = [] + for speed in np.arange(10, 40, 5): + man = Maneuver( + '', + duration=50, + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0], + cruise_values=[speed, 10.0], + prob_lead_values=[0.0, 0.0], + breakpoints=[1., 1.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_V)) + htmls.append(get_html_from_results(results, labels, Axis.EGO_A)) + + return htmls + +if __name__ == '__main__': + htmls = generate_mpc_tuning_report() + + if len(sys.argv) < 2: + file_name = 'long_mpc_tune_report.html' + else: + file_name = sys.argv[1] + + with open(file_name, 'w') as f: + f.write(markdown.markdown('# MPC longitudinal tuning report')) + for html in htmls: + f.write(html)