From 6ec603872b9c4e73cf3e377e6060c75605cb199d Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Fri, 13 Sep 2024 15:58:00 -0700
Subject: [PATCH] long maneuvers: add start from stop (#33556)

* long maneuvers: add start from stop

* fix

* git ignore
---
 tools/longitudinal_maneuvers/.gitignore    |  1 +
 tools/longitudinal_maneuvers/maneuversd.py | 16 ++++++++++++----
 2 files changed, 13 insertions(+), 4 deletions(-)
 create mode 100644 tools/longitudinal_maneuvers/.gitignore

diff --git a/tools/longitudinal_maneuvers/.gitignore b/tools/longitudinal_maneuvers/.gitignore
new file mode 100644
index 0000000000..e819fa6268
--- /dev/null
+++ b/tools/longitudinal_maneuvers/.gitignore
@@ -0,0 +1 @@
+/longitudinal_reports/
diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py
index 892d58da00..5096c19a0b 100755
--- a/tools/longitudinal_maneuvers/maneuversd.py
+++ b/tools/longitudinal_maneuvers/maneuversd.py
@@ -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'