forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ 67fd9c2
This commit is contained in:
IQ.Lvbs CI [bot]
1
iqpilot/tools/maneuvers/.gitignore
vendored
Normal file
1
iqpilot/tools/maneuvers/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
/reports/
|
||||
87
iqpilot/tools/maneuvers/README-lateral.md
Normal file
87
iqpilot/tools/maneuvers/README-lateral.md
Normal file
@@ -0,0 +1,87 @@
|
||||
# Lateral Maneuvers Testing Tool
|
||||
|
||||
> [!WARNING]
|
||||
> Use caution when using this tool.
|
||||
|
||||
Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
|
||||
|
||||
## Instructions
|
||||
|
||||
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
|
||||
2. The full maneuver suite runs at 20 and 30 mph.
|
||||
3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually:
|
||||
|
||||
```sh
|
||||
echo -n 1 > /data/params/d/LateralManeuverMode
|
||||
```
|
||||
|
||||
To run only some of the maneuvers, set `LateralManeuverFilter` to a substring of their
|
||||
descriptions. Aborts mean a session often never reaches the later maneuvers, so target the one
|
||||
you need directly. Unset or unmatched runs the full suite.
|
||||
|
||||
```sh
|
||||
echo -n 'sine 0.5Hz 30mph' > /data/params/d/LateralManeuverFilter # comma's published comparison
|
||||
echo -n '30mph' > /data/params/d/LateralManeuverFilter # all four 30 mph maneuvers
|
||||
```
|
||||
|
||||
4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
|
||||
|
||||
5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages.
|
||||
|
||||
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. 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 iqpilot/tools/maneuvers/lateral_report.py 98395b7c5b27882e/000001cc--5a73bde686
|
||||
|
||||
processing report for KIA_EV6
|
||||
plotting maneuver: step right 20mph, runs: 3
|
||||
plotting maneuver: step left 20mph, runs: 3
|
||||
plotting maneuver: sine 0.5Hz 20mph, runs: 3
|
||||
plotting maneuver: step right 30mph, runs: 3
|
||||
|
||||
Opening report: iqpilot/tools/maneuvers/reports/lateral/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
|
||||
```
|
||||
|
||||
The IQ.Pilot `lateral_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports
|
||||
auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the
|
||||
top-N highest-peak sweeps by speed/peak filters. See `lateral_report.py --help`.
|
||||
|
||||
## Blog-style response plot
|
||||
|
||||
`lateral_response_plot.py` renders the "requested vs actual + 50% response time" figure comma
|
||||
publishes, for one or more routes on the same axes:
|
||||
|
||||
```sh
|
||||
$ python iqpilot/tools/maneuvers/lateral_response_plot.py '<route>' \
|
||||
--maneuver 'sine 0.5Hz 30mph' --label 'IQ.Lvbs angle — VW Golf MK7' --out response.png
|
||||
```
|
||||
|
||||
Three stacked panels sharing a time axis: lateral acceleration (comma's panel, with the 50% marker),
|
||||
steering wheel angle (commanded vs measured, the commanded trace only exists on angle-control cars),
|
||||
and steering wheel rate. `--accel-only` drops to comma's single panel.
|
||||
|
||||
`controlsd` derives curvature as `-calc_curvature(steeringAngleDeg)`, so the raw wheel angle always
|
||||
reads opposite to lateral acceleration. The angle and rate panels are flipped to match the
|
||||
acceleration panel; pass `--raw-angle` to plot the raw log sign instead.
|
||||
|
||||
The 50% response time is only comparable between runs of the **same maneuver at the same speed**. A
|
||||
step and a sine of equal amplitude do not produce comparable numbers: the step's request rises
|
||||
instantly, so its 50% crossing measures rack rise time alone, while the 0.5 Hz sine's own request
|
||||
takes ~0.167 s to reach 50%. comma's published 350 ms (ID.4) and 423 ms (Model Y) are both from the
|
||||
0.5 Hz sine at 30 mph.
|
||||
|
||||
## Testing the tooling without a car
|
||||
|
||||
`simulate_lateral.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
|
||||
rlog that `lateral_report.py` reads. Use it to verify the daemon and the report generator after changing either:
|
||||
|
||||
```sh
|
||||
$ python iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat/rlog.zst
|
||||
$ python iqpilot/tools/maneuvers/lateral_report.py /tmp/lat/rlog.zst
|
||||
```
|
||||
|
||||
The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.
|
||||
60
iqpilot/tools/maneuvers/README-longitudinal.md
Normal file
60
iqpilot/tools/maneuvers/README-longitudinal.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 iqpilot/tools/maneuvers/longitudinal_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 iqpilot/tools/maneuvers/reports/longitudinal/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
|
||||
```
|
||||
|
||||
`longitudinal_report.py` also takes a path to a local `rlog.zst` or a directory of them.
|
||||
|
||||
## Testing the tooling without a car
|
||||
|
||||
`simulate_longitudinal.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog
|
||||
that `longitudinal_report.py` reads. Use it to verify the daemon and the report generator after changing either:
|
||||
|
||||
```sh
|
||||
$ python iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long/rlog.zst
|
||||
$ python iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long/rlog.zst
|
||||
```
|
||||
|
||||
The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.
|
||||
238
iqpilot/tools/maneuvers/lateral_maneuversd.py
Normal file
238
iqpilot/tools/maneuvers/lateral_maneuversd.py
Normal file
@@ -0,0 +1,238 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqpilot.cereal import messaging, car
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.realtime import DT_MDL, Ratekeeper
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
from iqpilot.tools.maneuvers.longitudinal_maneuversd import Action, Maneuver as _Maneuver
|
||||
|
||||
# thresholds for starting maneuvers
|
||||
MAX_SPEED_DEV = 0.7 # deviation in m/s
|
||||
MAX_CURV = 0.004 # 250 m radius
|
||||
MAX_ROLL = 0.12 # 6.8°
|
||||
TIMER = 2.0 # sec stable conditions before starting maneuver
|
||||
|
||||
# The curvature step yanks the rim and spikes driver torque for a frame or two, which single-frame
|
||||
# aborts read as a driver grab. Measured on VW_GOLF_MK7: 9/9 maneuvers died 0.15s in with the driver
|
||||
# near hands-off. The EPS torque signal is also noisy enough to clip the ALC override threshold for
|
||||
# ~10 ms at a time, blipping steeringPressed on its own, so require the hold to exceed 0.2 s of
|
||||
# continuous frames — longer than any sensor blip or step reaction, far shorter than a real grab.
|
||||
STEER_PRESSED_ABORT_S = 0.25
|
||||
STEER_PRESSED_FRAMES = int(STEER_PRESSED_ABORT_S / DT_MDL) # 5 frames at 20 Hz
|
||||
|
||||
|
||||
@dataclass
|
||||
class Maneuver(_Maneuver):
|
||||
_baseline_curvature: float = 0.0
|
||||
|
||||
def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
|
||||
self._run_completed = False
|
||||
# only start maneuver on straight, flat roads
|
||||
ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
|
||||
self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)
|
||||
|
||||
if self._ready_cnt > (TIMER / DT_MDL):
|
||||
if not self._active:
|
||||
self._baseline_curvature = curvature
|
||||
self._active = True
|
||||
|
||||
if not self._active:
|
||||
return 0.0
|
||||
|
||||
return self._step()
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self._ready_cnt = 0
|
||||
|
||||
|
||||
def _sine_action(amplitude, period, duration):
|
||||
t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
|
||||
a = amplitude * np.sin(2 * np.pi * t / period)
|
||||
return Action(a.tolist(), t.tolist())
|
||||
|
||||
|
||||
MANEUVERS = [
|
||||
Maneuver(
|
||||
"step right 20mph",
|
||||
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step left 20mph",
|
||||
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"sine 0.5Hz 20mph",
|
||||
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"jitter 20mph",
|
||||
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step right 30mph",
|
||||
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step left 30mph",
|
||||
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"sine 0.5Hz 30mph",
|
||||
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"jitter 30mph",
|
||||
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def select_maneuvers(params) -> list[Maneuver]:
|
||||
# LateralManeuverFilter runs only the maneuvers whose description contains it, so a session can
|
||||
# target one maneuver (e.g. the 0.5 Hz sine at 30 mph) without driving the whole suite to reach it
|
||||
needle = (params.get("LateralManeuverFilter") or "").strip()
|
||||
if not needle:
|
||||
return MANEUVERS
|
||||
selected = [m for m in MANEUVERS if needle.lower() in m.description.lower()]
|
||||
if not selected:
|
||||
cloudlog.error(f"LateralManeuverFilter {needle!r} matched no maneuvers, running the full suite")
|
||||
return MANEUVERS
|
||||
cloudlog.info(f"LateralManeuverFilter {needle!r} selected: {[m.description for m in selected]}")
|
||||
return selected
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
cloudlog.info("lateral_maneuversd is waiting for CarParams")
|
||||
messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
# iqpilot: subscribe only to the services we actually read and drive timing with a
|
||||
# Ratekeeper instead of polling modelV2. msgq caps each topic at NUM_READERS (15) and
|
||||
# evicts ALL subscribers when exceeded; iqpilot runs many daemons, and unlike longitudinal
|
||||
# maneuver mode (which disables plannerd), lateral mode keeps plannerd running. Subscribing
|
||||
# to selfdriveState/modelV2 here (selfdriveState is unused; modelV2 was only a poll source)
|
||||
# tips those topics past 15 → eviction storm → UI/speed render drops to a few fps.
|
||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'])
|
||||
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
|
||||
rk = Ratekeeper(int(1. / DT_MDL), print_delay_threshold=None) # 20 Hz, matches DT_MDL maneuver timing
|
||||
|
||||
maneuvers = iter(select_maneuvers(params))
|
||||
maneuver = None
|
||||
complete_cnt = 0
|
||||
aborted_cnt = 0
|
||||
steer_pressed_cnt = 0
|
||||
abort_reason = ''
|
||||
display_holdoff = 0
|
||||
prev_text = ''
|
||||
|
||||
while True:
|
||||
sm.update(0)
|
||||
|
||||
if maneuver is None:
|
||||
maneuver = next(maneuvers, None)
|
||||
|
||||
alert_msg = messaging.new_message('alertDebug')
|
||||
alert_msg.valid = True
|
||||
|
||||
plan_send = messaging.new_message('lateralManeuverPlan')
|
||||
|
||||
accel = 0
|
||||
v_ego = max(sm['carState'].vEgo, 0)
|
||||
curvature = sm['controlsState'].desiredCurvature
|
||||
|
||||
if complete_cnt > 0:
|
||||
complete_cnt -= 1
|
||||
alert_msg.alertDebug.alertText1 = 'Completed'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif maneuver is not None:
|
||||
# any driver input aborts the maneuver, but only a sustained hold counts as steering override
|
||||
CS = sm['carState']
|
||||
steer_pressed_cnt = (steer_pressed_cnt + 1) if CS.steeringPressed else 0
|
||||
steer_override = steer_pressed_cnt >= STEER_PRESSED_FRAMES
|
||||
if steer_override or CS.gasPressed:
|
||||
aborted_cnt = int(1.0 / DT_MDL)
|
||||
abort_reason = ('steering pressed' if steer_override else 'gas pressed').ljust(20)
|
||||
aborted = aborted_cnt > 0
|
||||
speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
|
||||
if aborted or speed_out_of_range:
|
||||
maneuver.reset()
|
||||
|
||||
roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
|
||||
|
||||
if maneuver._run_completed:
|
||||
complete_cnt = int(1.0 / DT_MDL)
|
||||
alert_msg.alertDebug.alertText1 = 'Complete'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif maneuver.active:
|
||||
action_remaining = maneuver.actions[maneuver._action_index].time_bp[-1] - maneuver._action_frames * DT_MDL
|
||||
if maneuver.description.startswith('sine'):
|
||||
freq = maneuver.description.split()[1]
|
||||
alert_msg.alertDebug.alertText1 = f'Active sine {freq} {max(action_remaining, 0):.1f}s'
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif aborted_cnt > 0:
|
||||
aborted_cnt -= 1
|
||||
alert_msg.alertDebug.alertText1 = abort_reason
|
||||
elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive):
|
||||
alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph'
|
||||
elif maneuver._ready_cnt > 0:
|
||||
ready_time = max(TIMER - maneuver._ready_cnt * DT_MDL, 0)
|
||||
alert_msg.alertDebug.alertText1 = f'Starting: {int(ready_time) + 1}'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
else:
|
||||
curv_ok = abs(curvature) < MAX_CURV
|
||||
reason = 'road not straight' if not curv_ok else 'road not flat'
|
||||
alert_msg.alertDebug.alertText1 = f'Waiting: {reason}'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
|
||||
|
||||
# prevent flickering text
|
||||
setup = ('Set speed', 'Starting', 'Waiting')
|
||||
text = alert_msg.alertDebug.alertText1
|
||||
same = text == prev_text or (text.startswith('Starting') and prev_text.startswith('Starting'))
|
||||
if not same and text.startswith(setup) and prev_text.startswith(setup) and display_holdoff > 0:
|
||||
alert_msg.alertDebug.alertText1 = prev_text
|
||||
display_holdoff -= 1
|
||||
else:
|
||||
prev_text = text
|
||||
display_holdoff = int(0.5 / DT_MDL) if text.startswith(setup) else 0
|
||||
|
||||
pm.send('alertDebug', alert_msg)
|
||||
|
||||
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
|
||||
if plan_send.valid:
|
||||
plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
|
||||
pm.send('lateralManeuverPlan', plan_send)
|
||||
|
||||
if maneuver is not None and maneuver.finished and complete_cnt == 0:
|
||||
maneuver = None
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
280
iqpilot/tools/maneuvers/lateral_report.py
Normal file
280
iqpilot/tools/maneuvers/lateral_report.py
Normal file
@@ -0,0 +1,280 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import base64
|
||||
import io
|
||||
import math
|
||||
import numpy as np
|
||||
import os
|
||||
import webbrowser
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
import matplotlib.pyplot as plt
|
||||
from iqpilot.common.utils import tabulate
|
||||
|
||||
from iqpilot.cereal import car
|
||||
from iqpilot.common.filter_simple import FirstOrderFilter
|
||||
from iqpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
|
||||
from iqpilot.tools.lib.logreader import LogReader
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.tools.maneuvers.lateral_maneuversd import STEER_PRESSED_FRAMES
|
||||
from iqpilot.tools.maneuvers.longitudinal_report import format_car_params
|
||||
|
||||
ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
|
||||
STEER_OVERRIDE_S = STEER_PRESSED_FRAMES * DT_MDL
|
||||
|
||||
|
||||
def lat_accel(curvature, v):
|
||||
return curvature * max(v, 1.0) ** 2
|
||||
|
||||
|
||||
def steering_overridden(t_carState, carState):
|
||||
# mirrors lateral_maneuversd: the curvature step spikes driver torque for a frame or two on
|
||||
# cars with a tight override threshold, so only a sustained hold invalidates the run
|
||||
start = None
|
||||
for t, cs in zip(t_carState, carState, strict=True):
|
||||
if not cs.steeringPressed:
|
||||
start = None
|
||||
elif start is None:
|
||||
start = t
|
||||
elif t - start >= STEER_OVERRIDE_S:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def report(platform, route, _description, CP, ID, maneuvers):
|
||||
output_path = Path(__file__).resolve().parent / "reports" / "lateral"
|
||||
output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
|
||||
output_path.mkdir(parents=True, exist_ok=True)
|
||||
target_cross_times = defaultdict(list)
|
||||
|
||||
builder = [
|
||||
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
|
||||
"<h1>Lateral 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:
|
||||
# filter incomplete runs
|
||||
completed_runs = [msgs for msgs in runs
|
||||
if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')]
|
||||
print(f'plotting maneuver: {description}, runs: {len(completed_runs)}')
|
||||
if not completed_runs:
|
||||
continue
|
||||
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(completed_runs):
|
||||
last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
|
||||
msgs = [m for m in msgs if m.logMonoTime <= last_active]
|
||||
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
|
||||
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
|
||||
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
|
||||
t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
|
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
|
||||
t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
|
||||
# maneuver validity
|
||||
latActive = [m.latActive for m in carControl]
|
||||
maneuver_valid = all(latActive) and not steering_overridden(t_carState, carState)
|
||||
|
||||
_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")
|
||||
|
||||
baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
|
||||
v_ego = [m.vEgo for m in carState]
|
||||
cross_markers = []
|
||||
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
|
||||
for lp, v in zip(lateralPlan, v_ego, strict=False))
|
||||
threshold = amplitude * 0.5
|
||||
builder.append('<h3 style="font-weight: normal">50% peak')
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
actual = lat_accel(cs.curvature, v) - baseline_accel
|
||||
if abs(actual) > threshold:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
cross_markers.append((t, actual + baseline_accel))
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(t)
|
||||
break
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
else:
|
||||
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
|
||||
for i in range(1, min(len(lateralPlan), len(v_ego))):
|
||||
if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
|
||||
desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
|
||||
action_targets.append((i, desired))
|
||||
|
||||
for j, (start_i, act_target) in enumerate(action_targets):
|
||||
start_time = t_lateralPlan[start_i]
|
||||
end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1]
|
||||
|
||||
builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
|
||||
prev_crossed = False
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
if not (start_time <= t <= end_time):
|
||||
continue
|
||||
actual_accel = lat_accel(cs.curvature, v) - baseline_accel
|
||||
crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel)
|
||||
if crossed and prev_crossed:
|
||||
cross_time = t - start_time
|
||||
builder.append(f', <strong>crossed in {cross_time:.3f}s</strong>')
|
||||
cross_markers.append((t, act_target + baseline_accel))
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(cross_time)
|
||||
break
|
||||
prev_crossed = crossed
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
|
||||
plt.rcParams['font.size'] = 40
|
||||
fig = plt.figure(figsize=(30, 40))
|
||||
ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
|
||||
|
||||
ax[0].grid(linewidth=4)
|
||||
desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
|
||||
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
|
||||
else:
|
||||
t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
|
||||
desired_lat_accel = [baseline_accel] + desired_lat_accel
|
||||
ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
|
||||
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
|
||||
ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
|
||||
ax[0].set_ylabel('Lateral Accel (m/s^2)')
|
||||
for ct, cv in cross_markers:
|
||||
ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
|
||||
ax[0].legend(prop={'size': 30})
|
||||
|
||||
ax[1].grid(linewidth=4)
|
||||
if CP.steerControlType in ANGLE_CONTROL:
|
||||
steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
|
||||
else:
|
||||
steer_field, steer_ylabel = 'torque', 'Steer torque'
|
||||
ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6)
|
||||
ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
|
||||
ax[1].set_ylabel(steer_ylabel)
|
||||
ax[1].legend(prop={'size': 30})
|
||||
|
||||
ax[2].grid(linewidth=4)
|
||||
ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
|
||||
ax[2].set_ylabel('Velocity (mph)')
|
||||
ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f'))
|
||||
ax[2].legend()
|
||||
|
||||
t_accel = np.array(t_controlsState[:len(actual_lat_accel)])
|
||||
raw_jerk = np.gradient(actual_lat_accel, t_accel)
|
||||
dt_avg = np.mean(np.diff(t_accel))
|
||||
jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg)
|
||||
filtered_jerk = [jerk_filter.update(j) for j in raw_jerk]
|
||||
ax[3].grid(linewidth=4)
|
||||
ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6)
|
||||
ax[3].set_ylabel('Jerk (m/s^3)')
|
||||
ax[3].legend()
|
||||
|
||||
ax[4].grid(linewidth=4)
|
||||
ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl],
|
||||
label='carControl.orientationNED[0]', linewidth=6)
|
||||
ax[4].set_ylabel('Roll (deg)')
|
||||
ax[4].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', 'mean', 'min', 'max']
|
||||
table = []
|
||||
for description, times in target_cross_times.items():
|
||||
l = [description, len(times)]
|
||||
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))
|
||||
|
||||
|
||||
def open_route(route: str) -> LogReader:
|
||||
if os.path.isdir(route):
|
||||
rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst"))
|
||||
if not rlogs:
|
||||
raise SystemExit(f"no *rlog.zst files in {route}")
|
||||
print(f"loading {len(rlogs)} rlogs from {route}")
|
||||
return LogReader(rlogs, only_union_types=True)
|
||||
if route.endswith(('.zst', '.bz2', '.log')) and not os.path.exists(route):
|
||||
raise SystemExit(f"no such file: {route}")
|
||||
if os.path.exists(route) or '/' in route or '|' in route:
|
||||
return LogReader(route, only_union_types=True)
|
||||
segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg]
|
||||
return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route')
|
||||
parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs')
|
||||
parser.add_argument('description', type=str, nargs='?')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
lr = open_route(args.route)
|
||||
|
||||
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 = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
|
||||
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)
|
||||
238
iqpilot/tools/maneuvers/lateral_response_plot.py
Executable file
238
iqpilot/tools/maneuvers/lateral_response_plot.py
Executable file
@@ -0,0 +1,238 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Blog-style lateral actuator response plot.
|
||||
|
||||
Renders the single-panel "requested vs actual lateral acceleration + 50% response time" figure
|
||||
comma publishes for lateral maneuver comparisons, for one or more routes on the same axes.
|
||||
|
||||
./iqpilot/tools/maneuvers/lateral_response_plot.py 1ce1b50dd82993a1'|'00000011--6cb007b200/0:4 \
|
||||
--maneuver 'sine 0.5Hz 30mph' --label 'IQ.Lvbs angle (Golf MK7)'
|
||||
|
||||
The 50% response time is only comparable between runs of the SAME maneuver at the SAME speed;
|
||||
a step and a sine of equal amplitude do not produce comparable numbers.
|
||||
"""
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
from typing import NamedTuple
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.tools.maneuvers.lateral_report import lat_accel, open_route, steering_overridden
|
||||
|
||||
SERIES_COLORS = ('#2ca02c', '#ff7f0e', '#1f77b4', '#d62728')
|
||||
REQUESTED_COLOR = '#999999'
|
||||
|
||||
|
||||
def completed_runs(msgs, maneuver):
|
||||
runs, active_prev, desc_prev = [], False, None
|
||||
for m in msgs:
|
||||
if m.which() == 'alertDebug':
|
||||
active = 'Active' in m.alertDebug.alertText1 or m.alertDebug.alertText1 == 'Complete'
|
||||
if active and not active_prev:
|
||||
if m.alertDebug.alertText2 == desc_prev:
|
||||
runs[-1][1].append([])
|
||||
else:
|
||||
runs.append((m.alertDebug.alertText2, [[]]))
|
||||
desc_prev = runs[-1][0]
|
||||
active_prev = active
|
||||
if active_prev:
|
||||
runs[-1][1][-1].append(m)
|
||||
|
||||
out = []
|
||||
for description, windows in runs:
|
||||
if maneuver is not None and description != maneuver:
|
||||
continue
|
||||
for w in windows:
|
||||
if any(m.alertDebug.alertText1 == 'Complete' for m in w if m.which() == 'alertDebug'):
|
||||
out.append((description, w))
|
||||
return out
|
||||
|
||||
|
||||
class Run(NamedTuple):
|
||||
t_requested: np.ndarray
|
||||
requested: np.ndarray
|
||||
t_actual: np.ndarray
|
||||
actual: np.ndarray
|
||||
t_wheel: np.ndarray
|
||||
angle: np.ndarray
|
||||
rate: np.ndarray
|
||||
t_angle_cmd: np.ndarray
|
||||
angle_cmd: np.ndarray | None
|
||||
v_mean: float
|
||||
valid: bool
|
||||
|
||||
|
||||
def extract(msgs, raw_angle: bool = False) -> Run:
|
||||
"""requested/actual lateral accel and wheel angle on a common relative timebase, baseline removed"""
|
||||
t_cs, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
|
||||
t_ct, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
|
||||
t_cc, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
|
||||
t_lp, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs
|
||||
if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
|
||||
|
||||
t0 = t_lp[0]
|
||||
|
||||
def rel(ts):
|
||||
return np.array([(t - t0) / 1e9 for t in ts])
|
||||
|
||||
t_cs_s, t_ct_s, t_cc_s, t_lp_s = rel(t_cs), rel(t_ct), rel(t_cc), rel(t_lp)
|
||||
|
||||
v_ego = np.array([m.vEgo for m in carState])
|
||||
v_at_lp = np.interp(t_lp_s, t_cs_s, v_ego)
|
||||
v_at_ct = np.interp(t_ct_s, t_cs_s, v_ego)
|
||||
|
||||
baseline = lat_accel(controlsState[0].curvature, carState[0].vEgo)
|
||||
requested = np.array([lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_at_lp, strict=True)]) - baseline
|
||||
actual = np.array([lat_accel(m.curvature, v) for m, v in zip(controlsState, v_at_ct, strict=True)]) - baseline
|
||||
|
||||
# controlsd derives curvature as -calc_curvature(steeringAngleDeg), so raw wheel angle always reads
|
||||
# opposite to lateral accel; flip it unless the caller wants the raw signal
|
||||
sign = 1.0 if raw_angle else -1.0
|
||||
angle = sign * (np.array([m.steeringAngleDeg for m in carState]) - carState[0].steeringAngleDeg)
|
||||
rate = sign * np.array([m.steeringRateDeg for m in carState])
|
||||
if not np.any(rate): # not all brands populate steeringRateDeg
|
||||
rate = np.gradient(angle, t_cs_s)
|
||||
|
||||
# angle command only exists on angle-control cars
|
||||
cmd = sign * (np.array([m.actuators.steeringAngleDeg for m in carControl]) - carControl[0].actuators.steeringAngleDeg)
|
||||
angle_cmd = cmd if np.any(cmd) else None
|
||||
|
||||
window = lambda t: (t >= 0) & (t <= t_lp_s[-1]) # noqa: E731
|
||||
k_ct, k_cs, k_cc = window(t_ct_s), window(t_cs_s), window(t_cc_s)
|
||||
|
||||
lat_active = all(m.latActive for m in carControl)
|
||||
# steering_overridden takes seconds, not raw logMonoTime
|
||||
overridden = steering_overridden(t_cs_s.tolist(), carState)
|
||||
return Run(t_lp_s, requested, t_ct_s[k_ct], actual[k_ct],
|
||||
t_cs_s[k_cs], angle[k_cs], rate[k_cs],
|
||||
t_cc_s[k_cc], angle_cmd[k_cc] if angle_cmd is not None else None,
|
||||
float(np.mean(v_ego)), lat_active and not overridden)
|
||||
|
||||
|
||||
def response_time(t_actual, actual, requested):
|
||||
"""time to first reach 50% of the requested peak, comma's metric"""
|
||||
amplitude = float(np.max(np.abs(requested)))
|
||||
if amplitude < 1e-3:
|
||||
return None, amplitude
|
||||
threshold = 0.5 * amplitude
|
||||
crossed = np.flatnonzero(np.abs(actual) > threshold)
|
||||
if not len(crossed):
|
||||
return None, amplitude
|
||||
return float(t_actual[crossed[0]]), amplitude
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
parser.add_argument('routes', nargs='+', help='route, local rlog path, or directory (one per series)')
|
||||
parser.add_argument('--label', action='append', default=[], help='series label, repeat to match routes')
|
||||
parser.add_argument('--maneuver', default='sine 0.5Hz 30mph', help='maneuver description to plot')
|
||||
parser.add_argument('--run', type=int, default=0, help='which completed run to plot (default first)')
|
||||
parser.add_argument('--out', type=Path, default=Path('lateral_response.png'))
|
||||
parser.add_argument('--title', default=None)
|
||||
parser.add_argument('--accel-only', action='store_true',
|
||||
help="just comma's single lateral-accel panel, without wheel angle and rate")
|
||||
parser.add_argument('--raw-angle', action='store_true',
|
||||
help='plot wheel angle in the raw log sign instead of aligned to lateral accel')
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.accel_only:
|
||||
fig, ax0 = plt.subplots(figsize=(9, 5.5), dpi=200)
|
||||
ax_angle = ax_rate = None
|
||||
axes = [ax0]
|
||||
else:
|
||||
fig, axes = plt.subplots(3, 1, figsize=(9, 10), dpi=200, sharex=True,
|
||||
gridspec_kw={'height_ratios': [3, 2, 2]})
|
||||
ax0, ax_angle, ax_rate = axes
|
||||
ax = ax0
|
||||
annotations = []
|
||||
plotted_requested = False
|
||||
plotted_cmd = False
|
||||
|
||||
for i, route in enumerate(args.routes):
|
||||
label = args.label[i] if i < len(args.label) else route
|
||||
color = SERIES_COLORS[i % len(SERIES_COLORS)]
|
||||
|
||||
msgs = list(open_route(route))
|
||||
runs = completed_runs(msgs, args.maneuver)
|
||||
if not runs:
|
||||
have = sorted({d for d, _ in completed_runs(msgs, None)})
|
||||
raise SystemExit(f"{route}: no completed '{args.maneuver}' runs. completed maneuvers in this route: {have or 'none'}")
|
||||
if args.run >= len(runs):
|
||||
raise SystemExit(f"{route}: only {len(runs)} completed '{args.maneuver}' run(s), --run {args.run} out of range")
|
||||
|
||||
description, msgs_run = runs[args.run]
|
||||
r = extract(msgs_run, args.raw_angle)
|
||||
t_req, requested, t_act, actual, v_mean, valid = r.t_requested, r.requested, r.t_actual, r.actual, r.v_mean, r.valid
|
||||
cross, amplitude = response_time(t_act, actual, requested)
|
||||
|
||||
if not plotted_requested:
|
||||
ax.plot(t_req, requested, color=REQUESTED_COLOR, linestyle=':', linewidth=2.5, label='requested', zorder=1)
|
||||
plotted_requested = True
|
||||
ax.plot(t_act, actual, color=color, linewidth=2.5, label=label, zorder=3)
|
||||
|
||||
if ax_angle is not None:
|
||||
if r.angle_cmd is not None and not plotted_cmd:
|
||||
ax_angle.plot(r.t_angle_cmd, r.angle_cmd, color=REQUESTED_COLOR, linestyle=':', linewidth=2.5,
|
||||
label='commanded', zorder=1)
|
||||
plotted_cmd = True
|
||||
ax_angle.plot(r.t_wheel, r.angle, color=color, linewidth=2.5, zorder=3)
|
||||
ax_rate.plot(r.t_wheel, r.rate, color=color, linewidth=2.5, zorder=3)
|
||||
|
||||
if cross is not None:
|
||||
y = float(np.interp(cross, t_act, actual))
|
||||
ax.axvline(cross, color=color, linestyle='--', linewidth=1.5, ymax=0.92, zorder=2)
|
||||
ax.plot(cross, y, marker='o', markersize=9, markeredgewidth=2,
|
||||
markeredgecolor=color, markerfacecolor='none', zorder=4)
|
||||
annotations.append((f'50% response in {cross:.3f} s', color))
|
||||
else:
|
||||
annotations.append(('50% response not reached', color))
|
||||
|
||||
flag = '' if valid else ' (INVALID: lat not active or steering overridden)'
|
||||
cross_str = f'{cross:.3f} s' if cross else 'n/a'
|
||||
print(', '.join([
|
||||
f"{label}: {description}",
|
||||
f"run {args.run}",
|
||||
f"{v_mean * 2.23694:.1f} mph",
|
||||
f"peak requested {amplitude:.2f} m/s^2",
|
||||
f"50% in {cross_str}",
|
||||
f"peak wheel {np.abs(r.angle).max():.1f} deg",
|
||||
f"peak rate {np.abs(r.rate).max():.0f} deg/s{flag}",
|
||||
]))
|
||||
|
||||
# headroom so the annotation block never sits on the trace
|
||||
lo, hi = ax.get_ylim()
|
||||
ax.set_ylim(lo, hi + (hi - lo) * 0.10 * max(len(annotations), 1))
|
||||
for j, (text, color) in enumerate(annotations):
|
||||
ax.text(0.03, 0.965 - j * 0.06, text, transform=ax.transAxes, color=color,
|
||||
fontsize=11, fontweight='bold', va='top',
|
||||
bbox={'facecolor': 'white', 'edgecolor': 'none', 'alpha': 0.75, 'pad': 2})
|
||||
|
||||
ax.set_ylabel('Lateral acceleration (m/s²)')
|
||||
if ax_angle is not None:
|
||||
ax_angle.set_ylabel('Steering wheel angle (deg)')
|
||||
ax_rate.set_ylabel('Steering wheel rate (deg/s)')
|
||||
if not args.raw_angle:
|
||||
fig.text(0.5, 0.005, 'wheel angle sign aligned to lateral accel (raw log sign is inverted; --raw-angle to keep it)',
|
||||
ha='center', fontsize=8, color='#777777')
|
||||
if plotted_cmd:
|
||||
ax_angle.legend(loc='upper right', frameon=False, fontsize=9)
|
||||
axes[-1].set_xlabel('Time (s)')
|
||||
|
||||
for a in axes:
|
||||
a.grid(True, color='#dddddd', linewidth=0.8)
|
||||
a.set_axisbelow(True)
|
||||
for side in ('top', 'right'):
|
||||
a.spines[side].set_visible(False)
|
||||
|
||||
ax.legend(loc='upper center', bbox_to_anchor=(0.5, 1.12 if args.accel_only else 1.10),
|
||||
ncol=4, frameon=False, fontsize=10)
|
||||
if args.title:
|
||||
ax.set_title(args.title, pad=28)
|
||||
|
||||
fig.tight_layout()
|
||||
fig.savefig(args.out, bbox_inches='tight')
|
||||
print(f"\nwrote {args.out}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
200
iqpilot/tools/maneuvers/longitudinal_maneuversd.py
Executable file
200
iqpilot/tools/maneuvers/longitudinal_maneuversd.py
Executable file
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.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
|
||||
187
iqpilot/tools/maneuvers/longitudinal_report.py
Executable file
187
iqpilot/tools/maneuvers/longitudinal_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 iqpilot.common.utils import tabulate
|
||||
|
||||
from iqpilot.tools.lib.logreader import LogReader
|
||||
from iqpilot.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 / "reports" / "longitudinal"
|
||||
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
|
||||
output_path.mkdir(parents=True, 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_deviceMotion, deviceMotion = zip(*[(m.logMonoTime, m.deviceMotion) for m in msgs if m.which() == 'deviceMotion'], 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_deviceMotion = [(t - t_deviceMotion[0]) / 1e9 for t in t_deviceMotion]
|
||||
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_deviceMotion, deviceMotion, 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_deviceMotion, [m.accelerationDevice.x for m in deviceMotion], label='deviceMotion.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
iqpilot/tools/maneuvers/maneuver_helpers.py
Normal file
18
iqpilot/tools/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)'}
|
||||
293
iqpilot/tools/maneuvers/mpc_longitudinal_tuning_report.py
Normal file
293
iqpilot/tools/maneuvers/mpc_longitudinal_tuning_report.py
Normal file
@@ -0,0 +1,293 @@
|
||||
import io
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
|
||||
from iqpilot.tools.maneuvers.maneuver_helpers import Axis, axis_labels
|
||||
from iqpilot.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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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(f'<h1>{name}</h1>')
|
||||
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('<h1>MPC longitudinal tuning report</h1>')
|
||||
for html in htmls:
|
||||
f.write(html)
|
||||
244
iqpilot/tools/maneuvers/sim_harness.py
Normal file
244
iqpilot/tools/maneuvers/sim_harness.py
Normal file
@@ -0,0 +1,244 @@
|
||||
#!/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 the report generators 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 iqpilot.cereal import car, messaging
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
|
||||
PUB_20HZ = ('modelV2', 'deviceMotion', 'vehicleParameters')
|
||||
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
|
||||
self.steering_pressed = False
|
||||
self.gas_pressed = False
|
||||
|
||||
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.steeringPressed = self.steering_pressed
|
||||
cs.gasPressed = self.gas_pressed
|
||||
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 == 'deviceMotion':
|
||||
msg.deviceMotion.accelerationDevice.x = float(self.plant.a_ego)
|
||||
msg.deviceMotion.accelerationDevice.y = float(self.plant.lat_accel)
|
||||
msg.deviceMotion.velocityDevice.x = float(self.plant.v_ego)
|
||||
msg.deviceMotion.inputsOK = True
|
||||
msg.deviceMotion.posenetOK = True
|
||||
msg.deviceMotion.sensorsOK = True
|
||||
elif s == 'vehicleParameters':
|
||||
msg.vehicleParameters.valid = True
|
||||
msg.vehicleParameters.roll = 0.0
|
||||
msg.vehicleParameters.steerRatio = STEER_RATIO
|
||||
return msg
|
||||
87
iqpilot/tools/maneuvers/simulate_lateral.py
Executable file
87
iqpilot/tools/maneuvers/simulate_lateral.py
Executable file
@@ -0,0 +1,87 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
|
||||
|
||||
./iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat_rlog.zst
|
||||
./iqpilot/tools/maneuvers/lateral_report.py /tmp/lat_rlog.zst
|
||||
"""
|
||||
import argparse
|
||||
import re
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.tools.maneuvers.lateral_maneuversd import MANEUVERS
|
||||
from iqpilot.tools.maneuvers.sim_harness import ManeuverSim, Plant
|
||||
|
||||
CURV_TAU = 0.05 # controlsd curvature command tracking
|
||||
RACK_WN = 8.0 # steering rack + tire natural frequency (rad/s)
|
||||
RACK_ZETA = 0.7 # underdamped, so achieved curvature overshoots like a real rack
|
||||
CRUISE_ACCEL = 1.2
|
||||
|
||||
SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph")
|
||||
|
||||
|
||||
class LateralPlant(Plant):
|
||||
PLAN = 'lateralManeuverPlan'
|
||||
|
||||
def __init__(self, steer_input: float = 0.0):
|
||||
super().__init__(v_ego=MANEUVERS[0].initial_speed)
|
||||
self.sim = None
|
||||
self._rack_rate = 0.0
|
||||
self.target_speed = MANEUVERS[0].initial_speed
|
||||
self._by_description = {m.description: m.initial_speed for m in MANEUVERS}
|
||||
# seconds of steeringPressed to assert at each maneuver start, mimicking the driver-torque
|
||||
# spike a curvature step produces on a car with a tight override threshold
|
||||
self.steer_input = steer_input
|
||||
self._steer_hold = 0.0
|
||||
self._was_active = False
|
||||
|
||||
def _update_target(self):
|
||||
if self.sim is None:
|
||||
return
|
||||
speed = self._by_description.get(self.sim.alert2)
|
||||
if speed is None:
|
||||
match = SET_SPEED_RE.search(self.sim.alert1)
|
||||
speed = float(match.group(1)) * CV.MPH_TO_MS if match else None
|
||||
if speed is not None:
|
||||
self.target_speed = speed
|
||||
|
||||
def step(self, dt, plan):
|
||||
self._update_target()
|
||||
|
||||
active = plan is not None
|
||||
if self.steer_input > 0 and active and not self._was_active:
|
||||
self._steer_hold = self.steer_input
|
||||
self._was_active = active
|
||||
self.steering_pressed = self._steer_hold > 0
|
||||
self._steer_hold = max(self._steer_hold - dt, 0.0)
|
||||
|
||||
err = self.target_speed - self.v_ego
|
||||
self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL)
|
||||
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
|
||||
|
||||
desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0
|
||||
self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature)
|
||||
|
||||
self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate)
|
||||
self.achieved_curvature += dt * self._rack_rate
|
||||
self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_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)
|
||||
parser.add_argument("--steer-input", type=float, default=0.0,
|
||||
help="seconds of steeringPressed to assert at each maneuver start (0 = hands off)")
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = ManeuverSim("iqpilot.tools.maneuvers.lateral_maneuversd", LateralPlant(args.steer_input),
|
||||
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()
|
||||
51
iqpilot/tools/maneuvers/simulate_longitudinal.py
Executable file
51
iqpilot/tools/maneuvers/simulate_longitudinal.py
Executable file
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
|
||||
|
||||
./iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long_rlog.zst
|
||||
./iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long_rlog.zst
|
||||
"""
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.tools.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("iqpilot.tools.maneuvers.longitudinal_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