mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-06 16:55:51 +08:00
firestar5683
284 lines
12 KiB
Python
284 lines
12 KiB
Python
import json
|
|
|
|
import numpy as np
|
|
|
|
from cereal import log
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
LaneChangeState = log.LaneChangeState
|
|
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]
|
|
NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS = [0.0, 15.0, 30.0]
|
|
NAV_KEEP_DISTANCE_BREAKPOINTS = [25.0, 90.0, 160.0]
|
|
|
|
DESIRES = {
|
|
LaneChangeDirection.none: {
|
|
LaneChangeState.off: log.Desire.none,
|
|
LaneChangeState.preLaneChange: log.Desire.none,
|
|
LaneChangeState.laneChangeStarting: log.Desire.none,
|
|
LaneChangeState.laneChangeFinishing: log.Desire.none,
|
|
},
|
|
LaneChangeDirection.left: {
|
|
LaneChangeState.off: log.Desire.none,
|
|
LaneChangeState.preLaneChange: log.Desire.none,
|
|
LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft,
|
|
LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft,
|
|
},
|
|
LaneChangeDirection.right: {
|
|
LaneChangeState.off: log.Desire.none,
|
|
LaneChangeState.preLaneChange: log.Desire.none,
|
|
LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight,
|
|
LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight,
|
|
},
|
|
}
|
|
|
|
TurnDirection = log.Desire
|
|
|
|
TURN_DESIRES = {
|
|
TurnDirection.none: log.Desire.none,
|
|
TurnDirection.turnLeft: log.Desire.turnLeft,
|
|
TurnDirection.turnRight: log.Desire.turnRight,
|
|
}
|
|
|
|
|
|
class DesireHelper:
|
|
def __init__(self):
|
|
self.params = Params()
|
|
self.params_memory = Params(memory=True)
|
|
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.keep_pulse_timer = 0.0
|
|
self.prev_one_blinker = False
|
|
self.desire = log.Desire.none
|
|
|
|
self.lane_change_completed = False
|
|
|
|
self.lane_change_wait_timer = 0.0
|
|
self.nav_desires_allowed = False
|
|
self._nav_param_counter = -1
|
|
self._nav_instruction_state_raw: object = None
|
|
self._nav_instruction_state: dict[str, object] = {}
|
|
|
|
def _update_nav_params(self):
|
|
self._nav_param_counter += 1
|
|
if self._nav_param_counter % 60 == 0:
|
|
self.nav_desires_allowed = self.params.get_bool("NavDesiresAllowed")
|
|
|
|
raw = self.params_memory.get("NavInstructionState") or {}
|
|
if raw == self._nav_instruction_state_raw:
|
|
return
|
|
|
|
self._nav_instruction_state_raw = raw
|
|
if not raw:
|
|
self._nav_instruction_state = {}
|
|
return
|
|
|
|
if isinstance(raw, dict):
|
|
self._nav_instruction_state = raw
|
|
return
|
|
|
|
if isinstance(raw, str):
|
|
try:
|
|
parsed = json.loads(raw)
|
|
self._nav_instruction_state = parsed if isinstance(parsed, dict) else {}
|
|
return
|
|
except Exception:
|
|
pass
|
|
|
|
self._nav_instruction_state = {}
|
|
|
|
@staticmethod
|
|
def _nav_keep_direction_is_clear(carstate, lane_change_direction):
|
|
return not (
|
|
(lane_change_direction == LaneChangeDirection.left and carstate.leftBlindspot) or
|
|
(lane_change_direction == LaneChangeDirection.right and carstate.rightBlindspot)
|
|
)
|
|
|
|
@staticmethod
|
|
def _nav_torque_applied(carstate, lane_change_direction):
|
|
return carstate.steeringPressed and (
|
|
(lane_change_direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
|
|
(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))
|
|
|
|
@staticmethod
|
|
def _nav_keep_is_imminent(carstate, maneuver_distance):
|
|
try:
|
|
distance = float(maneuver_distance)
|
|
except (TypeError, ValueError):
|
|
return False
|
|
|
|
return distance <= float(np.interp(carstate.vEgo, NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS, NAV_KEEP_DISTANCE_BREAKPOINTS))
|
|
|
|
@staticmethod
|
|
def _nav_effective_modifier(nav_instruction_state, carstate, maneuver_distance):
|
|
modifier = str(nav_instruction_state.get("maneuverModifier", ""))
|
|
maneuver_type = str(nav_instruction_state.get("maneuverType", ""))
|
|
active_lane_direction = str(nav_instruction_state.get("activeLaneDirection", ""))
|
|
|
|
if modifier in ("left", "right") and maneuver_type in ("off ramp", "fork") and DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance):
|
|
if active_lane_direction in ("slightLeft", "left"):
|
|
return "slightLeft"
|
|
if active_lane_direction in ("slightRight", "right"):
|
|
return "slightRight"
|
|
|
|
return modifier
|
|
|
|
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)):
|
|
return log.Desire.none
|
|
|
|
maneuver_distance = self._nav_instruction_state.get("maneuverDistance", 0.0)
|
|
modifier = self._nav_effective_modifier(self._nav_instruction_state, carstate, maneuver_distance)
|
|
if modifier == "":
|
|
return log.Desire.none
|
|
|
|
if modifier == "slightLeft":
|
|
lane_change_direction = LaneChangeDirection.left
|
|
desired_lane_width = starpilotPlan.laneWidthLeft
|
|
nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
|
|
if not carstate.rightBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction):
|
|
if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed:
|
|
return log.Desire.keepLeft
|
|
elif modifier == "slightRight":
|
|
lane_change_direction = LaneChangeDirection.right
|
|
desired_lane_width = starpilotPlan.laneWidthRight
|
|
nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
|
|
if not carstate.leftBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction):
|
|
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 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 and self._nav_turn_is_imminent(carstate, maneuver_distance):
|
|
return log.Desire.turnRight
|
|
|
|
return log.Desire.none
|
|
|
|
@staticmethod
|
|
def get_lane_change_direction(CS):
|
|
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
|
|
|
def update(self, carstate, lateral_active, lane_change_prob, starpilotPlan, starpilot_toggles):
|
|
v_ego = carstate.vEgo
|
|
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
|
below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed
|
|
|
|
lane_change_time_max = getattr(starpilot_toggles, 'lane_change_time_max', LANE_CHANGE_TIME_MAX)
|
|
if not lateral_active or self.lane_change_timer > lane_change_time_max or not starpilot_toggles.lane_changes:
|
|
self.lane_change_state = LaneChangeState.off
|
|
self.lane_change_direction = LaneChangeDirection.none
|
|
else:
|
|
# LaneChangeState.off
|
|
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
|
|
self.lane_change_state = LaneChangeState.preLaneChange
|
|
self.lane_change_ll_prob = 1.0
|
|
# Initialize lane change direction to prevent UI alert flicker
|
|
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
|
|
|
# LaneChangeState.preLaneChange
|
|
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
|
# Update lane change direction
|
|
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
|
|
|
torque_applied = carstate.steeringPressed and \
|
|
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
|
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
|
|
|
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
|
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
|
|
|
if torque_applied:
|
|
self.lane_change_wait_timer = starpilot_toggles.lane_change_delay
|
|
else:
|
|
torque_applied |= starpilot_toggles.nudgeless
|
|
torque_applied &= self.lane_change_wait_timer >= starpilot_toggles.lane_change_delay
|
|
|
|
desired_lane_width = starpilotPlan.laneWidthLeft if self.lane_change_direction == LaneChangeDirection.left else starpilotPlan.laneWidthRight
|
|
torque_applied &= desired_lane_width >= starpilot_toggles.lane_detection_width
|
|
|
|
if not one_blinker or below_lane_change_speed or self.lane_change_completed:
|
|
self.lane_change_state = LaneChangeState.off
|
|
self.lane_change_direction = LaneChangeDirection.none
|
|
elif torque_applied and not blindspot_detected:
|
|
self.lane_change_state = LaneChangeState.laneChangeStarting
|
|
|
|
self.lane_change_completed = starpilot_toggles.one_lane_change
|
|
|
|
self.lane_change_wait_timer = 0.0
|
|
|
|
self.lane_change_wait_timer += DT_MDL
|
|
|
|
# LaneChangeState.laneChangeStarting
|
|
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
|
|
# fade out over .5s
|
|
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
|
|
|
|
# 98% certainty
|
|
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
|
|
self.lane_change_state = LaneChangeState.laneChangeFinishing
|
|
|
|
# LaneChangeState.laneChangeFinishing
|
|
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
|
|
# fade in laneline over 1s
|
|
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
|
|
|
|
if self.lane_change_ll_prob > 0.99:
|
|
self.lane_change_direction = LaneChangeDirection.none
|
|
if one_blinker:
|
|
self.lane_change_state = LaneChangeState.preLaneChange
|
|
else:
|
|
self.lane_change_state = LaneChangeState.off
|
|
|
|
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
|
|
self.lane_change_timer = 0.0
|
|
else:
|
|
self.lane_change_timer += DT_MDL
|
|
|
|
self.prev_one_blinker = one_blinker
|
|
|
|
if lateral_active and one_blinker and below_lane_change_speed and not carstate.standstill and starpilot_toggles.use_turn_desires:
|
|
self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
|
|
self.desire = TURN_DESIRES[self.turn_direction]
|
|
else:
|
|
self.turn_direction = TurnDirection.none
|
|
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
|
|
|
|
# Send keep pulse once per second during LaneChangeStart.preLaneChange
|
|
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
|
|
self.keep_pulse_timer = 0.0
|
|
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
|
self.keep_pulse_timer += DT_MDL
|
|
if self.keep_pulse_timer > 1.0:
|
|
self.keep_pulse_timer = 0.0
|
|
elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight):
|
|
self.desire = log.Desire.none
|
|
|
|
if not one_blinker:
|
|
self.lane_change_completed = False
|
|
|
|
self.lane_change_wait_timer = 0.0
|
|
|
|
nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles)
|
|
if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off:
|
|
self.desire = nav_desire
|