From 21e05837520bc7a8ef9a019cc7ac0e7fc01252b7 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 18 Jun 2026 11:14:06 -0700 Subject: [PATCH] Fix RELC road edge detection --- selfdrive/modeld/modeld.py | 2 +- sunnypilot/modeld_v2/modeld.py | 2 +- sunnypilot/selfdrive/controls/lib/relc.py | 34 +++++++++-- .../selfdrive/controls/lib/tests/test_relc.py | 60 +++++++++++++++---- 4 files changed, 77 insertions(+), 21 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c8b3b25499..8aa0753d34 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -327,7 +327,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego) + RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego, modelv2_send.modelV2.roadEdges) mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected) diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 0eb10c1b29..cac2b7f11d 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -435,7 +435,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego) + RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego, modelv2_send.modelV2.roadEdges) mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected) diff --git a/sunnypilot/selfdrive/controls/lib/relc.py b/sunnypilot/selfdrive/controls/lib/relc.py index 747b7909de..4ff2f6458c 100644 --- a/sunnypilot/selfdrive/controls/lib/relc.py +++ b/sunnypilot/selfdrive/controls/lib/relc.py @@ -10,11 +10,14 @@ from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params -NEARSIDE_PROB = 0.2 +NEARSIDE_PROB = 0.25 EDGE_PROB = 0.35 EDGE_REACTION_TIME = 1.0 EDGE_CLEAR_TIME = 0.3 MIN_SPEED = 20 * CV.MPH_TO_MS +NEAR_EDGE_DISTANCE = 4.5 +LEFT_NEARSIDE_LANE_IDX = 1 +RIGHT_NEARSIDE_LANE_IDX = 2 class RoadEdgeLaneChangeController: @@ -46,7 +49,21 @@ class RoadEdgeLaneChangeController: self.left_clear_timer = 0.0 self.right_clear_timer = 0.0 - def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None: + @staticmethod + def _road_edge_y(road_edges, idx: int) -> float | None: + if road_edges is None or len(road_edges) <= idx or len(road_edges[idx].y) == 0: + return None + return road_edges[idx].y[0] + + @staticmethod + def _edge_is_near(edge_y: float | None, left: bool) -> bool: + if edge_y is None: + return False + if left: + return bool(-NEAR_EDGE_DISTANCE < edge_y < 0.0) + return bool(0.0 < edge_y < NEAR_EDGE_DISTANCE) + + def update(self, road_edge_stds, lane_line_probs, v_ego: float, road_edges=None) -> None: self.update_params() if not self.enabled or v_ego < MIN_SPEED: @@ -55,11 +72,16 @@ class RoadEdgeLaneChangeController: left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0) right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0) - left_lane_prob = lane_line_probs[0] - right_lane_prob = lane_line_probs[3] + left_lane_prob = lane_line_probs[LEFT_NEARSIDE_LANE_IDX] + right_lane_prob = lane_line_probs[RIGHT_NEARSIDE_LANE_IDX] - left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob - right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob + left_edge_y = self._road_edge_y(road_edges, 0) + right_edge_y = self._road_edge_y(road_edges, 1) + left_edge_near = self._edge_is_near(left_edge_y, True) + right_edge_near = self._edge_is_near(right_edge_y, False) + + left_cond = left_edge_prob > EDGE_PROB and (left_edge_near or (left_edge_y is None and left_lane_prob < NEARSIDE_PROB)) + right_cond = right_edge_prob > EDGE_PROB and (right_edge_near or (right_edge_y is None and right_lane_prob < NEARSIDE_PROB)) if left_cond: self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_relc.py b/sunnypilot/selfdrive/controls/lib/tests/test_relc.py index b54435a6d5..3ff1be4a7c 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_relc.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_relc.py @@ -16,6 +16,11 @@ V_HIGH = MIN_SPEED + 2.0 V_LOW = MIN_SPEED - 1.0 +class DummyRoadEdge: + def __init__(self, y): + self.y = [y] + + @pytest.fixture def relc(mocker): mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params") @@ -25,14 +30,18 @@ def relc(mocker): return controller -def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH): +def make_road_edges(left_y=-3.0, right_y=3.0): + return [DummyRoadEdge(left_y), DummyRoadEdge(right_y)] + + +def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH, road_edges=None): for _ in range(int(seconds / DT_MDL) + 1): - controller.update(road_edge_stds, lane_line_probs, v_ego) + controller.update(road_edge_stds, lane_line_probs, v_ego, road_edges) @pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [ - ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"), - ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"), + ([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], "left_edge_detected"), + ([0.9, 0.0], [0.8, 0.8, 0.0, 0.8], "right_edge_detected"), ]) def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr): drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1) @@ -40,18 +49,18 @@ def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr): def test_edge_detection_requires_time(relc): - drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05) + drive(relc, [0.0, 0.9], [0.8, 0.0, 0.8, 0.8], EDGE_REACTION_TIME - 0.05) assert not relc.left_edge_detected def test_both_edges_detected(relc): - drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1) + drive(relc, [0.0, 0.0], [0.8, 0.0, 0.0, 0.8], EDGE_REACTION_TIME + 0.1) assert relc.left_edge_detected assert relc.right_edge_detected def test_noise_doesnt_clear(relc): - edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8]) + edge = ([0.0, 0.9], [0.8, 0.0, 0.8, 0.8]) clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8]) drive(relc, *edge, EDGE_REACTION_TIME + 0.1) @@ -63,7 +72,7 @@ def test_noise_doesnt_clear(relc): def test_clears_after_window(relc): - edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8]) + edge = ([0.0, 0.9], [0.8, 0.0, 0.8, 0.8]) clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8]) drive(relc, *edge, EDGE_REACTION_TIME + 0.1) @@ -75,25 +84,50 @@ def test_clears_after_window(relc): def test_low_speed_skips(relc): - drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW) + drive(relc, [0.0, 0.9], [0.8, 0.0, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW) assert not relc.left_edge_detected assert relc.left_edge_timer == 0.0 def test_speed_drop_resets(relc): - drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) + drive(relc, [0.0, 0.9], [0.8, 0.0, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) assert relc.left_edge_detected - relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW) + relc.update([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], V_LOW) assert not relc.left_edge_detected def test_param_off_resets(relc): - drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) + drive(relc, [0.0, 0.9], [0.8, 0.0, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) assert relc.left_edge_detected relc.params.get_bool.return_value = False relc.read_params() - relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH) + relc.update([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], V_HIGH) + assert not relc.left_edge_detected + assert not relc.right_edge_detected + + +@pytest.mark.parametrize("lane_line_probs", [ + [0.0, 0.8, 0.8, 0.8], + [0.8, 0.8, 0.8, 0.0], +]) +def test_outer_lane_lines_do_not_drive_edge_detection(relc, lane_line_probs): + drive(relc, [0.0, 0.0], lane_line_probs, EDGE_REACTION_TIME + 0.1) + assert not relc.left_edge_detected + assert not relc.right_edge_detected + + +@pytest.mark.parametrize("road_edge_stds,road_edges,attr", [ + ([0.0, 0.9], make_road_edges(left_y=-3.0, right_y=8.0), "left_edge_detected"), + ([0.9, 0.0], make_road_edges(left_y=-8.0, right_y=3.0), "right_edge_detected"), +]) +def test_near_road_edge_geometry_blocks_with_visible_lane_lines(relc, road_edge_stds, road_edges, attr): + drive(relc, road_edge_stds, [0.8, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=road_edges) + assert getattr(relc, attr) + + +def test_far_road_edge_geometry_does_not_block(relc): + drive(relc, [0.0, 0.0], [0.8, 0.0, 0.0, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=make_road_edges(left_y=-8.0, right_y=8.0)) assert not relc.left_edge_detected assert not relc.right_edge_detected