long manuevers

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)
 

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

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 = ["<details><summary><h3 style='display: inline-block;'>Platform Caveats</h3></summary>\n"]
+  builder.append(tabulate(rows, headers=("capability", "value"), tablefmt="html") + "\n")
+
+  if support.caveats:
+    builder.append("<h4>Caveats</h4><ul>\n")
+    for caveat in support.caveats:
+      builder.append(f"<li>{caveat}</li>\n")
+    builder.append("</ul>\n")
+
+  if support.skippedManeuvers:
+    builder.append("<h4>Expected skipped maneuvers</h4><ul>\n")
+    for description in support.skippedManeuvers:
+      builder.append(f"<li>{description}</li>\n")
+    builder.append("</ul>\n")
+
+  builder.append("</details>\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 = [
     "<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
@@ -35,6 +67,7 @@ def report(platform, route, _description, CP, ID, maneuvers):
   if _description is not None:
     builder.append(f"<h3>Description: {_description}</h3>\n")
   builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\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}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')

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: