Compare commits
45 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
e4e0b47f9c | ||
|
|
0bd7eeb471 | ||
|
|
644273cde9 | ||
|
|
b23341dc99 | ||
|
|
27cbb21a00 | ||
|
|
03abd296b0 | ||
|
|
4708cab071 | ||
|
|
df0836e374 | ||
|
|
1a0ceb7980 | ||
|
|
0245d75637 | ||
|
|
43b117a54e | ||
|
|
f7daa3e0b0 | ||
|
|
424e038e60 | ||
|
|
6d39d9abce | ||
|
|
10d937cc29 | ||
|
|
b9d8548bd9 | ||
|
|
b04c2216f5 | ||
|
|
c058b42fb2 | ||
|
|
06e95b59a5 | ||
|
|
beee28d7d6 | ||
|
|
323b7d2641 | ||
|
|
7f37226d78 | ||
|
|
a637ec6b09 | ||
|
|
0fd09792ce | ||
|
|
ecd68206c2 | ||
|
|
3d6b659464 | ||
|
|
086320f195 | ||
|
|
f52a64fb9e | ||
|
|
0706e02ce9 | ||
|
|
cfa0e4a7c9 | ||
|
|
0b682d0179 | ||
|
|
2923021a8e | ||
|
|
cfdbc18dee | ||
|
|
9ca4eadc79 | ||
|
|
b65f6a124e | ||
|
|
1f310ebc5c | ||
|
|
1d3a568020 | ||
|
|
bdd9854bf4 | ||
|
|
b28da6a643 | ||
|
|
4a49579adf | ||
|
|
e79f018f7c | ||
|
|
dbb3749dc2 | ||
|
|
a9bbd247d4 | ||
|
|
17d64d3f3b | ||
|
|
d3fae889c4 |
@@ -341,6 +341,7 @@ struct OnroadEventSP @0xda96579883444c35 {
|
||||
speedLimitChanged @21;
|
||||
speedLimitPending @22;
|
||||
e2eChime @23;
|
||||
laneChangeRoadEdge @24;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -447,6 +448,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
|
||||
|
||||
struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||
laneTurnDirection @0 :TurnDirection;
|
||||
leftLaneChangeEdgeBlock @1 :Bool;
|
||||
rightLaneChangeEdgeBlock @2 :Bool;
|
||||
|
||||
enum TurnDirection {
|
||||
none @0;
|
||||
|
||||
@@ -190,6 +190,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -56,7 +56,7 @@ class DesireHelper:
|
||||
def get_lane_change_direction(CS):
|
||||
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob):
|
||||
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected, right_edge_detected):
|
||||
self.alc.update_params()
|
||||
self.lane_turn_controller.update_params()
|
||||
v_ego = carstate.vEgo
|
||||
@@ -88,8 +88,8 @@ class DesireHelper:
|
||||
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
||||
|
||||
|
||||
@@ -33,7 +33,7 @@ from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -298,6 +298,7 @@ def main(demo=False):
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -395,7 +396,10 @@ 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
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
|
||||
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)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
|
||||
@@ -302,9 +302,15 @@ class SelfdriveD(CruiseHelper):
|
||||
# Handle lane change
|
||||
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
|
||||
direction = self.sm['modelV2'].meta.laneChangeDirection
|
||||
mdv2sp = self.sm['modelDataV2SP']
|
||||
|
||||
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
|
||||
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
||||
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
||||
self.events.add(EventName.laneChangeBlocked)
|
||||
|
||||
elif mdv2sp.leftLaneChangeEdgeBlock or mdv2sp.rightLaneChangeEdgeBlock:
|
||||
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
|
||||
|
||||
else:
|
||||
if direction == LaneChangeDirection.left:
|
||||
self.events.add(EventName.preLaneChangeLeft)
|
||||
|
||||
@@ -45,6 +45,9 @@ DESCRIPTIONS = {
|
||||
"DisableScreenTimer": tr_noop(
|
||||
"The onroad screen is turned of after 10 seconds. It will be temporarily enabled on alerts"
|
||||
),
|
||||
"RoadEdgeLaneChangeEnabled": tr_noop(
|
||||
"Enable this toggle to block lane change when road edge is detected on the stalk actuated side."
|
||||
),
|
||||
"DisableCarSteerAlerts": tr_noop(
|
||||
"Disables audible steering alerts from car"
|
||||
),
|
||||
@@ -130,6 +133,12 @@ class ICTogglesLayout(Widget):
|
||||
"eye_closed.png",
|
||||
False,
|
||||
),
|
||||
"RoadEdgeLaneChangeEnabled": (
|
||||
lambda: tr("Block Lane Change: Road Edge Detection"),
|
||||
DESCRIPTIONS["RoadEdgeLaneChangeEnabled"],
|
||||
"chffr_wheel.png",
|
||||
False,
|
||||
),
|
||||
}
|
||||
|
||||
self._toggles = {}
|
||||
|
||||
@@ -28,6 +28,7 @@ class ICTogglesLayoutMici(NavWidget):
|
||||
enable_onroad_screen_timer = BigParamControl("Onroad Screen Timeout", "DisableScreenTimer")
|
||||
force_enable_torque_bar = BigParamControl("Force Enable Torque Bar", "ForceShowTorqueBar")
|
||||
enable_accel_bar = BigParamControl("Enable Accel Bar", "ShowAccelBar")
|
||||
road_edge_lane_change_block = BigParamControl("Block Lane Change: Road Edge Detection", "RoadEdgeLaneChangeEnabled")
|
||||
|
||||
|
||||
self._scroller = Scroller([
|
||||
@@ -44,6 +45,7 @@ class ICTogglesLayoutMici(NavWidget):
|
||||
enable_onroad_screen_timer,
|
||||
force_enable_torque_bar,
|
||||
enable_accel_bar,
|
||||
road_edge_lane_change_block,
|
||||
], snap_items=False)
|
||||
|
||||
# Toggle lists
|
||||
@@ -61,6 +63,7 @@ class ICTogglesLayoutMici(NavWidget):
|
||||
("DisableScreenTimer", enable_onroad_screen_timer),
|
||||
("ForceShowTorqueBar", force_enable_torque_bar),
|
||||
("ShowAccelBar", enable_accel_bar),
|
||||
("RoadEdgeLaneChangeEnabled", road_edge_lane_change_block),
|
||||
)
|
||||
|
||||
if ui_state.params.get_bool("ShowDebugInfo"):
|
||||
|
||||
@@ -29,6 +29,7 @@ from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
|
||||
@@ -211,6 +212,7 @@ def main(demo=False):
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -318,7 +320,10 @@ 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
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
|
||||
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)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
|
||||
@@ -34,6 +34,7 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
|
||||
|
||||
@@ -252,6 +253,9 @@ def main(demo=False):
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
|
||||
|
||||
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -355,7 +359,10 @@ 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
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
|
||||
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)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
|
||||
113
sunnypilot/selfdrive/controls/lib/relc.py
Normal file
113
sunnypilot/selfdrive/controls/lib/relc.py
Normal file
@@ -0,0 +1,113 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
|
||||
NEARSIDE_PROB = 0.2
|
||||
EDGE_PROB = 0.35
|
||||
EDGE_REACTION_TIME = 1.0
|
||||
|
||||
class RoadEdgeLaneChangeController:
|
||||
def __init__(self, desire_helper):
|
||||
self.desire_helper = desire_helper
|
||||
self.params = Params()
|
||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
||||
self.left_edge_detected = False
|
||||
self.right_edge_detected = False
|
||||
self.left_edge_timer = 0.0
|
||||
self.right_edge_timer = 0.0
|
||||
self._frame = 0
|
||||
|
||||
def set_enabled(self, enabled):
|
||||
self.enabled = enabled
|
||||
if not enabled:
|
||||
self._reset_state()
|
||||
|
||||
def _read_params(self) -> None:
|
||||
if self._frame % int(1. / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
||||
|
||||
def _reset_state(self):
|
||||
self.left_edge_detected = False
|
||||
self.right_edge_detected = False
|
||||
self.left_edge_timer = 0.0
|
||||
self.right_edge_timer = 0.0
|
||||
|
||||
def _update_edge_detection(self, road_edge_stds, lane_line_probs):
|
||||
if not self.enabled:
|
||||
return
|
||||
|
||||
left_road_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
|
||||
right_road_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
|
||||
|
||||
# Lane line probabilities: [left_outer, left_inner, right_inner, right_outer]
|
||||
left_lane_nearside_prob = lane_line_probs[0] if len(lane_line_probs) > 0 else 0.0
|
||||
right_lane_nearside_prob = lane_line_probs[3] if len(lane_line_probs) > 3 else 0.0
|
||||
|
||||
left_edge_conditions = (
|
||||
left_road_edge_prob > EDGE_PROB and
|
||||
left_lane_nearside_prob < NEARSIDE_PROB and
|
||||
(len(lane_line_probs) <= 3 or right_lane_nearside_prob >= left_lane_nearside_prob)
|
||||
)
|
||||
right_edge_conditions = (
|
||||
right_road_edge_prob > EDGE_PROB and
|
||||
right_lane_nearside_prob < NEARSIDE_PROB and
|
||||
(len(lane_line_probs) <= 0 or left_lane_nearside_prob >= right_lane_nearside_prob)
|
||||
)
|
||||
|
||||
if left_edge_conditions:
|
||||
self.left_edge_timer += DT_MDL
|
||||
self.left_edge_detected = self.left_edge_timer > EDGE_REACTION_TIME
|
||||
else:
|
||||
self.left_edge_timer = 0.0
|
||||
self.left_edge_detected = False
|
||||
|
||||
if right_edge_conditions:
|
||||
self.right_edge_timer += DT_MDL
|
||||
self.right_edge_detected = self.right_edge_timer > EDGE_REACTION_TIME
|
||||
else:
|
||||
self.right_edge_timer = 0.0
|
||||
self.right_edge_detected = False
|
||||
|
||||
def update(self, road_edge_stds, lane_line_probs):
|
||||
self._read_params()
|
||||
|
||||
if not self.enabled:
|
||||
self._frame += 1
|
||||
return
|
||||
|
||||
self._update_edge_detection(road_edge_stds, lane_line_probs)
|
||||
self._frame += 1
|
||||
|
||||
def should_trigger_lane_change(self, carstate, lateral_active):
|
||||
if not self.enabled:
|
||||
return False, log.LaneChangeDirection.none
|
||||
return False, log.LaneChangeDirection.none
|
||||
|
||||
def is_lane_change_blocked(self, direction):
|
||||
if not self.enabled:
|
||||
return False
|
||||
|
||||
if direction == log.LaneChangeDirection.left:
|
||||
return self.left_edge_detected
|
||||
elif direction == log.LaneChangeDirection.right:
|
||||
return self.right_edge_detected
|
||||
|
||||
return False
|
||||
|
||||
def can_change_lane_left(self):
|
||||
return not self.left_edge_detected if self.enabled else True
|
||||
|
||||
def can_change_lane_right(self):
|
||||
return not self.right_edge_detected if self.enabled else True
|
||||
|
||||
@property
|
||||
def edge_detected(self):
|
||||
return self.left_edge_detected or self.right_edge_detected
|
||||
@@ -5,6 +5,8 @@ from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
|
||||
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
||||
|
||||
@@ -107,7 +109,9 @@ def set_lane_turn_params():
|
||||
])
|
||||
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
|
||||
dh = DesireHelper()
|
||||
relc = RoadEdgeLaneChangeController(dh)
|
||||
relc.set_enabled(True)
|
||||
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
|
||||
for _ in range(10):
|
||||
dh.update(carstate, lateral_active, lane_change_prob)
|
||||
dh.update(carstate, lateral_active, lane_change_prob, left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
|
||||
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
|
||||
|
||||
190
sunnypilot/selfdrive/controls/lib/tests/test_relc.py
Normal file
190
sunnypilot/selfdrive/controls/lib/tests/test_relc.py
Normal file
@@ -0,0 +1,190 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
from cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController, EDGE_REACTION_TIME
|
||||
|
||||
@pytest.fixture
|
||||
def relc_controller(mocker):
|
||||
mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
|
||||
mock_params.return_value.get_bool.return_value = True
|
||||
|
||||
DH = DesireHelper()
|
||||
relc = RoadEdgeLaneChangeController(DH)
|
||||
relc.set_enabled(True)
|
||||
return relc
|
||||
|
||||
|
||||
def test_disable_resets_state(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.left_edge_detected = True
|
||||
relc.right_edge_detected = True
|
||||
relc.left_edge_timer = 5.0
|
||||
relc.right_edge_timer = 5.0
|
||||
|
||||
relc.set_enabled(False)
|
||||
|
||||
assert not relc.left_edge_detected
|
||||
assert not relc.right_edge_detected
|
||||
assert relc.left_edge_timer == 0.0
|
||||
assert relc.right_edge_timer == 0.0
|
||||
|
||||
|
||||
def test_lane_change_blocked_left(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.left_edge_detected = True
|
||||
assert relc.is_lane_change_blocked(log.LaneChangeDirection.left)
|
||||
|
||||
|
||||
def test_lane_change_blocked_right(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.right_edge_detected = True
|
||||
assert relc.is_lane_change_blocked(log.LaneChangeDirection.right)
|
||||
|
||||
|
||||
def test_lane_change_not_blocked_opposite_side(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.left_edge_detected = True
|
||||
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
|
||||
|
||||
relc.left_edge_detected = False
|
||||
relc.right_edge_detected = True
|
||||
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
|
||||
|
||||
|
||||
def test_lane_change_not_blocked_when_disabled(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.set_enabled(False)
|
||||
relc.left_edge_detected = True
|
||||
relc.right_edge_detected = True
|
||||
|
||||
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
|
||||
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
|
||||
|
||||
|
||||
def test_can_change_lane_left(relc_controller):
|
||||
relc = relc_controller
|
||||
assert relc.can_change_lane_left()
|
||||
|
||||
relc.left_edge_detected = True
|
||||
assert not relc.can_change_lane_left()
|
||||
|
||||
|
||||
def test_can_change_lane_right(relc_controller):
|
||||
relc = relc_controller
|
||||
assert relc.can_change_lane_right()
|
||||
|
||||
relc.right_edge_detected = True
|
||||
assert not relc.can_change_lane_right()
|
||||
|
||||
|
||||
def test_can_change_lane_when_disabled(relc_controller):
|
||||
relc = relc_controller
|
||||
relc.set_enabled(False)
|
||||
relc.left_edge_detected = True
|
||||
relc.right_edge_detected = True
|
||||
|
||||
assert relc.can_change_lane_left()
|
||||
assert relc.can_change_lane_right()
|
||||
|
||||
|
||||
def test_edge_detected_property(relc_controller):
|
||||
relc = relc_controller
|
||||
assert not relc.edge_detected
|
||||
|
||||
relc.left_edge_detected = True
|
||||
assert relc.edge_detected
|
||||
|
||||
relc.left_edge_detected = False
|
||||
relc.right_edge_detected = True
|
||||
assert relc.edge_detected
|
||||
|
||||
relc.left_edge_detected = True
|
||||
assert relc.edge_detected
|
||||
|
||||
|
||||
def test_should_trigger_lane_change(relc_controller):
|
||||
relc = relc_controller
|
||||
should_trigger, direction = relc.should_trigger_lane_change(None, True)
|
||||
assert not should_trigger
|
||||
assert direction == log.LaneChangeDirection.none
|
||||
|
||||
|
||||
def test_update_increments_frame(relc_controller):
|
||||
relc = relc_controller
|
||||
initial = relc._frame
|
||||
relc.update([0.5, 0.5], [0.5, 0.5, 0.5, 0.5])
|
||||
assert relc._frame == initial + 1
|
||||
|
||||
|
||||
def test_left_edge_detection(relc_controller):
|
||||
relc = relc_controller
|
||||
road_edge_stds = [0.0, 0.9]
|
||||
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
|
||||
|
||||
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
|
||||
for _ in range(num_updates):
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
|
||||
assert relc.left_edge_detected
|
||||
|
||||
|
||||
def test_right_edge_detection(relc_controller):
|
||||
relc = relc_controller
|
||||
road_edge_stds = [0.9, 0.0]
|
||||
lane_line_probs = [0.8, 0.8, 0.8, 0.0]
|
||||
|
||||
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
|
||||
for _ in range(num_updates):
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
|
||||
assert relc.right_edge_detected
|
||||
|
||||
|
||||
def test_edge_detection_requires_time(relc_controller):
|
||||
relc = relc_controller
|
||||
road_edge_stds = [0.0, 0.9]
|
||||
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
|
||||
|
||||
num_updates = int(EDGE_REACTION_TIME / DT_MDL) - 1
|
||||
for _ in range(num_updates):
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
|
||||
assert not relc.left_edge_detected
|
||||
|
||||
|
||||
def test_edge_detection_clears(relc_controller):
|
||||
relc = relc_controller
|
||||
road_edge_stds = [0.0, 0.9]
|
||||
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
|
||||
|
||||
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
|
||||
for _ in range(num_updates):
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
road_edge_stds = [0.9, 0.9]
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
|
||||
assert not relc.left_edge_detected
|
||||
assert relc.left_edge_timer == 0.0
|
||||
|
||||
|
||||
def test_both_edges_detected(relc_controller):
|
||||
relc = relc_controller
|
||||
road_edge_stds = [0.0, 0.0]
|
||||
lane_line_probs = [0.0, 0.8, 0.8, 0.0]
|
||||
|
||||
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
|
||||
for _ in range(num_updates):
|
||||
relc.update(road_edge_stds, lane_line_probs)
|
||||
|
||||
assert relc.left_edge_detected
|
||||
assert relc.right_edge_detected
|
||||
@@ -226,4 +226,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
|
||||
},
|
||||
|
||||
EventNameSP.laneChangeRoadEdge: {
|
||||
ET.WARNING: Alert(
|
||||
"Lane Change Unavailable: Road Edge",
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
|
||||
},
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user