mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
169 lines
8.0 KiB
Python
169 lines
8.0 KiB
Python
"""Route advice admitted through existing lateral and longitudinal owners."""
|
|
|
|
import math
|
|
|
|
from openpilot.cereal import log
|
|
from openpilot.common.constants import CV
|
|
|
|
from openpilot.starpilot.navigation.wire import navigation_state
|
|
|
|
SERVICE = 'starpilotNavigation'
|
|
MAX_AGE_NS = 2_500_000_000
|
|
TARGET_MPH = {'uturn': 5., 'sharpLeft': 10., 'sharpRight': 10., 'left': 14., 'right': 14.}
|
|
|
|
|
|
def current_instruction(sm, now_ns: int):
|
|
try:
|
|
nav, device = navigation_state(sm[SERVICE]), sm['deviceState']
|
|
stamp = sm.logMonoTime[SERVICE]
|
|
device_stamp = sm.logMonoTime['deviceState']
|
|
if (not sm.valid[SERVICE] or not sm.alive[SERVICE] or not nav.enabled or not nav.controlValid or
|
|
nav.status != 'guiding' or not nav.sessionId or not nav.revision or
|
|
not 0 < nav.frameMonoTime <= stamp <= now_ns <= nav.frameMonoTime + MAX_AGE_NS or
|
|
not sm.valid['deviceState'] or not sm.alive['deviceState'] or not device.started or
|
|
not 0 < device.startedMonoTime == nav.startedMonoTime <= device_stamp <= now_ns <= device_stamp + 2_000_000_000 or
|
|
not nav.startedMonoTime <= nav.locationMonoTime <= now_ns <= nav.locationMonoTime + MAX_AGE_NS):
|
|
return None
|
|
for service in ('carState', 'carControl'):
|
|
source = sm.logMonoTime[service]
|
|
if not sm.valid[service] or not sm.alive[service] or not nav.startedMonoTime <= source <= now_ns <= source + 100_000_000:
|
|
return None
|
|
cs = sm['carState']
|
|
if not cs.canValid or cs.canTimeout or str(cs.gearShifter) != 'drive':
|
|
return None
|
|
return nav
|
|
except (KeyError, AttributeError, TypeError):
|
|
return None
|
|
|
|
|
|
def turn_desire(sm, now_ns: int, current_desire, *, supported: bool):
|
|
if not supported or current_desire != log.Desire.none:
|
|
return current_desire
|
|
nav = current_instruction(sm, now_ns)
|
|
if nav is None:
|
|
return current_desire
|
|
cs, cc, instruction = sm['carState'], sm['carControl'], nav.instruction
|
|
if (not cc.latActive or cs.standstill or not math.isfinite(cs.vEgo) or not 0 <= cs.vEgo < 14 or
|
|
instruction.maneuverType != 'turn' or not math.isfinite(instruction.distanceMeters) or
|
|
not 0 <= instruction.distanceMeters <= min(90., max(35., cs.vEgo * 6.))):
|
|
return current_desire
|
|
if instruction.maneuverModifier in ('left', 'sharpLeft') and cs.leftBlinker and not cs.rightBlinker and not cs.leftBlindspot:
|
|
return log.Desire.turnLeft
|
|
if instruction.maneuverModifier in ('right', 'sharpRight') and cs.rightBlinker and not cs.leftBlinker and not cs.rightBlindspot:
|
|
return log.Desire.turnRight
|
|
return current_desire
|
|
|
|
|
|
def turn_speed(maneuver, min_steer_speed: float) -> float | None:
|
|
kind, modifier = maneuver.maneuverType, maneuver.maneuverModifier
|
|
target = TARGET_MPH.get(modifier) if kind == 'turn' else None
|
|
if modifier == 'uturn' or 'uturn' in kind or 'u-turn' in kind:
|
|
target = 5.
|
|
elif 'roundabout' in kind or 'rotary' in kind:
|
|
target = 12.
|
|
if target is None or not math.isfinite(maneuver.distanceMeters) or maneuver.distanceMeters < 0:
|
|
return None
|
|
target = max(target * CV.MPH_TO_MS, min_steer_speed)
|
|
return math.sqrt(target * target + 2 * .45 * max(0., maneuver.distanceMeters - 8.))
|
|
|
|
|
|
def cruise_ceiling(sm, CP, now_ns: int, cruise_mps: float) -> float | None:
|
|
nav = current_instruction(sm, now_ns)
|
|
if nav is None:
|
|
return None
|
|
cs, cc = sm['carState'], sm['carControl']
|
|
if (not CP.openpilotLongitudinalControl or CP.passive or CP.dashcamOnly or not cc.longActive or
|
|
cs.gasPressed or cs.brakePressed or not math.isfinite(cruise_mps) or cruise_mps <= 0):
|
|
return None
|
|
minimum = float(CP.minSteerSpeed)
|
|
if not math.isfinite(minimum):
|
|
return None
|
|
for instruction in (nav.instruction, nav.nextManeuver):
|
|
target = turn_speed(instruction, max(minimum, 0.))
|
|
if target is not None and target + .25 < cruise_mps:
|
|
return target
|
|
return None
|
|
|
|
|
|
def matching_turn_signal(sm, now_ns: int, *, supported: bool) -> bool:
|
|
nav = current_instruction(sm, now_ns) if supported else None
|
|
if nav is None:
|
|
return False
|
|
cs, instruction = sm['carState'], nav.instruction
|
|
matching = ((instruction.maneuverModifier in ('left', 'sharpLeft') and cs.leftBlinker and not cs.rightBlinker) or
|
|
(instruction.maneuverModifier in ('right', 'sharpRight') and cs.rightBlinker and not cs.leftBlinker))
|
|
return bool(instruction.maneuverType == 'turn' and matching and math.isfinite(cs.vEgo) and
|
|
math.isfinite(instruction.distanceMeters) and
|
|
0 <= instruction.distanceMeters <= min(250., max(30., 15. + max(0., cs.vEgo) * 12.)))
|
|
|
|
|
|
class TurnIntent:
|
|
def __init__(self, params=None, cp=None):
|
|
from openpilot.starpilot.lateral.auto_lane_change import steering_capable
|
|
from openpilot.starpilot.lateral.lane_change_preferences import LiveLaneChangePolicy
|
|
self.guidance = LiveLaneChangePolicy(params) if params is not None else None
|
|
self.steering_capable = steering_capable(cp)
|
|
self.signal_stop_hold = False
|
|
self.key = None
|
|
self.stop_hold = False
|
|
|
|
def select(self, sm, now_ns: int, current_desire, *, supported: bool, model_stop: bool):
|
|
nav = current_instruction(sm, now_ns) if supported else None
|
|
key = (nav.sessionId, nav.startedMonoTime) if nav is not None else None
|
|
if key != self.key:
|
|
self.key, self.stop_hold = key, False
|
|
if nav is None:
|
|
return self.signal_desire(sm, now_ns, current_desire, supported=supported, model_stop=model_stop)
|
|
cs = sm['carState']
|
|
try:
|
|
stamp, plan = sm.logMonoTime['longitudinalPlan'], sm['longitudinalPlan']
|
|
plan_valid = (sm.valid['longitudinalPlan'] and sm.alive['longitudinalPlan'] and
|
|
nav.startedMonoTime <= stamp <= now_ns <= stamp + 150_000_000 and
|
|
nav.startedMonoTime <= plan.modelMonoTime <= stamp)
|
|
except (KeyError, AttributeError, TypeError):
|
|
plan_valid = False
|
|
if cs.standstill or cs.leftBlinker == cs.rightBlinker:
|
|
self.stop_hold = False
|
|
elif model_stop or (plan_valid and plan.shouldStop):
|
|
self.stop_hold = True
|
|
if self.stop_hold or not plan_valid:
|
|
return current_desire
|
|
selected = turn_desire(sm, now_ns, current_desire, supported=supported)
|
|
if selected != log.Desire.none:
|
|
return selected
|
|
return self.signal_desire(sm, now_ns, current_desire, supported=supported, model_stop=model_stop)
|
|
|
|
def signal_desire(self, sm, now_ns, current_desire, *, supported, model_stop):
|
|
if self.guidance is None:
|
|
return current_desire
|
|
try:
|
|
cs, cc, device, plan = (sm[name] for name in ('carState', 'carControl', 'deviceState', 'longitudinalPlan'))
|
|
signaling = bool(cs.leftBlinker or cs.rightBlinker)
|
|
policy = self.guidance.sample(now_ns, signaling=signaling,
|
|
maneuver_active=current_desire != log.Desire.none)
|
|
if not signaling or cs.standstill:
|
|
self.signal_stop_hold = False
|
|
if model_stop or plan.shouldStop:
|
|
self.signal_stop_hold = True
|
|
if (not supported or not self.steering_capable or not policy.turn_signal_guidance or
|
|
current_desire != log.Desire.none or self.signal_stop_hold or not cc.latActive or
|
|
not cs.canValid or cs.canTimeout or str(cs.gearShifter) != 'drive' or cs.standstill or
|
|
not math.isfinite(cs.vEgo) or not 0.1 < cs.vEgo < 20 * CV.MPH_TO_MS or
|
|
cs.leftBlinker == cs.rightBlinker or not device.started):
|
|
return current_desire
|
|
for service, age in (('carState', 100_000_000), ('carControl', 100_000_000),
|
|
('longitudinalPlan', 150_000_000), ('deviceState', 2_000_000_000)):
|
|
stamp = sm.logMonoTime[service]
|
|
if (not sm.valid[service] or not sm.alive[service] or
|
|
not 0 < device.startedMonoTime <= stamp <= now_ns <= stamp + age):
|
|
return current_desire
|
|
if not device.startedMonoTime <= plan.modelMonoTime <= sm.logMonoTime['longitudinalPlan']:
|
|
return current_desire
|
|
if cs.leftBlinker and not cs.leftBlindspot:
|
|
return log.Desire.turnLeft
|
|
if cs.rightBlinker and not cs.rightBlindspot:
|
|
return log.Desire.turnRight
|
|
except (KeyError, AttributeError, TypeError, ValueError):
|
|
pass
|
|
return current_desire
|