diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py new file mode 100644 index 00000000..3c5cc0e3 --- /dev/null +++ b/selfdrive/controls/lib/lead_behavior.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +from openpilot.common.constants import CV + + +HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS + + +def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float) -> float: + gap_error = lead_distance - desired_gap + actual_hw = lead_distance / max(v_ego, 1e-3) + desired_hw = desired_gap / max(v_ego, 1e-3) + + if v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED: + return 0.0 + if gap_error <= 0.0: + return 0.0 + + # Encourage ACC to treat a tracked lead as the active constraint when we're + # hanging far above the requested time gap, but don't override cruise for a + # truly distant lead or one we're already closing on decisively. + if actual_hw <= max(desired_hw + 0.35, 1.75): + return 0.0 + if actual_hw >= max(desired_hw + 1.6, 3.0): + return 0.0 + if closing_speed > max(2.5, 0.12 * v_ego): + return 0.0 + + return min(gap_error * 0.5, max(12.0, 0.6 * v_ego)) + + +def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float, + closing_speed: float, following_lead: bool) -> bool: + actual_hw = lead_distance / max(v_ego, 1e-3) + desired_hw = desired_gap / max(v_ego, 1e-3) + + if following_lead or v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED: + return False + + # Don't coast if we're already materially above the requested headway. + if actual_hw > max(desired_hw + 0.45, 1.85): + return False + + coast_window_open = lead_distance > desired_gap + max(4.0, 0.15 * v_ego) + coast_window_far = lead_distance < desired_gap + max(15.0, 0.75 * v_ego) + gentle_closing = closing_speed < max(1.5, 0.08 * v_ego) + ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6 + + return coast_window_open and coast_window_far and gentle_closing and ttc > 6.0 and lead_distance > desired_gap + 6.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4566e307..4d5ee00a 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -13,6 +13,7 @@ from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function @@ -549,6 +550,10 @@ class LongitudinalMpc: v_lower, v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) + if tracking_lead and lead_one.status: + desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) + closing_speed = max(0.0, v_ego - lead_one.vLead) + cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py new file mode 100644 index 00000000..4b5e7ba7 --- /dev/null +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -0,0 +1,34 @@ +from openpilot.selfdrive.controls.lib.lead_behavior import ( + get_tracked_lead_catchup_bias, + should_disable_far_lead_throttle, +) + + +def test_tracked_lead_catchup_bias_for_hanging_gap(): + bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1) + assert bias > 10.0 + + +def test_tracked_lead_catchup_bias_ignores_near_desired_gap(): + bias = get_tracked_lead_catchup_bias(31.4, 50.0, 38.0, 0.1) + assert bias == 0.0 + + +def test_tracked_lead_catchup_bias_ignores_very_far_gap(): + bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1) + assert bias == 0.0 + + +def test_disable_far_lead_throttle_rejects_two_second_plus_gap(): + should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False) + assert not should_disable + + +def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap(): + should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 0.5, False) + assert should_disable + + +def test_disable_far_lead_throttle_rejects_fast_closing(): + should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 3.5, False) + assert not should_disable diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index 6c520610..19ca940c 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -2,13 +2,13 @@ import numpy as np from openpilot.common.constants import CV +from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] PERSONALITY_BP = [45. * CV.MPH_TO_MS, 70. * CV.MPH_TO_MS] -HIGHWAY_DISABLE_THROTTLE_MIN_SPEED = 45. * CV.MPH_TO_MS def get_longitudinal_personality(sm): @@ -96,18 +96,7 @@ class StarPilotFollowing: v_lead = self.starpilot_planner.lead_one.vLead closing_speed = max(0.0, v_ego - v_lead) desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow)) - ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6 - - # Keep a mild coasting behavior only for far/low-risk slower leads. - coast_window_open = lead_distance > desired_gap + max(4.0, 0.2 * v_ego) - coast_window_far = lead_distance < desired_gap + max(25.0, 1.2 * v_ego) - gentle_closing = closing_speed < max(2.0, 0.12 * v_ego) - - # Highway-only coast suppression: keep low-speed lead departures responsive. - self.disable_throttle = (not self.following_lead and v_ego > HIGHWAY_DISABLE_THROTTLE_MIN_SPEED and coast_window_open and - coast_window_far and gentle_closing) - # Never coast when we are entering a potentially late-braking scenario. - self.disable_throttle &= ttc > 6.0 and lead_distance > desired_gap + 6.0 + self.disable_throttle = should_disable_far_lead_throttle(v_ego, lead_distance, desired_gap, closing_speed, self.following_lead) if long_control_active and self.starpilot_planner.tracking_lead: self.update_follow_values(self.starpilot_planner.lead_one.dRel, v_ego, self.starpilot_planner.lead_one.vLead, starpilot_toggles)