mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

long manuevers

Browse Source
This commit is contained in:
firestar5683
2026-04-06 17:24:16 -05:00
parent 033d83252e
commit f1feaa2cb9
6 changed files with 227 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
starpilot/system/the_pond/assets/components/tools/longitudinal_maneuvers.js
+29 -1
View File
@@ -156,13 +156,41 @@ export function LongitudinalManeuvers() {
<p><strong>Popup Text:</strong> ${state.data.uiText1 || ""} ${state.data.uiText2 ? `| ${state.data.uiText2}` : ""}</p>
</div>
<div class="longManeuverStatusGrid">
${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")}
</div>
${(state.data.caveats || []).length ? html`
<div class="longManeuverInstructions">
<h3>Platform Caveats</h3>
<ul>
${(state.data.caveats || []).map((line) => html`<li>${line}</li>`)}
</ul>
</div>
` : ""}
${(state.data.skippedManeuvers || []).length ? html`
<div class="longManeuverInstructions">
<h3>Expected Skips</h3>
<ul>
${(state.data.skippedManeuvers || []).map((line) => html`<li>${line}</li>`)}
</ul>
</div>
` : ""}
<div class="longManeuverInstructions">
<h3>Quick Guide</h3>
<ol>
<li>Find a large, empty, straight road or lot with no traffic.</li>
<li>Press <strong>Start / Arm</strong> here, then engage openpilot with SET.</li>
<li>Keep full supervision and be ready to disengage at all times.</li>
<li>For GM pedal-long cars, the suite runs both pedal-only and pedal+paddle phases automatically.</li>
<li>Review the platform caveats before starting. Low-speed maneuvers can be skipped automatically on cars that do not fully stop.</li>
<li>When the status says complete, collect logs and generate your HTML report.</li>
</ol>
</div>
starpilot/system/the_pond/the_pond.py
+21
View File
@@ -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,
tools/longitudinal_maneuvers/README.md
+2 -2
View File
@@ -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
+75
View File
@@ -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
+34 -1
View File
@@ -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
+66 -50
View File
@@ -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: