From f1feaa2cb9375c307495f38f87eafdf51366e986 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 6 Apr 2026 17:24:16 -0500 Subject: [PATCH] long manuevers --- .../tools/longitudinal_maneuvers.js | 30 ++++- starpilot/system/the_pond/the_pond.py | 21 ++++ tools/longitudinal_maneuvers/README.md | 4 +- tools/longitudinal_maneuvers/capabilities.py | 75 +++++++++++ .../longitudinal_maneuvers/generate_report.py | 35 +++++- tools/longitudinal_maneuvers/maneuversd.py | 116 ++++++++++-------- 6 files changed, 227 insertions(+), 54 deletions(-) create mode 100644 tools/longitudinal_maneuvers/capabilities.py diff --git a/starpilot/system/the_pond/assets/components/tools/longitudinal_maneuvers.js b/starpilot/system/the_pond/assets/components/tools/longitudinal_maneuvers.js index 3c3b1b17..96292e26 100644 --- a/starpilot/system/the_pond/assets/components/tools/longitudinal_maneuvers.js +++ b/starpilot/system/the_pond/assets/components/tools/longitudinal_maneuvers.js @@ -156,13 +156,41 @@ export function LongitudinalManeuvers() {

Popup Text: ${state.data.uiText1 || ""} ${state.data.uiText2 ? `| ${state.data.uiText2}` : ""}

+
+ ${statusLine("openpilot Long", state.data.support?.openpilotLongitudinalControl ? "Yes" : "No")} + ${statusLine("Full Stop + Go", state.data.support?.fullStopAndGo ? "Yes" : "No")} + ${statusLine("Auto Resume", state.data.support?.autoResumeFromStop ? "Yes" : "No")} + ${statusLine("Resume Assist", state.data.support?.requiresResumeAssist ? "Yes" : "No")} + ${statusLine("Expected Zero", state.data.support?.expectedToReachZero ? "Yes" : "No")} + ${statusLine("Min Enable", Number.isFinite(Number(state.data.support?.minEnableSpeed)) ? `${Number(state.data.support.minEnableSpeed).toFixed(2)} m/s` : "n/a")} + ${statusLine("Stop Accel", Number.isFinite(Number(state.data.support?.stopAccel)) ? `${Number(state.data.support.stopAccel).toFixed(2)} m/s²` : "n/a")} +
+ + ${(state.data.caveats || []).length ? html` +
+

Platform Caveats

+ +
+ ` : ""} + + ${(state.data.skippedManeuvers || []).length ? html` +
+

Expected Skips

+ +
+ ` : ""} +

Quick Guide

  1. Find a large, empty, straight road or lot with no traffic.
  2. Press Start / Arm here, then engage openpilot with SET.
  3. Keep full supervision and be ready to disengage at all times.
    4. -
  4. For GM pedal-long cars, the suite runs both pedal-only and pedal+paddle phases automatically.
    5. +
  5. Review the platform caveats before starting. Low-speed maneuvers can be skipped automatically on cars that do not fully stop.
  6. When the status says complete, collect logs and generate your HTML report.
diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index 1b76dc28..21ab25fa 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -40,6 +40,7 @@ from openpilot.system.hardware import HARDWARE, PC from openpilot.system.hardware.hw import Paths from openpilot.system.loggerd.deleter import PRESERVE_ATTR_NAME, PRESERVE_ATTR_VALUE, PRESERVE_COUNT from openpilot.system.version import get_build_metadata +from openpilot.tools.longitudinal_maneuvers.capabilities import get_longitudinal_maneuver_support from panda import Panda from openpilot.starpilot.assets.theme_manager import HOLIDAY_THEME_PATH, THEME_COMPONENT_PARAMS @@ -2824,6 +2825,9 @@ def _default_longitudinal_maneuver_status(): "uiText1": "Long Maneuvers", "uiText2": "", "updatedAtSec": 0.0, + "support": {}, + "caveats": [], + "skippedManeuvers": [], "history": [], } @@ -2868,15 +2872,32 @@ def _append_longitudinal_maneuver_history(status, line): status["history"] = history[-120:] return status +def _get_longitudinal_maneuver_support_snapshot(): + cp_bytes = _safe_params_get_live_raw("CarParamsPersistent") + if not cp_bytes: + return None + + try: + with car.CarParams.from_bytes(cp_bytes) as cp: + return get_longitudinal_maneuver_support(cp).to_dict() + except Exception: + return None + def _serialize_longitudinal_maneuver_status(status): updated_at = _safe_float(status.get("updatedAtSec"), 0.0) age_seconds = max(0.0, time.time() - updated_at) if updated_at > 0 else None mode_enabled = params.get_bool("LongitudinalManeuverMode") paddle_mode = params.get("LongitudinalManeuverPaddleMode", encoding="utf-8") or str(status.get("paddleMode") or "auto") + support = _get_longitudinal_maneuver_support_snapshot() or status.get("support") or {} + caveats = support.get("caveats", status.get("caveats") or []) + skipped_maneuvers = support.get("skippedManeuvers", status.get("skippedManeuvers") or []) return { **status, "modeEnabled": mode_enabled, "paddleMode": paddle_mode, + "support": support, + "caveats": list(caveats), + "skippedManeuvers": list(skipped_maneuvers), "isOnroad": params.get_bool("IsOnroad"), "isEngaged": params.get_bool("IsEngaged"), "updatedAgeSec": age_seconds, diff --git a/tools/longitudinal_maneuvers/README.md b/tools/longitudinal_maneuvers/README.md index 643af7fd..98d81b5e 100644 --- a/tools/longitudinal_maneuvers/README.md +++ b/tools/longitudinal_maneuvers/README.md @@ -18,9 +18,9 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te ![videoframe_6652](https://github.com/user-attachments/assets/e9d4c95a-cd76-4ab7-933e-19937792fa0f) -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. +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:** The suite now reads vehicle stop/go capability from `CarParams`. Cars that can reach a stop but need resume assistance will allow standstill during zero-speed tests, and cars that are not expected to reach zero will skip stop/start/creep maneuvers automatically. ![cog-clip-00 01 11 250-00 01 22 250](https://github.com/user-attachments/assets/c312c1cc-76e8-46e1-a05e-bb9dfb58994f) diff --git a/tools/longitudinal_maneuvers/capabilities.py b/tools/longitudinal_maneuvers/capabilities.py new file mode 100644 index 00000000..fc54850f --- /dev/null +++ b/tools/longitudinal_maneuvers/capabilities.py @@ -0,0 +1,75 @@ +from __future__ import annotations + +from dataclasses import asdict, dataclass +from typing import Any + + +LOW_SPEED_MANEUVER_DESCRIPTIONS = ( + "come to stop", + "start from stop", + "creep: alternate between +1m/s^2 and -1m/s^2", +) + + +@dataclass(frozen=True) +class LongitudinalManeuverSupport: + openpilotLongitudinalControl: bool + fullStopAndGo: bool + autoResumeFromStop: bool + requiresResumeAssist: bool + expectedToReachZero: bool + minEnableSpeed: float + stopAccel: float + caveats: tuple[str, ...] + skippedManeuvers: tuple[str, ...] + + def to_dict(self) -> dict[str, Any]: + return asdict(self) + + +def get_longitudinal_maneuver_support(CP: Any) -> LongitudinalManeuverSupport: + openpilot_longitudinal = bool(getattr(CP, "openpilotLongitudinalControl", False)) + auto_resume_supported = bool(getattr(CP, "autoResumeSng", False)) + + min_enable_speed = float(getattr(CP, "minEnableSpeed", 0.0) or 0.0) + stop_accel = float(getattr(CP, "stopAccel", 0.0) or 0.0) + + # CarParams does not expose a dedicated "won't fully brake to zero" flag, + # so low-speed engagement support is the closest reliable proxy. + full_stop_and_go = min_enable_speed <= 0.0 + auto_resume_from_stop = full_stop_and_go and auto_resume_supported + expected_to_reach_zero = full_stop_and_go + requires_resume_assist = expected_to_reach_zero and not auto_resume_from_stop + + caveats: list[str] = [] + if not openpilot_longitudinal: + caveats.append("openpilot longitudinal is disabled, so the maneuver suite cannot drive longitudinal tests on this platform.") + + if not expected_to_reach_zero: + caveats.append("This car is not expected to reach a true standstill in the suite. Stop, start, and creep maneuvers will be skipped.") + elif requires_resume_assist: + caveats.append("This car can reach a stop, but restart-from-stop needs resume assistance. Zero-speed maneuvers will allow cruise standstill instead of treating it as setup failure.") + + skipped_maneuvers = LOW_SPEED_MANEUVER_DESCRIPTIONS if not expected_to_reach_zero else () + + return LongitudinalManeuverSupport( + openpilotLongitudinalControl=openpilot_longitudinal, + fullStopAndGo=full_stop_and_go, + autoResumeFromStop=auto_resume_from_stop, + requiresResumeAssist=requires_resume_assist, + expectedToReachZero=expected_to_reach_zero, + minEnableSpeed=min_enable_speed, + stopAccel=stop_accel, + caveats=tuple(caveats), + skippedManeuvers=tuple(skipped_maneuvers), + ) + + +def get_maneuver_skip_reason(description: str, support: LongitudinalManeuverSupport) -> str | None: + if not support.openpilotLongitudinalControl: + return "openpilot longitudinal is disabled" + + if description in LOW_SPEED_MANEUVER_DESCRIPTIONS and not support.expectedToReachZero: + return "vehicle is not expected to reach a true standstill" + + return None diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index 8c16e30d..fb17556d 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -12,6 +12,7 @@ import matplotlib.pyplot as plt from tabulate import tabulate from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.longitudinal_maneuvers.capabilities import get_longitudinal_maneuver_support from openpilot.system.hardware.hw import Paths @@ -19,11 +20,42 @@ 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 format_support_section(support): + rows = [ + ("openpilot longitudinal", "yes" if support.openpilotLongitudinalControl else "no"), + ("full stop and go", "yes" if support.fullStopAndGo else "no"), + ("auto resume from stop", "yes" if support.autoResumeFromStop else "no"), + ("resume assist expected", "yes" if support.requiresResumeAssist else "no"), + ("expected to reach zero", "yes" if support.expectedToReachZero else "no"), + ("min enable speed", f"{support.minEnableSpeed:.2f} m/s"), + ("stop accel", f"{support.stopAccel:.2f} m/s^2"), + ] + + builder = ["

Platform Caveats

\n"] + builder.append(tabulate(rows, headers=("capability", "value"), tablefmt="html") + "\n") + + if support.caveats: + builder.append("

Caveats

\n") + + if support.skippedManeuvers: + builder.append("

Expected skipped maneuvers

\n") + + builder.append("
\n") + return ''.join(builder) + + 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) + support = get_longitudinal_maneuver_support(CP) builder = [ "\n", @@ -35,6 +67,7 @@ def report(platform, route, _description, CP, ID, maneuvers): if _description is not None: builder.append(f"

Description: {_description}

\n") builder.append(f"

CarParams

{format_car_params(CP)}
\n") + builder.append(format_support_section(support)) builder.append('{ summary }') # to be replaced below for description, runs in maneuvers: print(f'plotting maneuver: {description}, runs: {len(runs)}') @@ -56,7 +89,7 @@ def report(platform, route, _description, CP, ID, maneuvers): # 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) + maneuver_valid = all(longActive) _open = 'open' if maneuver_valid else '' title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index c17ae237..7ca6b4db 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -7,6 +7,10 @@ from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog +from openpilot.tools.longitudinal_maneuvers.capabilities import ( + get_longitudinal_maneuver_support, + get_maneuver_skip_reason, +) @dataclass @@ -33,9 +37,11 @@ class Maneuver: _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 + ready = abs(v_ego - self.initial_speed) < 0.3 and long_active if self.initial_speed < 0.01: ready = ready and standstill + else: + ready = ready and not cruise_standstill self._ready_cnt = (self._ready_cnt + 1) if ready else 0 if self._ready_cnt > (3. / DT_MDL): @@ -76,65 +82,75 @@ class Maneuver: return self._active -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( - "brake step response: -4m/s^2 from 20mph", - [Action([-4], [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, - ), -] +def build_maneuvers(): + return [ + 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( + "brake step response: -4m/s^2 from 20mph", + [Action([-4], [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, + ), + ] def main(): params = Params() cloudlog.info("joystickd is waiting for CarParams") CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + support = get_longitudinal_maneuver_support(CP) + + supported_maneuvers = [] + for maneuver in build_maneuvers(): + skip_reason = get_maneuver_skip_reason(maneuver.description, support) + if skip_reason is not None: + cloudlog.warning(f"Skipping longitudinal maneuver '{maneuver.description}': {skip_reason}") + continue + supported_maneuvers.append(maneuver) sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug']) - maneuvers = iter(MANEUVERS) + maneuvers = iter(supported_maneuvers) maneuver = None while True: