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