diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 1a9585098..8d1330b49 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -224,12 +224,12 @@ class DesireHelper: if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction): return log.Desire.keepRight elif modifier in ("left", "sharpLeft"): - turn_allowed = not carstate.rightBlinker and not carstate.leftBlindspot + turn_allowed = carstate.leftBlinker and not carstate.rightBlinker and not carstate.leftBlindspot turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance): return log.Desire.turnLeft elif modifier in ("right", "sharpRight"): - turn_allowed = not carstate.leftBlinker and not carstate.rightBlindspot + turn_allowed = carstate.rightBlinker and not carstate.leftBlinker and not carstate.rightBlindspot turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance): return log.Desire.turnRight diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index cc6a9f3fd..13128651e 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -71,7 +71,7 @@ def test_nav_desires_turn_right_below_lane_change_speed(): helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right", "maneuverDistance": 10.0} helper.update( - make_car_state(vEgo=5.0), + make_car_state(vEgo=5.0, rightBlinker=True), True, 0.0, make_plan(), @@ -81,6 +81,24 @@ def test_nav_desires_turn_right_below_lane_change_speed(): assert helper.desire == log.Desire.turnRight +def test_nav_desires_turn_requires_matching_blinker(): + for modifier, opposite_blinker in (("left", "rightBlinker"), ("right", "leftBlinker")): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": modifier, "maneuverDistance": 10.0} + + helper.update( + make_car_state(vEgo=5.0, **{opposite_blinker: True}), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=10.0, nav_lane_positioning_allowed=False), + ) + + assert helper.desire == log.Desire.none + + def test_nav_desires_turn_right_waits_until_turn_is_close(): helper = DesireHelper() helper.nav_desires_allowed = True