IQ.Pilot Release Commit @ bec7652
This commit is contained in:
IQ.Lvbs history cleanup
294
iqpilot/selfdrive/controls/lib/desire_helper.py
Normal file
294
iqpilot/selfdrive/controls/lib/desire_helper.py
Normal file
@@ -0,0 +1,294 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from iqpilot.cereal import car, custom, log
|
||||
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
|
||||
IQLaneSwapController,
|
||||
AutoLaneChangeMode,
|
||||
NavExitLaneChangeController,
|
||||
)
|
||||
from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard
|
||||
from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
TurnDirection = custom.IQTurnSignalDirection
|
||||
LateralEdgeBlock = custom.IQLateralEdgeBlock
|
||||
NavManeuverPhase = custom.IQNavState.ManeuverPhase
|
||||
|
||||
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.0
|
||||
TURN_DESIRE_STOP_HOLD_TIME = 3.4
|
||||
TURN_DESIRE_STOP_GAP_TIME = 0.2
|
||||
TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
|
||||
TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS
|
||||
TURN_DESIRE_COMMIT_YAW_RATE = 0.08
|
||||
|
||||
_LANE_CHANGE_DESIRES = {
|
||||
(LaneChangeDirection.none, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft,
|
||||
(LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft,
|
||||
(LaneChangeDirection.right, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight,
|
||||
(LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight,
|
||||
}
|
||||
|
||||
_TURN_DESIRES = {
|
||||
TurnDirection.none: log.Desire.none,
|
||||
TurnDirection.turnLeft: log.Desire.turnLeft,
|
||||
TurnDirection.turnRight: log.Desire.turnRight,
|
||||
}
|
||||
|
||||
_STOP_CYCLING_TURN_DESIRES = {
|
||||
log.Desire.turnLeft,
|
||||
log.Desire.turnRight,
|
||||
}
|
||||
|
||||
|
||||
def turn_desire(turn_direction) -> log.Desire:
|
||||
return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)]
|
||||
|
||||
|
||||
def _direction_from_blinkers(carstate) -> int:
|
||||
if carstate.leftBlinker:
|
||||
return LaneChangeDirection.left
|
||||
if carstate.rightBlinker:
|
||||
return LaneChangeDirection.right
|
||||
return LaneChangeDirection.none
|
||||
|
||||
|
||||
def _steering_nudge_matches(carstate, direction: int) -> bool:
|
||||
if not carstate.steeringPressed:
|
||||
return False
|
||||
return (
|
||||
(direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
|
||||
(direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
|
||||
)
|
||||
|
||||
|
||||
def _blindspot_matches(carstate, direction: int) -> bool:
|
||||
return (
|
||||
(direction == LaneChangeDirection.left and carstate.leftBlindspot) or
|
||||
(direction == LaneChangeDirection.right and carstate.rightBlindspot)
|
||||
)
|
||||
|
||||
|
||||
def _read_enable_bsm() -> bool:
|
||||
try:
|
||||
with car.CarParams.from_bytes(Params().get("CarParams")) as cp:
|
||||
return bool(cp.enableBsm)
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
class DesireHelper:
|
||||
def __init__(self):
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
self.lane_change_ll_prob = 1.0
|
||||
self.prev_one_blinker = False
|
||||
self.prev_nav_exit_active = False
|
||||
self.desire = log.Desire.none
|
||||
|
||||
self.alc = IQLaneSwapController(self)
|
||||
self.lane_turn_controller = IQNavTurnController(self)
|
||||
self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
|
||||
self.lateral_edge_guard = LateralEdgeGuard()
|
||||
self.lateral_edge_block = LateralEdgeBlock.none
|
||||
self.lane_turn_direction = TurnDirection.none
|
||||
self.nav_turn_direction = TurnDirection.none
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = log.Desire.none
|
||||
self.turn_desire_committed = False
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(carstate):
|
||||
return _direction_from_blinkers(carstate)
|
||||
|
||||
@staticmethod
|
||||
def _nav_turn_desire(nav_state):
|
||||
if nav_state is None or not getattr(nav_state, "active", False):
|
||||
return TurnDirection.none
|
||||
if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive:
|
||||
return TurnDirection.none
|
||||
if not getattr(nav_state, "shouldSendTurnDesire", False):
|
||||
return TurnDirection.none
|
||||
return getattr(nav_state, "turnDesireDirection", TurnDirection.none)
|
||||
|
||||
def _clear_lane_change(self) -> None:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
|
||||
def _refresh_turn_overrides(self, carstate, nav_state) -> bool:
|
||||
speed_mps = carstate.vEgo
|
||||
self.lane_turn_controller.update_params()
|
||||
self.lane_turn_controller.update_lane_turn(
|
||||
blindspot_left=carstate.leftBlindspot,
|
||||
blindspot_right=carstate.rightBlindspot,
|
||||
left_blinker=carstate.leftBlinker,
|
||||
right_blinker=carstate.rightBlinker,
|
||||
v_ego=speed_mps,
|
||||
)
|
||||
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
|
||||
self.nav_turn_direction = self._nav_turn_desire(nav_state)
|
||||
|
||||
self.nav_exit.update_params()
|
||||
self.nav_exit.update(nav_state, carstate)
|
||||
return bool(self.nav_exit.active)
|
||||
|
||||
def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool:
|
||||
timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX
|
||||
feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active
|
||||
return (not lateral_active) or timed_out or feature_disabled
|
||||
|
||||
def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
|
||||
if below_speed:
|
||||
return
|
||||
if one_blinker and not self.prev_one_blinker:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
|
||||
self.lane_change_ll_prob = 1.0
|
||||
return
|
||||
if nav_exit_active and not self.prev_nav_exit_active:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = self.nav_exit.direction
|
||||
self.lane_change_ll_prob = 1.0
|
||||
|
||||
def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None:
|
||||
if one_blinker:
|
||||
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
|
||||
elif nav_exit_active:
|
||||
self.lane_change_direction = self.nav_exit.direction
|
||||
|
||||
def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
|
||||
self._refresh_requested_direction(one_blinker, nav_exit_active)
|
||||
blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
|
||||
self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction)
|
||||
lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none
|
||||
steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
|
||||
nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed)
|
||||
allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start
|
||||
|
||||
if (not (one_blinker or nav_exit_active)) or below_speed:
|
||||
self._clear_lane_change()
|
||||
elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
|
||||
def _step_lane_change_starting(self, lane_change_prob: float) -> None:
|
||||
self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0)
|
||||
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
|
||||
self.lane_change_state = LaneChangeState.laneChangeFinishing
|
||||
|
||||
def _step_lane_change_finishing(self, one_blinker: bool) -> None:
|
||||
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
|
||||
if self.lane_change_ll_prob <= 0.99:
|
||||
return
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off
|
||||
|
||||
def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None:
|
||||
if self.lane_change_state == LaneChangeState.off:
|
||||
self._begin_from_idle(one_blinker, nav_exit_active, below_speed)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
self._step_lane_change_starting(lane_change_prob)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.laneChangeFinishing:
|
||||
self._step_lane_change_finishing(one_blinker)
|
||||
|
||||
def _update_timer(self) -> None:
|
||||
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
|
||||
self.lane_change_timer = 0.0
|
||||
else:
|
||||
self.lane_change_timer += DT_MDL
|
||||
|
||||
def _clear_turn_desire_stop_cycle(self) -> None:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = log.Desire.none
|
||||
self.turn_desire_committed = False
|
||||
|
||||
def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire:
|
||||
if desired_output not in _STOP_CYCLING_TURN_DESIRES:
|
||||
self._clear_turn_desire_stop_cycle()
|
||||
return desired_output
|
||||
|
||||
if desired_output != self.turn_desire_cycle_input:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = desired_output
|
||||
self.turn_desire_committed = False
|
||||
|
||||
if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE:
|
||||
self.turn_desire_committed = True
|
||||
|
||||
if self.turn_desire_committed:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
return desired_output
|
||||
|
||||
if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
return desired_output
|
||||
|
||||
if not self.turn_desire_stop_active:
|
||||
self.turn_desire_stop_active = True
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
|
||||
cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME
|
||||
self.turn_desire_stop_timer += DT_MDL
|
||||
if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME:
|
||||
return log.Desire.none
|
||||
return desired_output
|
||||
|
||||
def _pick_desire_output(self) -> None:
|
||||
desired_output = log.Desire.none
|
||||
if self.nav_turn_direction != TurnDirection.none:
|
||||
desired_output = turn_desire(self.nav_turn_direction)
|
||||
elif self.lane_turn_direction != TurnDirection.none:
|
||||
desired_output = turn_desire(self.lane_turn_direction)
|
||||
else:
|
||||
desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)]
|
||||
|
||||
self.desire = self._cycle_turn_desire_when_stopped(desired_output)
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
|
||||
self._last_carstate = carstate
|
||||
self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL)
|
||||
self.lateral_edge_block = LateralEdgeBlock.none
|
||||
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
||||
below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
|
||||
nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
|
||||
|
||||
self.alc.update_params()
|
||||
if self._reset_required(lateral_active, nav_exit_active):
|
||||
self._clear_lane_change()
|
||||
else:
|
||||
self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob)
|
||||
|
||||
self._update_timer()
|
||||
self.prev_one_blinker = one_blinker and lateral_active
|
||||
self.prev_nav_exit_active = nav_exit_active
|
||||
self.alc.update_state()
|
||||
self._pick_desire_output()
|
||||
Reference in New Issue
Block a user