Stock longitudinal: spam resume button when lead starts moving (#24873)

* always log leads, we hide them in ui

* only spam resume when future is > vEgoStarting

* do rest but vw

* vw

* remove comments

* rename to resume

* maintain original button msg rate

* mazda: ensure no resume if cancelling

* same for non-HDA2

* Always run planner if not opLong

* try 0.2

* 0.1 should be pretty safe

* add test for resuming

* fix test

* stricter test, speeds[-1] is 0.14 when starting here

* no walrus

* fixup mazda cc

* remove extra import

selfdrive/car/honda/carcontroller.py

@@ -195,7 +195,7 @@ class CarController:
       # If using stock ACC, spam cancel command to kill gas when OP disengages.
       if pcm_cancel_cmd:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
-      elif CS.out.cruiseState.standstill:
+      elif CC.cruiseControl.resume:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
 
     else:

selfdrive/car/hyundai/carcontroller.py

@@ -78,7 +78,7 @@ class CarController:
           self.last_button_frame = self.frame
 
         # cruise standstill resume
-        elif CC.enabled and CS.out.cruiseState.standstill:
+        elif CC.cruiseControl.resume:
           can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True))
           self.last_button_frame = self.frame
     else:
@@ -96,7 +96,7 @@ class CarController:
       if not self.CP.openpilotLongitudinalControl:
         if CC.cruiseControl.cancel:
           can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
-        elif CS.out.cruiseState.standstill:
+        elif CC.cruiseControl.resume:
           # send resume at a max freq of 10Hz
           if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
             # send 25 messages at a time to increases the likelihood of resume being accepted

selfdrive/car/mazda/carcontroller.py

@@ -29,13 +29,6 @@ class CarController:
                                                   CS.out.steeringTorque, CarControllerParams)
       self.steer_rate_limited = new_steer != apply_steer
 
-    if CC.enabled:
-      if CS.out.standstill and self.frame % 5 == 0:
-        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
-        # Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
-        # TODO: improve the resume trigger logic by looking at actual radar data
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
-
     if CC.cruiseControl.cancel:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
       # a race condition with the stock system, where the second cancel from openpilot
@@ -48,6 +41,10 @@ class CarController:
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
     else:
       self.brake_counter = 0
+      if CC.cruiseControl.resume and self.frame % 5 == 0:
+        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
+        # Send Resume button when planner wants car to move
+        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
 
     self.apply_steer_last = apply_steer
 

selfdrive/car/volkswagen/carcontroller.py

@@ -96,9 +96,8 @@ class CarController:
           # Cancel ACC if it's engaged with OP disengaged.
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["cancel"] = True
-        elif CC.enabled and CS.out.cruiseState.standstill:
-          # Blip the Resume button if we're engaged at standstill.
-          # FIXME: This is a naive implementation, improve with visiond or radar input.
+        elif CC.cruiseControl.resume:
+          # Send Resume button when planner wants car to move
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["resumeCruise"] = True
 

selfdrive/controls/controlsd.py

@@ -658,6 +658,10 @@ class Controls:
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
 
+    speeds = self.sm['longitudinalPlan'].speeds
+    if len(speeds):
+      CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
+
     hudControl = CC.hudControl
     hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
     hudControl.speedVisible = self.enabled

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -16,6 +16,7 @@ class Maneuver():
 
     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.duration = duration
     self.title = title
@@ -52,5 +53,9 @@ class Maneuver():
         print("Crashed!!!!")
         valid = False
 
+      if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
+        print('Planner not starting!')
+        valid = False
+
     print("maneuver end", valid)
     return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -28,6 +28,7 @@ class Plant():
     self.distance = 0.
     self.speed = speed
     self.acceleration = 0.0
+    self.speeds = []
 
     # lead car
     self.distance_lead = distance_lead
@@ -98,6 +99,7 @@ class Plant():
     self.planner.update(sm)
     self.speed = self.planner.v_desired_filter.x
     self.acceleration = self.planner.a_desired
+    self.speeds = self.planner.v_desired_trajectory.tolist()
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
@@ -129,6 +131,7 @@ class Plant():
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
+      "speeds": self.speeds,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
     }

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -3,6 +3,7 @@ import os
 import unittest
 
 from common.params import Params
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -106,6 +107,17 @@ maneuvers = [
     breakpoints=[1., 1.01, 11.],
     cruise_values=[float("nan"), 15., 15.],
   ),
+  # controls relies on planner commanding to move for stock-ACC resume spamming
+  Maneuver(
+    "resume from a stop",
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=STOP_DISTANCE,
+    speed_lead_values=[0., 0., 2.],
+    breakpoints=[1., 10., 15.],
+    ensure_start=True,
+  ),
 ]