mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-16 10:22:07 +08:00
Shane Smiskol
29e55f99a5
* start at param * start by sending personality * change to personality * POC: button changes personality * what's wrong with this? * fix * not really possible but fuzzy test catches this * there's always a typo * dang, we're dropping messages * clean up * no comment * bump * rename * not all cars yet * works but at what cost * clean up * inside settings * write param so we save the distance button changes * setChecked activates buttonToggled and already writes param! * don't need this, we update from longitudinalPlan on changes * some clean up * more * ui * allow some time for ui to receive and write param * plannerd: only track changes in case no ui * Revert "plannerd: only track changes in case no ui" This reverts commit 2b081aa6ceb92c67a621b74592b2292756d29871. * write in plannerd as well, I assume this is atomic? * don't write when setting checked (only user clicks) * better nane * more * Update selfdrive/controls/lib/longitudinal_planner.py Co-authored-by: Cameron Clough <cameronjclough@gmail.com> * doesn't write param now * ParamWatcher is nice * no debug * Update translations * fix * odd drain sock proc replay behavior * vanish * Revert "odd drain sock proc replay behavior" This reverts commit 29b70b39413e1852bb512155af6b6a94a5bd9454. * add GM * only if OP long * move personality to controlsState, since eventually it won't be exclusive to long planner more bump * diff without translations * fix * put nonblocking * CS should start at up to date personality always (no ui flicker) * update toggle on cereal message change * fix * fix that * ubmp * mypy doesn't know this is an int :( * update translations * fix the tests * revert ui * not here * migrate controlsState * Revert "migrate controlsState" - i see no reason we need to test with any specific personality This reverts commit 6063508f2df1a5623f113cda34dcd59a1f4b2ac9. * Update ref_commit --------- Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
74 lines
2.8 KiB
Python
74 lines
2.8 KiB
Python
import numpy as np
|
|
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
|
|
|
|
|
class Maneuver:
|
|
def __init__(self, title, duration, **kwargs):
|
|
# Was tempted to make a builder class
|
|
self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
|
|
self.speed = kwargs.get("initial_speed", 0.0)
|
|
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
|
|
|
|
self.breakpoints = kwargs.get("breakpoints", [0.0, duration])
|
|
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))])
|
|
self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))])
|
|
self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
|
|
|
|
self.only_lead2 = kwargs.get("only_lead2", False)
|
|
self.only_radar = kwargs.get("only_radar", False)
|
|
self.ensure_start = kwargs.get("ensure_start", False)
|
|
self.enabled = kwargs.get("enabled", True)
|
|
self.e2e = kwargs.get("e2e", False)
|
|
self.personality = kwargs.get("personality", 0)
|
|
self.force_decel = kwargs.get("force_decel", False)
|
|
|
|
self.duration = duration
|
|
self.title = title
|
|
|
|
def evaluate(self):
|
|
plant = Plant(
|
|
lead_relevancy=self.lead_relevancy,
|
|
speed=self.speed,
|
|
distance_lead=self.distance_lead,
|
|
enabled=self.enabled,
|
|
only_lead2=self.only_lead2,
|
|
only_radar=self.only_radar,
|
|
e2e=self.e2e,
|
|
personality=self.personality,
|
|
force_decel=self.force_decel,
|
|
)
|
|
|
|
valid = True
|
|
logs = []
|
|
while plant.current_time < self.duration:
|
|
speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values)
|
|
prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values)
|
|
cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values)
|
|
log = plant.step(speed_lead, prob, cruise)
|
|
|
|
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
|
|
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
|
|
log['d_rel'] = d_rel
|
|
log['v_rel'] = v_rel
|
|
logs.append(np.array([plant.current_time,
|
|
log['distance'],
|
|
log['distance_lead'],
|
|
log['speed'],
|
|
speed_lead,
|
|
log['acceleration']]))
|
|
|
|
if d_rel < .4 and (self.only_radar or prob > 0.5):
|
|
print("Crashed!!!!")
|
|
valid = False
|
|
|
|
if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
|
|
print('LongitudinalPlanner not starting!')
|
|
valid = False
|
|
if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
|
|
print('Not stopping with force decel')
|
|
valid = False
|
|
|
|
|
|
print("maneuver end", valid)
|
|
return valid, np.array(logs)
|