From d6dc906ff8bae042af099676ec2fda8b5f5da810 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 23 Jun 2026 20:00:29 -0500 Subject: [PATCH] mica --- .../controls/lib/longitudinal_planner.py | 58 +++++++++++++++--- .../tests/test_longitudinal_planner.py | 59 +++++++++++++++++++ 2 files changed, 110 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 925046e99..5c22b3fad 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -378,6 +378,7 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35 NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22 NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32 NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18 +DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN = 0.35 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED = 12.0 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB = 0.95 DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL = 0.35 @@ -602,6 +603,7 @@ class LongitudinalPlanner: self.lead_depart_accel_hold_floor = None self.spacious_follow_cap_bypass_until = 0.0 self.post_departure_follow_settle_until = 0.0 + self.duplicate_vision_comfort_lead_source = None if self.is_preap: try: @@ -1763,6 +1765,44 @@ class LongitudinalPlanner: return self.lead_two return None + def get_duplicate_vision_comfort_lead(self, v_ego): + if not (self.lead_one.status and self.lead_two.status): + self.duplicate_vision_comfort_lead_source = None + return None + + if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)): + self.duplicate_vision_comfort_lead_source = None + return None + + if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego): + self.duplicate_vision_comfort_lead_source = None + return None + + lead_sources = { + "lead0": self.lead_one, + "lead1": self.lead_two, + } + latched_lead = lead_sources.get(self.duplicate_vision_comfort_lead_source) + if latched_lead is not None and latched_lead.status: + return latched_lead + + min_center_offset = min(abs(float(getattr(lead, "yRel", 0.0))) for lead in lead_sources.values()) + centered_sources = [ + (source, lead) for source, lead in lead_sources.items() + if abs(float(getattr(lead, "yRel", 0.0))) <= min_center_offset + DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN + ] + selected_source, selected_lead = min( + centered_sources, + key=lambda item: ( + float(item[1].dRel), + float(item[1].vLead), + -max(0.0, -float(getattr(item[1], "aLeadK", 0.0))), + abs(float(getattr(item[1], "yRel", 0.0))), + ), + ) + self.duplicate_vision_comfort_lead_source = selected_source + return selected_lead + def lead_is_spacious_brake_cap_window(self, lead, v_ego, base_t_follow): if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: return False @@ -2915,6 +2955,8 @@ class LongitudinalPlanner: effective_t_follow, allow_optional_far_lead_logic=True, ) + duplicate_vision_comfort_lead = self.get_duplicate_vision_comfort_lead(scene_v_ego) if allow_complex_follow_logic else None + comfort_follow_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None and follow_control_lead is not None else follow_control_lead if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass: if not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( @@ -2946,7 +2988,9 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, low_speed_transition_brake_cap) output_a_target = max(output_a_target, low_speed_transition_brake_cap) - comfort_lead = self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one + comfort_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None else ( + self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one + ) if allow_complex_follow_logic and comfort_lead is not None and not panic_bypass: far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow) if far_lead_brake_cap is not None: @@ -2964,9 +3008,9 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap) output_a_target = max(output_a_target, tracked_vision_model_brake_cap) - if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + if allow_complex_follow_logic and comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: matched_follow_transition_target = self.get_matched_follow_transition_target( - follow_control_lead, + comfort_follow_lead, scene_v_ego, effective_t_follow, prev_output_a_target, @@ -3014,9 +3058,9 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, duplicate_slow_lead_brake_hold_target) output_a_target = duplicate_slow_lead_brake_hold_target - if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + if allow_complex_follow_logic and comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap( - follow_control_lead, + comfort_follow_lead, scene_v_ego, effective_t_follow, self.mpc.source, @@ -3027,7 +3071,7 @@ class LongitudinalPlanner: output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap) cruise_tracking_lead_transition_target = self.get_cruise_tracking_lead_accel_transition_target( - follow_control_lead, + comfort_follow_lead, scene_v_ego, effective_t_follow, prev_output_a_target, @@ -3039,7 +3083,7 @@ class LongitudinalPlanner: output_a_target = min(output_a_target, cruise_tracking_lead_transition_target) mild_follow_zero_cross_guard_target = self.get_mild_follow_zero_cross_guard_target( - follow_control_lead, + comfort_follow_lead, scene_v_ego, effective_t_follow, prev_output_a_target, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9d25a752e..3f2fff3d3 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -3052,6 +3052,65 @@ def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): assert lead_1_bias == 0.0 +def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate(): + v_ego = 25.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) + lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + selected = planner.get_duplicate_vision_comfort_lead(v_ego) + + assert selected is lead_one + assert planner.duplicate_vision_comfort_lead_source == "lead0" + + +def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves(): + v_ego = 25.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) + lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + initial = planner.get_duplicate_vision_comfort_lead(v_ego) + + lead_one.yRel = 1.25 + lead_two.yRel = 0.05 + lead_one.dRel = 44.4 + lead_two.dRel = 44.5 + latched = planner.get_duplicate_vision_comfort_lead(v_ego) + + assert initial is lead_one + assert latched is lead_one + assert planner.duplicate_vision_comfort_lead_source == "lead0" + + +def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears(): + v_ego = 25.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) + lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one + + lead_two.dRel = 49.5 + assert planner.get_duplicate_vision_comfort_lead(v_ego) is None + assert planner.duplicate_vision_comfort_lead_source is None + + def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)