mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-18 12:33:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix RELC road edge detection

Browse Source
This commit is contained in:
rav4kumar
2026-06-18 11:14:06 -07:00
parent 88dad50ee1
commit 21e0583752
4 changed files with 77 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.py
+1 -1
View File
@@ -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)
sunnypilot/modeld_v2/modeld.py
+1 -1
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/relc.py
+28 -6
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/tests/test_relc.py
+47 -13
View File
@@ -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