diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index da3e90464..73c58bdac 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,5 +1,7 @@ import json +import numpy as np + from cereal import log from openpilot.common.constants import CV from openpilot.common.params import Params @@ -10,6 +12,8 @@ LaneChangeDirection = log.LaneChangeDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. +NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0] +NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0] DESIRES = { LaneChangeDirection.none: { @@ -103,6 +107,15 @@ class DesireHelper: (lane_change_direction == LaneChangeDirection.right and carstate.steeringTorque < 0) ) + @staticmethod + def _nav_turn_is_imminent(carstate, maneuver_distance): + try: + distance = float(maneuver_distance) + except (TypeError, ValueError): + return False + + return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) + def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles): self._update_nav_params() if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)): @@ -112,6 +125,8 @@ class DesireHelper: if modifier == "": return log.Desire.none + maneuver_distance = self._nav_instruction_state.get("maneuverDistance", 0.0) + if modifier == "slightLeft": lane_change_direction = LaneChangeDirection.left desired_lane_width = starpilotPlan.laneWidthLeft @@ -127,10 +142,10 @@ class DesireHelper: if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: return log.Desire.keepRight elif modifier in ("left", "sharpLeft"): - if not carstate.rightBlinker and not carstate.leftBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill: + if not carstate.rightBlinker and not carstate.leftBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill and self._nav_turn_is_imminent(carstate, maneuver_distance): return log.Desire.turnLeft elif modifier in ("right", "sharpRight"): - if not carstate.leftBlinker and not carstate.rightBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill: + if not carstate.leftBlinker and not carstate.rightBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill and self._nav_turn_is_imminent(carstate, maneuver_distance): return log.Desire.turnRight return log.Desire.none diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 6f47f0afc..dcd75de7e 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -63,7 +63,7 @@ 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._nav_instruction_state = {"valid": True, "maneuverModifier": "right", "maneuverDistance": 10.0} helper.update( make_car_state(vEgo=5.0), @@ -76,6 +76,23 @@ def test_nav_desires_turn_right_below_lane_change_speed(): assert helper.desire == log.Desire.turnRight +def test_nav_desires_turn_right_waits_until_turn_is_close(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right", "maneuverDistance": 300.0} + + 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 + + def test_nav_desires_do_not_override_lane_change_state_machine(): helper = DesireHelper() helper.nav_desires_allowed = True