hurry up

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -57,6 +57,12 @@ COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 MIN_X_LEAD_FACTOR = 0.5
 
+# aggressive matches stock Hyundai SCC closest setting: 2.4 m stopped, 14.8 m @ 14, 20.2 m @ 17.8, 25.2 m @ 21.3 m/s
+# the MPC settles ~1.1 m short of its target at standstill, so 3.5 m stops at ~2.4 m
+AGGRESSIVE_STOP_DISTANCE = 3.5
+AGGRESSIVE_T_FOLLOW_BP = [0., 14., 16., 18., 20., 21.3, 30.]
+AGGRESSIVE_T_FOLLOW_V = [0.82, 0.82, 0.87, 0.94, 0.97, 1.02, 1.08]
+
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
     return 1.0
@@ -309,6 +315,11 @@ class LongitudinalMpc:
 
   def update(self, radarstate, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
+    stop_distance_offset = 0.
+    if personality == log.LongitudinalPersonality.aggressive:
+      t_follow = float(np.interp(self.x0[1], AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
+      # STOP_DISTANCE is compiled into the solver, so move the lead forward instead
+      stop_distance_offset = STOP_DISTANCE - AGGRESSIVE_STOP_DISTANCE
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -316,8 +327,8 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + stop_distance_offset
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset
 
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
     self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -2,6 +2,7 @@
 import numpy as np
 
 import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -20,6 +21,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import Lon
 J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
 J_CRUISE_BP = [0., 10.0, 25., 40.]
 A_CRUISE_MIN = -0.5
+AGGRESSIVE_LEAD_ONLY_DIST = 40.  # m
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
 
@@ -114,6 +116,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
                   (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
+    # aggressive: with a close radar lead, follow it like stock SCC instead of the model's larger stop gap
+    lead = sm['radarState'].leadOne
+    if sm['selfdriveState'].personality == log.LongitudinalPersonality.aggressive and lead.radar and lead.dRel < AGGRESSIVE_LEAD_ONLY_DIST:
+      candidates.pop()
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)

openpilot/selfdrive/controls/tests/test_following_distance.py

@@ -4,7 +4,9 @@ from openpilot.common.parameterized import parameterized_class
 
 from openpilot.cereal import log
 
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+import numpy as np
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW, \
+  STOP_DISTANCE, AGGRESSIVE_STOP_DISTANCE, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V
 from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -38,7 +40,11 @@ class TestFollowingDistance(OpenpilotTestCase):
   def test_following_distance(self):
     v_lead = float(self.speed)
     simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
-    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    if self.personality == log.LongitudinalPersonality.aggressive:
+      t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
+      correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - AGGRESSIVE_STOP_DISTANCE)
+    else:
+      correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
     abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
     self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
                            delta=0.2 * correct_steady_state + abs_err_margin)