mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-20 08:42:11 +08:00
firestar5683
114 lines
2.8 KiB
Python
114 lines
2.8 KiB
Python
from types import SimpleNamespace
|
|
|
|
from cereal import log
|
|
|
|
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeDirection, LaneChangeState
|
|
|
|
|
|
def make_car_state(**overrides):
|
|
defaults = {
|
|
"vEgo": 20.0,
|
|
"leftBlinker": False,
|
|
"rightBlinker": False,
|
|
"leftBlindspot": False,
|
|
"rightBlindspot": False,
|
|
"steeringPressed": False,
|
|
"steeringTorque": 0.0,
|
|
"standstill": False,
|
|
}
|
|
defaults.update(overrides)
|
|
return SimpleNamespace(**defaults)
|
|
|
|
|
|
def make_toggles(**overrides):
|
|
defaults = {
|
|
"lane_changes": True,
|
|
"lane_change_delay": 0.0,
|
|
"lane_detection_width": 3.0,
|
|
"minimum_lane_change_speed": 10.0,
|
|
"nudgeless": True,
|
|
"one_lane_change": False,
|
|
"use_turn_desires": False,
|
|
}
|
|
defaults.update(overrides)
|
|
return SimpleNamespace(**defaults)
|
|
|
|
|
|
def make_plan(**overrides):
|
|
defaults = {
|
|
"laneWidthLeft": 4.0,
|
|
"laneWidthRight": 4.0,
|
|
}
|
|
defaults.update(overrides)
|
|
return SimpleNamespace(**defaults)
|
|
|
|
def test_nav_desires_keep_left_when_route_requests_it():
|
|
helper = DesireHelper()
|
|
helper.nav_desires_allowed = True
|
|
helper._update_nav_params = lambda: None
|
|
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"}
|
|
|
|
helper.update(
|
|
make_car_state(vEgo=20.0),
|
|
True,
|
|
0.0,
|
|
make_plan(laneWidthLeft=4.2),
|
|
make_toggles(nudgeless=True),
|
|
)
|
|
|
|
assert helper.desire == log.Desire.keepLeft
|
|
|
|
|
|
def test_nav_desires_turn_right_below_lane_change_speed():
|
|
helper = DesireHelper()
|
|
helper.nav_desires_allowed = True
|
|
helper._update_nav_params = lambda: None
|
|
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right"}
|
|
|
|
helper.update(
|
|
make_car_state(vEgo=5.0),
|
|
True,
|
|
0.0,
|
|
make_plan(),
|
|
make_toggles(minimum_lane_change_speed=10.0),
|
|
)
|
|
|
|
assert helper.desire == log.Desire.turnRight
|
|
|
|
|
|
def test_nav_desires_do_not_override_lane_change_state_machine():
|
|
helper = DesireHelper()
|
|
helper.nav_desires_allowed = True
|
|
helper._update_nav_params = lambda: None
|
|
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
|
|
helper.lane_change_state = LaneChangeState.laneChangeStarting
|
|
helper.lane_change_direction = LaneChangeDirection.left
|
|
helper.lane_change_ll_prob = 0.5
|
|
|
|
helper.update(
|
|
make_car_state(vEgo=25.0, leftBlinker=True),
|
|
True,
|
|
0.5,
|
|
make_plan(),
|
|
make_toggles(),
|
|
)
|
|
|
|
assert helper.desire == log.Desire.laneChangeLeft
|
|
|
|
|
|
def test_nav_desires_disabled_leave_desire_unchanged():
|
|
helper = DesireHelper()
|
|
helper.nav_desires_allowed = False
|
|
helper._update_nav_params = lambda: None
|
|
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left"}
|
|
|
|
helper.update(
|
|
make_car_state(vEgo=5.0),
|
|
True,
|
|
0.0,
|
|
make_plan(),
|
|
make_toggles(minimum_lane_change_speed=10.0),
|
|
)
|
|
|
|
assert helper.desire == log.Desire.none
|