add longitudinal maneuvers (#33527)
* add longitudinal profiles * stash * unfortunately even longitudinalPlan causes circle * add to process config * reach target speed smoothly * stash * works * clean up * debug alert * rename * fix * better text * toggle via exp button * try coming to a stop better, smoother target reaching * closer to target * revert controlsd migration * add description to alert * generate report from local logs * hide bad maneuvers * pdflike * Revert "pdflike" This reverts commitpull/33549/head6d4af1bf9b. * try this * use alert manager * fix that check * wat * Revert "wat" This reverts commit93d0d27ab8. * some clean up * rm * cleanup * move * fix test * more fix * clean up * fix that
Shane Smiskol
8 months ago
committed by
GitHub
parent
9211b701ce
commit
82f8db87f4
10 changed files with 320 additions and 3 deletions
@ -0,0 +1,115 @@ |
||||
#!/usr/bin/env python3 |
||||
import argparse |
||||
import base64 |
||||
import io |
||||
import os |
||||
import time |
||||
from pathlib import Path |
||||
import matplotlib.pyplot as plt |
||||
|
||||
from openpilot.tools.lib.logreader import LogReader |
||||
from openpilot.system.hardware.hw import Paths |
||||
|
||||
|
||||
def report(platform, maneuvers): |
||||
output_path = Path(__file__).resolve().parent / "longitudinal_reports" |
||||
output_fn = output_path / f"{platform}_{time.strftime('%Y%m%d-%H_%M_%S')}.html" |
||||
output_path.mkdir(exist_ok=True) |
||||
with open(output_fn, "w") as f: |
||||
f.write("<h1>Longitudinal maneuver report</h1>\n") |
||||
f.write(f"<h3>{platform}</h3>\n") |
||||
for description, runs in maneuvers: |
||||
print('plotting maneuver:', description, 'runs:', len(runs)) |
||||
f.write("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n") |
||||
f.write(f"<h2>{description}</h2>\n") |
||||
for run, msgs in enumerate(runs): |
||||
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True) |
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True) |
||||
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True) |
||||
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True) |
||||
|
||||
longActive = [m.longActive for m in carControl] |
||||
gasPressed = [m.gasPressed for m in carState] |
||||
brakePressed = [m.brakePressed for m in carState] |
||||
|
||||
maneuver_valid = all(longActive) and not (any(gasPressed) or any(brakePressed)) |
||||
|
||||
_open = 'open' if maneuver_valid else '' |
||||
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '') |
||||
|
||||
f.write(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\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(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) |
||||
# TODO localizer accel |
||||
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(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, gasPressed, label='gasPressed', linewidth=6) |
||||
ax[3].plot(t_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"<img src='data:image/png;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n") |
||||
f.write("</details>\n") |
||||
|
||||
print(f"\nReport written to {output_fn}\n") |
||||
|
||||
|
||||
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)') |
||||
|
||||
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') for seg in segs]) |
||||
|
||||
CP = lr.first('carParams') |
||||
platform = CP.carFingerprint |
||||
print('processing report for', platform) |
||||
|
||||
maneuvers: list[tuple[str, list[list]]] = [] |
||||
active_prev = False |
||||
description_prev = None |
||||
|
||||
for msg in lr: |
||||
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, maneuvers) |
||||
@ -0,0 +1,165 @@ |
||||
#!/usr/bin/env python3 |
||||
from dataclasses import dataclass |
||||
|
||||
from cereal import messaging, car |
||||
from opendbc.car.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 |
||||
|
||||
|
||||
@dataclass |
||||
class Action: |
||||
accel: float # m/s^2 |
||||
duration: float # seconds |
||||
|
||||
|
||||
@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, enabled: bool, standstill: bool) -> float: |
||||
ready = abs(v_ego - self.initial_speed) < 0.3 and enabled and not 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] |
||||
|
||||
self._action_frames += 1 |
||||
|
||||
# reached duration of action |
||||
if self._action_frames > (action.duration / 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 action.accel |
||||
|
||||
@property |
||||
def finished(self): |
||||
return self._finished |
||||
|
||||
@property |
||||
def active(self): |
||||
return self._active |
||||
|
||||
|
||||
MANEUVERS = [ |
||||
Maneuver( |
||||
"creep: alternate between +1m/s^2 and -1m/s^2", |
||||
[ |
||||
Action(1, 2), Action(-1, 2), |
||||
Action(1, 2), Action(-1, 2), |
||||
Action(1, 2), Action(-1, 2), |
||||
], |
||||
repeat=1, |
||||
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) |
||||
|
||||
sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') |
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug']) |
||||
|
||||
maneuvers = iter(MANEUVERS) |
||||
maneuver = None |
||||
|
||||
while True: |
||||
sm.update() |
||||
|
||||
if maneuver is None: |
||||
maneuver = next(maneuvers, None) |
||||
|
||||
alert_msg = messaging.new_message('alertDebug') |
||||
alert_msg.valid = True |
||||
|
||||
plan_send = messaging.new_message('longitudinalPlan') |
||||
plan_send.valid = sm.all_checks() |
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan |
||||
accel = 0 |
||||
cs = sm['carState'] |
||||
v_ego = max(cs.vEgo, 0) |
||||
|
||||
if maneuver is not None: |
||||
accel = maneuver.get_accel(v_ego, cs.cruiseState.enabled, cs.cruiseState.standstill) |
||||
|
||||
if maneuver.active: |
||||
alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2' |
||||
else: |
||||
alert_msg.alertDebug.alertText1 = f'Reaching Speed: {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph' |
||||
alert_msg.alertDebug.alertText2 = f'{maneuver.description}' |
||||
else: |
||||
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished' |
||||
|
||||
pm.send('alertDebug', alert_msg) |
||||
|
||||
longitudinalPlan.aTarget = accel |
||||
longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2 |
||||
|
||||
longitudinalPlan.allowBrake = True |
||||
longitudinalPlan.allowThrottle = True |
||||
longitudinalPlan.hasLead = True |
||||
|
||||
pm.send('longitudinalPlan', plan_send) |
||||
|
||||
assistance_send = messaging.new_message('driverAssistance') |
||||
assistance_send.valid = True |
||||
pm.send('driverAssistance', assistance_send) |
||||
|
||||
if maneuver is not None and maneuver.finished: |
||||
maneuver = None |
||||
Loading…
Reference in new issue