IQ.Pilot Release Commit @ 0798119
This commit is contained in:
IQ.Lvbs history cleanup
1
tools/longitudinal_maneuvers/.gitignore
vendored
Normal file
1
tools/longitudinal_maneuvers/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
/longitudinal_reports/
|
||||
60
tools/longitudinal_maneuvers/README.md
Normal file
60
tools/longitudinal_maneuvers/README.md
Normal file
@@ -0,0 +1,60 @@
|
||||
# Longitudinal Maneuvers Testing Tool
|
||||
|
||||
Test your vehicle's longitudinal control tuning with this tool. The tool will test the vehicle's ability to follow a few longitudinal maneuvers and includes a tool to generate a report from the route.
|
||||
|
||||
<details><summary>Sample snapshot of a report.</summary><img width="600px" src="https://github.com/user-attachments/assets/d18d0c7d-2bde-44c1-8e86-1741ed442ad8"></details>
|
||||
|
||||
## Instructions
|
||||
|
||||
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
|
||||
2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage IQ.Pilot between maneuvers and turn around if there is not enough space.
|
||||
3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually:
|
||||
|
||||
```sh
|
||||
echo -n 1 > /data/params/d/LongitudinalManeuverMode
|
||||
```
|
||||
|
||||
4. Turn your vehicle back on. You will see the "Longitudinal Maneuver Mode" alert:
|
||||
|
||||
|
||||
|
||||
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.
|
||||
|
||||
|
||||
|
||||
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
|
||||
|
||||
|
||||
|
||||
7. Visit https://connect.comma.ai and locate the route(s). They will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
|
||||
|
||||
|
||||
|
||||
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
|
||||
|
||||
```sh
|
||||
$ python tools/longitudinal_maneuvers/generate_report.py 57048cfce01d9625/0000010e--5b26bc3be7 'pcm accel compensation'
|
||||
|
||||
processing report for LEXUS_ES_TSS2
|
||||
plotting maneuver: start from stop, runs: 4
|
||||
plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
|
||||
plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
|
||||
|
||||
Report written to tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
|
||||
```
|
||||
|
||||
`generate_report.py` also takes a path to a local `rlog.zst` or a directory of them.
|
||||
|
||||
## Testing the tooling without a car
|
||||
|
||||
`sim_maneuvers.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog
|
||||
that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either:
|
||||
|
||||
```sh
|
||||
$ python tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long/rlog.zst
|
||||
$ python tools/longitudinal_maneuvers/generate_report.py /tmp/long/rlog.zst
|
||||
```
|
||||
|
||||
The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.
|
||||
187
tools/longitudinal_maneuvers/generate_report.py
Executable file
187
tools/longitudinal_maneuvers/generate_report.py
Executable file
@@ -0,0 +1,187 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import base64
|
||||
import io
|
||||
import os
|
||||
import math
|
||||
import pprint
|
||||
import webbrowser
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.common.utils import tabulate
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
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 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)
|
||||
|
||||
builder = [
|
||||
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
|
||||
"<h1>Longitudinal maneuver report</h1>\n",
|
||||
f"<h3>{platform}</h3>\n",
|
||||
f"<h3>{route}</h3>\n",
|
||||
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
|
||||
]
|
||||
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('{ summary }') # to be replaced below
|
||||
for description, runs in maneuvers:
|
||||
print(f'plotting maneuver: {description}, runs: {len(runs)}')
|
||||
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
|
||||
builder.append(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_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], strict=True)
|
||||
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose]
|
||||
t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
|
||||
|
||||
# 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)
|
||||
|
||||
_open = 'open' if maneuver_valid else ''
|
||||
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
|
||||
|
||||
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
|
||||
|
||||
# get first acceleration target and first intersection
|
||||
aTarget = longitudinalPlan[0].aTarget
|
||||
target_cross_time = None
|
||||
builder.append(f'<h3 style="font-weight: normal">Initial aTarget: {round(aTarget, 2)} m/s^2')
|
||||
|
||||
# Localizer is noisy, require two consecutive 20Hz frames above threshold
|
||||
prev_crossed = False
|
||||
for t, lp in zip(t_livePose, livePose, strict=True):
|
||||
crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x)
|
||||
if crossed and prev_crossed:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
target_cross_time = t
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(t)
|
||||
break
|
||||
prev_crossed = crossed
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
|
||||
pitches = [math.degrees(m.orientationNED[1]) for m in carControl]
|
||||
builder.append(f'<h3 style="font-weight: normal">Average pitch: <strong>{sum(pitches) / len(pitches):0.2f} degrees</strong></h3>')
|
||||
|
||||
plt.rcParams['font.size'] = 40
|
||||
fig = plt.figure(figsize=(30, 26))
|
||||
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'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)
|
||||
ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6)
|
||||
# TODO localizer accel
|
||||
ax[0].set_ylabel('Acceleration (m/s^2)')
|
||||
#ax[0].set_ylim(-6.5, 6.5)
|
||||
ax[0].legend(prop={'size': 30})
|
||||
|
||||
if target_cross_time is not None:
|
||||
ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
|
||||
|
||||
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, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
|
||||
ax[3].plot(t_carState, [m.brakePressed for m in carState], 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='webp')
|
||||
plt.close(fig)
|
||||
buffer.seek(0)
|
||||
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
|
||||
builder.append("</details>\n")
|
||||
|
||||
summary = ["<h2>Summary</h2>\n"]
|
||||
cols = ['maneuver', 'crossed', 'runs', 'mean', 'min', 'max']
|
||||
table = []
|
||||
for description, runs in maneuvers:
|
||||
times = target_cross_times[description]
|
||||
l = [description, len(times), len(runs)]
|
||||
if len(times):
|
||||
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
|
||||
table.append(l)
|
||||
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
|
||||
|
||||
sum_idx = builder.index('{ summary }')
|
||||
builder[sum_idx:sum_idx + 1] = summary
|
||||
|
||||
with open(output_fn, "w") as f:
|
||||
f.write(''.join(builder))
|
||||
|
||||
print(f"\nOpening report: {output_fn}\n")
|
||||
webbrowser.open_new_tab(str(output_fn))
|
||||
|
||||
|
||||
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)')
|
||||
parser.add_argument('description', type=str, nargs='?')
|
||||
|
||||
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.zst') for seg in segs])
|
||||
|
||||
CP = lr.first('carParams')
|
||||
ID = lr.first('initData')
|
||||
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, args.route, args.description, CP, ID, maneuvers)
|
||||
18
tools/longitudinal_maneuvers/maneuver_helpers.py
Normal file
18
tools/longitudinal_maneuvers/maneuver_helpers.py
Normal file
@@ -0,0 +1,18 @@
|
||||
from enum import IntEnum
|
||||
|
||||
class Axis(IntEnum):
|
||||
TIME = 0
|
||||
EGO_POSITION = 1
|
||||
LEAD_DISTANCE= 2
|
||||
EGO_V = 3
|
||||
LEAD_V = 4
|
||||
EGO_A = 5
|
||||
D_REL = 6
|
||||
|
||||
axis_labels = {Axis.TIME: 'Time (s)',
|
||||
Axis.EGO_POSITION: 'Ego position (m)',
|
||||
Axis.LEAD_DISTANCE: 'Lead absolute position (m)',
|
||||
Axis.EGO_V: 'Ego Velocity (m/s)',
|
||||
Axis.LEAD_V: 'Lead Velocity (m/s)',
|
||||
Axis.EGO_A: 'Ego acceleration (m/s^2)',
|
||||
Axis.D_REL: 'Lead distance (m)'}
|
||||
200
tools/longitudinal_maneuvers/maneuversd.py
Executable file
200
tools/longitudinal_maneuvers/maneuversd.py
Executable file
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from cereal import messaging
|
||||
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.selfdrive.controls.lib.drive_helpers import should_stop
|
||||
|
||||
|
||||
@dataclass
|
||||
class Action:
|
||||
accel_bp: list[float] # m/s^2
|
||||
time_bp: list[float] # seconds
|
||||
|
||||
def __post_init__(self):
|
||||
assert len(self.accel_bp) == len(self.time_bp)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Maneuver:
|
||||
description: str
|
||||
actions: list[Action]
|
||||
repeat: int = 0
|
||||
initial_speed: float = 0. # m/s
|
||||
|
||||
_active: bool = False
|
||||
_finished: bool = False
|
||||
_run_completed: bool = False
|
||||
_action_index: int = 0
|
||||
_action_frames: int = 0
|
||||
_ready_cnt: int = 0
|
||||
_repeated: int = 0
|
||||
|
||||
def _step(self) -> float:
|
||||
self._run_completed = False
|
||||
action = self.actions[self._action_index]
|
||||
action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
|
||||
|
||||
self._action_frames += 1
|
||||
|
||||
# reached duration of action
|
||||
if self._action_frames > (action.time_bp[-1] / 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._run_completed = True
|
||||
self.reset()
|
||||
# finish maneuver
|
||||
else:
|
||||
self._run_completed = True
|
||||
self._finished = True
|
||||
|
||||
return float(action_accel)
|
||||
|
||||
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
|
||||
if self.initial_speed < 0.01:
|
||||
ready = ready and 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.)
|
||||
|
||||
return self._step()
|
||||
|
||||
def reset(self):
|
||||
self._active = False
|
||||
self._action_frames = 0
|
||||
self._action_index = 0
|
||||
|
||||
@property
|
||||
def finished(self):
|
||||
return self._finished
|
||||
|
||||
@property
|
||||
def active(self):
|
||||
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 main():
|
||||
params = Params()
|
||||
cloudlog.info("maneuversd is waiting for CarParams")
|
||||
params.get("CarParams", block=True)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'iqPlan', '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
|
||||
v_ego = max(sm['carState'].vEgo, 0)
|
||||
|
||||
if maneuver is not None:
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)
|
||||
|
||||
if maneuver.active:
|
||||
alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = f'Setting up to {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 = should_stop(v_ego, accel)
|
||||
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = True
|
||||
longitudinalPlan.hasLead = True
|
||||
|
||||
longitudinalPlan.speeds = [0.2] # triggers carControl.cruiseControl.resume in controlsd
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
||||
plan_iq_send = messaging.new_message('iqPlan')
|
||||
plan_iq_send.valid = True
|
||||
pm.send('iqPlan', plan_iq_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
|
||||
294
tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py
Normal file
294
tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py
Normal file
@@ -0,0 +1,294 @@
|
||||
import io
|
||||
import sys
|
||||
import markdown
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
|
||||
from openpilot.tools.longitudinal_maneuvers.maneuver_helpers import Axis, axis_labels
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
|
||||
def get_html_from_results(results, labels, AXIS):
|
||||
fig, ax = plt.subplots(figsize=(16, 8))
|
||||
for idx, key in enumerate(results.keys()):
|
||||
ax.plot(results[key][:, Axis.TIME], results[key][:, AXIS], label=labels[idx])
|
||||
|
||||
ax.set_xlabel(axis_labels[Axis.TIME])
|
||||
ax.set_ylabel(axis_labels[AXIS])
|
||||
ax.legend(bbox_to_anchor=(1.02, 1), loc='upper left', borderaxespad=0)
|
||||
ax.grid(True, linestyle='--', alpha=0.7)
|
||||
ax.text(-0.075, 0.5, '.', transform=ax.transAxes, color='none')
|
||||
|
||||
fig_buffer = io.StringIO()
|
||||
fig.savefig(fig_buffer, format='svg', bbox_inches='tight')
|
||||
plt.close(fig)
|
||||
return fig_buffer.getvalue() + '<br/>'
|
||||
|
||||
|
||||
def generate_mpc_tuning_report():
|
||||
htmls = []
|
||||
|
||||
results = {}
|
||||
name = 'Resuming behind lead'
|
||||
labels = []
|
||||
for lead_accel in np.linspace(1.0, 4.0, 4):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=11,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 10 * lead_accel],
|
||||
cruise_values=[100, 100],
|
||||
prob_lead_values=[1.0, 1.0],
|
||||
breakpoints=[1., 11],
|
||||
)
|
||||
valid, results[lead_accel] = man.evaluate()
|
||||
labels.append(f'{lead_accel} m/s^2 lead acceleration')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Approaching stopped car from 140m'
|
||||
labels = []
|
||||
for speed in np.arange(0, 45, 5):
|
||||
man = Maneuver(
|
||||
name,
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=140.,
|
||||
speed_lead_values=[0.0, 0.],
|
||||
breakpoints=[0., 30.],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s approach speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following 5s (triangular) oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil],
|
||||
breakpoints=[0., 2., 5, 8, 15, 18, 25.],
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscillation size')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following 5s (sinusoidal) oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
duration = float(30)
|
||||
f_osc = 1. / 5
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
bps = DT_MDL * np.arange(int(duration / DT_MDL))
|
||||
lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps)
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=duration,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=lead_speeds,
|
||||
breakpoints=bps,
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscillation size')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 30m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[30.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 20m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=20.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[20.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following car at 30m/s that comes to a stop'
|
||||
labels = []
|
||||
for stop_time in np.arange(4, 14, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30,
|
||||
initial_speed=30.0,
|
||||
cruise_values=[30.0, 30.0, 30.0],
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=60.0,
|
||||
speed_lead_values=[30.0, 30.0, 0.0],
|
||||
breakpoints=[0., 5., 5 + stop_time],
|
||||
)
|
||||
valid, results[stop_time] = man.evaluate()
|
||||
labels.append(f'{stop_time} seconds stop time')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Response to cut-in at half follow distance'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=20,
|
||||
initial_speed=float(speed),
|
||||
cruise_values=[speed, speed, speed],
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed) / 2,
|
||||
speed_lead_values=[speed, speed, speed],
|
||||
prob_lead_values=[0.0, 0.0, 1.0],
|
||||
breakpoints=[0., 5.0, 5.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=60,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0, speed],
|
||||
prob_lead_values=[1.0, 1.0, 1.0],
|
||||
breakpoints=[0., 1.0, speed / 2],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From stop to cruise'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[0.0, speed],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From cruise to min'
|
||||
labels = []
|
||||
for speed in np.arange(10, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[speed, 10.0],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
return htmls
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
htmls = generate_mpc_tuning_report()
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
file_name = 'long_mpc_tune_report.html'
|
||||
else:
|
||||
file_name = sys.argv[1]
|
||||
|
||||
with open(file_name, 'w') as f:
|
||||
f.write(markdown.markdown('# MPC longitudinal tuning report'))
|
||||
for html in htmls:
|
||||
f.write(html)
|
||||
240
tools/longitudinal_maneuvers/sim_harness.py
Normal file
240
tools/longitudinal_maneuvers/sim_harness.py
Normal file
@@ -0,0 +1,240 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Closed-loop offline harness for the maneuver daemons.
|
||||
|
||||
Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a
|
||||
synthetic vehicle, and records every message to an rlog that generate_report.py can read.
|
||||
Used to validate the maneuver tooling without a car.
|
||||
"""
|
||||
import math
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import NamedTuple
|
||||
|
||||
import numpy as np
|
||||
import zstandard as zstd
|
||||
|
||||
from cereal import car, messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
|
||||
PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
|
||||
PUB_20HZ = ('modelV2', 'livePose', 'liveParameters')
|
||||
SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan')
|
||||
|
||||
STEER_RATIO = 15.0
|
||||
WHEELBASE = 2.78
|
||||
|
||||
|
||||
class LongPlan(NamedTuple):
|
||||
aTarget: float
|
||||
shouldStop: bool
|
||||
|
||||
|
||||
class LatPlan(NamedTuple):
|
||||
desiredCurvature: float
|
||||
|
||||
|
||||
class Plant:
|
||||
"""Vehicle model. Subclasses consume the daemon's plan and fill the published messages."""
|
||||
|
||||
sim = None
|
||||
PLAN = 'longitudinalPlan'
|
||||
|
||||
def __init__(self, v_ego: float = 0.0):
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = 0.0
|
||||
self.curvature = 0.0 # commanded, controlsState.desiredCurvature
|
||||
self.achieved_curvature = 0.0 # measured, controlsState.curvature
|
||||
self.lat_accel = 0.0
|
||||
self.long_active = True
|
||||
self.lat_active = True
|
||||
|
||||
def step(self, dt: float, plan) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def _angle(self, curvature: float) -> float:
|
||||
return math.degrees(curvature * WHEELBASE * STEER_RATIO)
|
||||
|
||||
def _torque(self, curvature: float) -> float:
|
||||
return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0))
|
||||
|
||||
def fill_car_state(self, cs) -> None:
|
||||
cs.vEgo = float(self.v_ego)
|
||||
cs.vEgoRaw = float(self.v_ego)
|
||||
cs.vEgoCluster = float(self.v_ego)
|
||||
cs.aEgo = float(self.a_ego)
|
||||
cs.standstill = self.v_ego < 0.01
|
||||
cs.steeringAngleDeg = self._angle(self.achieved_curvature)
|
||||
cs.cruiseState.enabled = True
|
||||
cs.cruiseState.available = True
|
||||
cs.cruiseState.speed = float(max(self.v_ego, 1.0))
|
||||
|
||||
def fill_car_control(self, cc) -> None:
|
||||
cc.enabled = True
|
||||
cc.latActive = self.lat_active
|
||||
cc.longActive = self.long_active
|
||||
cc.orientationNED = [0.0, 0.0, 0.0]
|
||||
cc.actuators.curvature = float(self.curvature)
|
||||
cc.actuators.accel = float(self.a_ego)
|
||||
cc.actuators.steeringAngleDeg = self._angle(self.curvature)
|
||||
cc.actuators.torque = self._torque(self.curvature)
|
||||
|
||||
|
||||
class ManeuverSim:
|
||||
def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA",
|
||||
max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True):
|
||||
self.module = module
|
||||
self.plant = plant
|
||||
plant.sim = self
|
||||
self.fingerprint = fingerprint
|
||||
self.max_maneuvers = max_maneuvers
|
||||
self.timeout = timeout
|
||||
self.verbose = verbose
|
||||
|
||||
self.events: list[bytes] = []
|
||||
self.alert1 = ''
|
||||
self.alert2 = ''
|
||||
self.seen_maneuvers: list[str] = []
|
||||
self.finished = False
|
||||
|
||||
def _write_car_params(self):
|
||||
CP = car.CarParams.new_message()
|
||||
CP.carFingerprint = self.fingerprint
|
||||
CP.brand = "toyota"
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP.autoResumeSng = True
|
||||
CP.steerRatio = STEER_RATIO
|
||||
CP.wheelbase = WHEELBASE
|
||||
Params().put("CarParams", CP.to_bytes())
|
||||
return CP
|
||||
|
||||
def _head_events(self, CP):
|
||||
init = messaging.new_message('initData')
|
||||
init.valid = True
|
||||
init.initData.gitCommit = "simulated"
|
||||
init.initData.gitBranch = "sim"
|
||||
init.initData.gitRemote = "iqpilot-sim"
|
||||
self.events.append(init.to_bytes())
|
||||
|
||||
cpm = messaging.new_message('carParams')
|
||||
cpm.valid = True
|
||||
cpm.carParams = CP
|
||||
self.events.append(cpm.to_bytes())
|
||||
|
||||
def _launch(self):
|
||||
env = dict(os.environ)
|
||||
env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "")
|
||||
return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"],
|
||||
cwd=str(BASEDIR), env=env, start_new_session=True)
|
||||
|
||||
def _on_alert(self, ad):
|
||||
text1, text2 = ad.alertText1, ad.alertText2
|
||||
if (text1, text2) != (self.alert1, self.alert2):
|
||||
if self.verbose:
|
||||
print(f" [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}")
|
||||
if text2 and text2 not in self.seen_maneuvers:
|
||||
self.seen_maneuvers.append(text2)
|
||||
if text1 == 'Maneuvers Finished':
|
||||
self.finished = True
|
||||
self.alert1, self.alert2 = text1, text2
|
||||
|
||||
def run(self, out: Path) -> Path:
|
||||
self._head_events(self._write_car_params())
|
||||
|
||||
pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ))
|
||||
socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB}
|
||||
|
||||
proc = self._launch()
|
||||
self.t_start = time.monotonic()
|
||||
rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None)
|
||||
|
||||
plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None}
|
||||
frame = 0
|
||||
try:
|
||||
while True:
|
||||
for s, sock in socks.items():
|
||||
while True:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
break
|
||||
self.events.append(raw)
|
||||
evt = messaging.log_from_bytes(raw)
|
||||
if s == 'alertDebug':
|
||||
self._on_alert(evt.alertDebug)
|
||||
elif s == 'longitudinalPlan':
|
||||
plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop)
|
||||
elif s == 'lateralManeuverPlan':
|
||||
plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None
|
||||
|
||||
self.plant.step(DT_CTRL, plans[self.plant.PLAN])
|
||||
|
||||
for s in PUB_100HZ:
|
||||
raw = self._build(s).to_bytes()
|
||||
self.events.append(raw)
|
||||
pm.send(s, raw)
|
||||
|
||||
if frame % 5 == 0:
|
||||
for s in PUB_20HZ:
|
||||
raw = self._build(s).to_bytes()
|
||||
self.events.append(raw)
|
||||
pm.send(s, raw)
|
||||
|
||||
frame += 1
|
||||
if self.finished:
|
||||
break
|
||||
if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers:
|
||||
break
|
||||
if time.monotonic() - self.t_start > self.timeout:
|
||||
print(" timed out")
|
||||
break
|
||||
rk.keep_time()
|
||||
finally:
|
||||
if proc.poll() is None:
|
||||
os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
|
||||
proc.wait(timeout=5)
|
||||
for sock in socks.values():
|
||||
del sock
|
||||
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
out.write_bytes(zstd.compress(b"".join(self.events), 10))
|
||||
return out
|
||||
|
||||
def _build(self, s: str):
|
||||
msg = messaging.new_message(s)
|
||||
msg.valid = True
|
||||
if s == 'carState':
|
||||
self.plant.fill_car_state(msg.carState)
|
||||
elif s == 'carControl':
|
||||
self.plant.fill_car_control(msg.carControl)
|
||||
elif s == 'carOutput':
|
||||
msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego)
|
||||
msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature)
|
||||
msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature)
|
||||
msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature)
|
||||
elif s == 'controlsState':
|
||||
msg.controlsState.curvature = float(self.plant.achieved_curvature)
|
||||
msg.controlsState.desiredCurvature = float(self.plant.curvature)
|
||||
elif s == 'selfdriveState':
|
||||
msg.selfdriveState.enabled = True
|
||||
msg.selfdriveState.active = True
|
||||
msg.selfdriveState.state = 'enabled'
|
||||
elif s == 'modelV2':
|
||||
msg.modelV2.frameId = 0
|
||||
msg.modelV2.action.desiredCurvature = 0.0
|
||||
elif s == 'livePose':
|
||||
msg.livePose.accelerationDevice.x = float(self.plant.a_ego)
|
||||
msg.livePose.accelerationDevice.y = float(self.plant.lat_accel)
|
||||
msg.livePose.velocityDevice.x = float(self.plant.v_ego)
|
||||
msg.livePose.inputsOK = True
|
||||
msg.livePose.posenetOK = True
|
||||
msg.livePose.sensorsOK = True
|
||||
elif s == 'liveParameters':
|
||||
msg.liveParameters.valid = True
|
||||
msg.liveParameters.roll = 0.0
|
||||
msg.liveParameters.steerRatio = STEER_RATIO
|
||||
return msg
|
||||
51
tools/longitudinal_maneuvers/sim_maneuvers.py
Executable file
51
tools/longitudinal_maneuvers/sim_maneuvers.py
Executable file
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
|
||||
|
||||
./tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long_rlog.zst
|
||||
./tools/longitudinal_maneuvers/generate_report.py /tmp/long_rlog.zst
|
||||
"""
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant
|
||||
|
||||
WN = 6.0 # powertrain natural frequency (rad/s)
|
||||
ZETA = 0.6 # underdamped, so actual accel overshoots the target like a real car
|
||||
|
||||
|
||||
class LongitudinalPlant(Plant):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.jerk = 0.0
|
||||
|
||||
def step(self, dt, plan):
|
||||
a_target = float(plan.aTarget) if plan is not None else 0.0
|
||||
if plan is not None and plan.shouldStop:
|
||||
a_target = min(a_target, -0.5)
|
||||
|
||||
self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk)
|
||||
self.a_ego += dt * self.jerk
|
||||
|
||||
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
|
||||
if self.v_ego <= 0.0:
|
||||
self.a_ego = min(self.a_ego, 0.0)
|
||||
self.jerk = min(self.jerk, 0.0)
|
||||
self.lat_accel = 0.0
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_maneuvers_sim/rlog.zst"))
|
||||
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
|
||||
parser.add_argument("--timeout", type=float, default=900.0)
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = ManeuverSim("openpilot.tools.longitudinal_maneuvers.maneuversd", LongitudinalPlant(),
|
||||
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
|
||||
out = sim.run(args.out)
|
||||
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
|
||||
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user