diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py index a47579f227..6b85adefd4 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -36,6 +36,7 @@ class LaneTurnController: self.DH = desire_helper self.turn_direction = TurnDirection.none self.armed_direction = TurnDirection.none + self.cue_sent = False self.params = Params() self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS self.param_read_counter = 0 @@ -54,6 +55,16 @@ class LaneTurnController: def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float, steering_pressed: bool = False, steering_torque: float = 0., turn_probs: tuple[float, float] = (0., 0.)) -> None: + if not left_blinker and not right_blinker: + self.armed_direction = TurnDirection.none + self.cue_sent = False + elif left_blinker != right_blinker: + direction = TurnDirection.turnLeft if left_blinker else TurnDirection.turnRight + if direction != self.armed_direction: + self.armed_direction = direction + self.turn_direction = TurnDirection.none + self.cue_sent = False + if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: requested = TurnDirection.turnLeft elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: @@ -62,17 +73,18 @@ class LaneTurnController: requested = TurnDirection.none if not self.wait_for_turn or requested == TurnDirection.none: - self.armed_direction = TurnDirection.none self.turn_direction = requested return - if requested != self.armed_direction: - # newly armed, or the blinker switched sides: wait again - self.armed_direction = requested + if not self.enabled and self.cue_sent: self.turn_direction = TurnDirection.none - if self.turn_direction == TurnDirection.none and self.turn_is_here(requested, v_ego, steering_pressed, steering_torque, turn_probs): - self.turn_direction = requested # latched until the blinker goes off + # Eligibility can clear the request, but only a canceled or switched signal permits another cue. + if (self.turn_direction == TurnDirection.none and not self.cue_sent and + self.turn_is_here(requested, v_ego, steering_pressed, steering_torque, turn_probs)): + self.turn_direction = requested + if self.enabled and self.turn_direction != TurnDirection.none: + self.cue_sent = True @staticmethod def turn_is_here(direction, v_ego: float, steering_pressed: bool, steering_torque: float, turn_probs: tuple[float, float]) -> bool: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py index 115277154c..c38e9b4fff 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -97,6 +97,39 @@ def set_lane_turn_params(): class TestDesireHelperIntegration(OpenpilotTestCase): + @parameterized.expand([(True,), (False,)], names=["left"]) + def test_turn_cue_is_not_repeated_after_lateral_reengagement(self, left, set_lane_turn_params): + dh = DesireHelper() + dh.lane_turn_controller.wait_for_turn = True + carstate = DummyCarState(vEgo=6, leftBlinker=left, rightBlinker=not left) + direction = log.Desire.turnLeft if left else log.Desire.turnRight + prediction = [0.] * (DESIRE_PRED_WIDTH * 4) + prediction[1 if left else 2] = MODEL_TURN_PROB + requests = [log.Desire.none] + + def update(active): + dh.update(carstate, active, 1., desire_prediction=prediction) + requests.append(dh.desire) + + update(True) + carstate.vEgo = LANE_CHANGE_SPEED_MIN + .1 + update(False) + carstate.vEgo = 6 + carstate.steeringPressed = True + carstate.steeringTorque = 2.08 if left else -2.08 + update(False) + carstate.steeringPressed = False + update(True) + assert requests == [log.Desire.none, direction, log.Desire.none, log.Desire.none, log.Desire.none] + + carstate.leftBlinker = carstate.rightBlinker = False + update(True) + carstate.leftBlinker, carstate.rightBlinker = left, not left + update(True) + pulses = sum(current == direction and previous != direction for previous, current in zip(requests, requests[1:], strict=False)) + assert pulses == 2 + assert requests[-1] == direction + @parameterized.expand([ # Lane turn desire overrides lane change desire (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, @@ -232,6 +265,111 @@ class TestWaitForTurn(OpenpilotTestCase): controller.update_lane_turn(False, False, True, False, 6) assert controller.get_turn_direction() == TurnDirection.none + @parameterized.expand([(True, True), (True, False), (False, True), (False, False)]) + def test_speed_crossing_does_not_send_another_cue(self, left, model_trigger): + controller = make_wait_controller() + direction = TurnDirection.turnLeft if left else TurnDirection.turnRight + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == direction + retry = {"turn_probs": probs} if model_trigger else {"steering_pressed": True, "steering_torque": 2.08 if left else -2.08} + for speed in (LANE_CHANGE_SPEED_MIN + .1, LANE_CHANGE_SPEED_MIN, 6, 6): + controller.update_lane_turn(False, False, left, not left, speed, **retry) + assert controller.get_turn_direction() == TurnDirection.none + + @parameterized.expand([(True, True), (True, False), (False, True), (False, False)]) + def test_blindspot_interruption_does_not_send_another_cue(self, left, model_trigger): + controller = make_wait_controller() + direction = TurnDirection.turnLeft if left else TurnDirection.turnRight + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == direction + controller.update_lane_turn(left, not left, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == TurnDirection.none + retry = {"turn_probs": probs} if model_trigger else {"steering_pressed": True, "steering_torque": 2.08 if left else -2.08} + controller.update_lane_turn(False, False, left, not left, 6, **retry) + assert controller.get_turn_direction() == TurnDirection.none + + @parameterized.expand([(True, "speed"), (False, "speed"), (True, "blindspot"), (False, "blindspot")]) + def test_blocking_before_first_cue_does_not_consume_signal(self, left, block): + controller = make_wait_controller() + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(block == "blindspot" and left, block == "blindspot" and not left, + left, not left, LANE_CHANGE_SPEED_MIN if block == "speed" else 6, turn_probs=probs) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, left, not left, 6) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == (TurnDirection.turnLeft if left else TurnDirection.turnRight) + + @parameterized.expand([(True,), (False,)]) + def test_canceling_consumed_signal_allows_next_turn(self, left): + controller = make_wait_controller() + direction = TurnDirection.turnLeft if left else TurnDirection.turnRight + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == direction + controller.update_lane_turn(False, False, left, not left, LANE_CHANGE_SPEED_MIN) + controller.update_lane_turn(False, False, False, False, 6) + controller.update_lane_turn(False, False, left, not left, 6) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == direction + + @parameterized.expand([(True,), (False,)]) + def test_switching_signal_side_allows_next_turn(self, left): + controller = make_wait_controller() + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == (TurnDirection.turnLeft if left else TurnDirection.turnRight) + controller.update_lane_turn(False, False, not left, left, 6) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, not left, left, 6, turn_probs=probs[::-1]) + assert controller.get_turn_direction() == (TurnDirection.turnRight if left else TurnDirection.turnLeft) + + @parameterized.expand([(True,), (False,)]) + def test_hazards_do_not_rearm_same_signal(self, left): + controller = make_wait_controller() + probs = (MODEL_TURN_PROB, 0.) if left else (0., MODEL_TURN_PROB) + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == (TurnDirection.turnLeft if left else TurnDirection.turnRight) + controller.update_lane_turn(False, False, True, True, 6, turn_probs=probs) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, left, not left, 6, turn_probs=probs) + assert controller.get_turn_direction() == TurnDirection.none + + def test_disabled_feature_does_not_consume_unemitted_cue(self): + controller = make_wait_controller() + controller.enabled = False + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + assert controller.get_turn_direction() == TurnDirection.none + controller.update_lane_turn(False, False, True, False, LANE_CHANGE_SPEED_MIN) + controller.enabled = True + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + assert controller.get_turn_direction() == TurnDirection.turnLeft + + def test_enabling_latched_turn_remembers_emitted_cue(self): + controller = make_wait_controller() + controller.enabled = False + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + controller.enabled = True + controller.update_lane_turn(False, False, True, False, 6) + assert controller.get_turn_direction() == TurnDirection.turnLeft + controller.update_lane_turn(False, False, True, False, LANE_CHANGE_SPEED_MIN) + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + assert controller.get_turn_direction() == TurnDirection.none + + def test_feature_toggle_does_not_repeat_emitted_cue(self): + controller = make_wait_controller() + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + assert controller.get_turn_direction() == TurnDirection.turnLeft + controller.enabled = False + controller.update_lane_turn(False, False, True, False, 6) + assert controller.get_turn_direction() == TurnDirection.none + controller.enabled = True + controller.update_lane_turn(False, False, True, False, 6, turn_probs=(MODEL_TURN_PROB, 0.)) + assert controller.get_turn_direction() == TurnDirection.none + def test_model_turn_probs(self): pred = [0.] * (DESIRE_PRED_WIDTH * 4) pred[DESIRE_PRED_WIDTH + 1] = 0.6 # turnLeft, second horizon