mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

long maneuvers: add start from stop (#33556) 

* long maneuvers: add start from stop

* fix

* git ignore
This commit is contained in:
Adeeb Shihadeh 2024-09-13 15:58:00 -07:00 committed by GitHub
parent 3da5ddae71
commit 6ec603872b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
tools/longitudinal_maneuvers/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/longitudinal_reports/

16
tools/longitudinal_maneuvers/maneuversd.py
View File

@ -28,8 +28,10 @@ class Maneuver:
_ready_cnt: int = 0
_repeated: int = 0
def get_accel(self, v_ego: float, long_active: bool, standstill: bool) -> float:
ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not standstill
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 = v_ego < 0.1 and standstill
self._ready_cnt = (self._ready_cnt + 1) if ready else 0
if self._ready_cnt > (3. / DT_MDL):
@ -70,6 +72,12 @@ class Maneuver:
MANEUVERS = [
Maneuver(
"start from stop",
[Action(1.5, 3)],
repeat=3,
initial_speed=0.,
),
Maneuver(
"creep: alternate between +1m/s^2 and -1m/s^2",
[
@ -135,12 +143,12 @@ def main():
v_ego = max(sm['carState'].vEgo, 0)
if maneuver is not None:
accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].cruiseState.standstill)
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'Reaching Speed: {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
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'