Release 260111
This commit is contained in:
Comma Device
528
selfdrive/carrot/carrot_functions.py
Normal file
528
selfdrive/carrot/carrot_functions.py
Normal file
@@ -0,0 +1,528 @@
|
||||
from enum import Enum
|
||||
|
||||
from cereal import log
|
||||
from openpilot.common.params import Params
|
||||
import numpy as np
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import MyMovingAverage
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
LaneChangeState = log.LaneChangeState
|
||||
|
||||
class XState(Enum):
|
||||
lead = 0
|
||||
cruise = 1
|
||||
e2eCruise = 2
|
||||
e2eStop = 3
|
||||
e2ePrepare = 4
|
||||
e2eStopped = 5
|
||||
|
||||
def __str__(self):
|
||||
return self.name
|
||||
|
||||
class DrivingMode(Enum):
|
||||
Eco = 1
|
||||
Safe = 2
|
||||
Normal = 3
|
||||
High = 4
|
||||
|
||||
def __str__(self):
|
||||
return self.name
|
||||
|
||||
class TrafficState(Enum):
|
||||
off = 0
|
||||
red = 1
|
||||
green = 2
|
||||
|
||||
def __str__(self):
|
||||
return self.name
|
||||
|
||||
A_CRUISE_MAX_BP_CARROT = [0., 10 * CV.KPH_TO_MS, 40 * CV.KPH_TO_MS, 60 * CV.KPH_TO_MS, 80 * CV.KPH_TO_MS, 110 * CV.KPH_TO_MS, 140 * CV.KPH_TO_MS]
|
||||
|
||||
class CarrotPlanner:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.params_count = 0
|
||||
self.frame = 0
|
||||
|
||||
#self.log = ""
|
||||
|
||||
#self.aChangeCost = 200
|
||||
#self.aChangeCostStart = 40
|
||||
#self.tFollowSpeedAdd = 0.0
|
||||
#self.tFollowSpeedAddM = 0.0
|
||||
#self.tFollowLeadCarSpeed = 0.0
|
||||
#self.tFollowLeadCarAccel = 0.0
|
||||
#self.lo_timer = 0
|
||||
#self.v_ego_prev = 0.0
|
||||
|
||||
self.trafficState = TrafficState.off
|
||||
self.xStopFilter = MyMovingAverage(3)
|
||||
self.xStopFilter2 = MyMovingAverage(15)
|
||||
self.vFilter = MyMovingAverage(10)
|
||||
#self.t_follow_prev = self.get_T_FOLLOW()
|
||||
self.stop_distance = 6.0
|
||||
self.fakeCruiseDistance = 0.0
|
||||
self.xState = XState.cruise
|
||||
self.xStop = 0.0
|
||||
self.actual_stop_distance = 0.0
|
||||
#self.debugLongText = ""
|
||||
self.stopping_count = 0
|
||||
self.traffic_starting_count = 0
|
||||
self.user_stop_distance = -1
|
||||
|
||||
#self.t_follow = 0
|
||||
|
||||
self.startSignCount = 0
|
||||
self.stopSignCount = 0
|
||||
|
||||
self.stop_distance = 6.0
|
||||
self.trafficStopDistanceAdjust = 2.5 #params.get_float("TrafficStopDistanceAdjust") / 100.
|
||||
self.comfortBrake = 2.4
|
||||
self.comfort_brake = self.comfortBrake
|
||||
|
||||
self.soft_hold_active = 0
|
||||
self.events = Events()
|
||||
self.myDrivingMode = DrivingMode(self.params.get_int("MyDrivingMode"))
|
||||
self.myDrivingMode_last = self.myDrivingMode
|
||||
self.myDrivingMode_disable_auto = False
|
||||
self.myEcoModeFactor = 0.9
|
||||
self.mySafeModeFactor = 0.8
|
||||
self.myHighModeFactor = 1.2
|
||||
self.drivingModeDetector = DrivingModeDetector()
|
||||
self.mySafeFactor = 1.0
|
||||
|
||||
self.tFollowGap1 = 1.1
|
||||
self.tFollowGap2 = 1.3
|
||||
self.tFollowGap3 = 1.45
|
||||
self.tFollowGap4 = 1.6
|
||||
|
||||
self.dynamicTFollow = 0.0
|
||||
self.dynamicTFollowLC = 0.0
|
||||
|
||||
self.cruiseMaxVals0 = 1.6
|
||||
self.cruiseMaxVals1 = 1.6
|
||||
self.cruiseMaxVals2 = 1.2
|
||||
self.cruiseMaxVals3 = 1.0
|
||||
self.cruiseMaxVals4 = 0.8
|
||||
self.cruiseMaxVals5 = 0.7
|
||||
self.cruiseMaxVals6 = 0.6
|
||||
|
||||
self.aChangeCostStarting = 10.0
|
||||
|
||||
self.trafficLightDetectMode = 2 # 0: None, 1:Stop, 2:Stop&Go
|
||||
self.trafficState_carrot = 0
|
||||
self.carrot_stay_stop = False
|
||||
|
||||
self.eco_over_speed = 2
|
||||
self.eco_target_speed = 0
|
||||
|
||||
self.autoNaviSpeedDecelRate = 1.5
|
||||
|
||||
self.desireState = 0.0
|
||||
self.desireStateCount = 0
|
||||
self.jerk_factor = 1.0
|
||||
self.jerk_factor_apply = 1.0
|
||||
|
||||
self.j_lead_factor = 0.0
|
||||
|
||||
self.activeCarrot = 0
|
||||
self.xDistToTurn = 0
|
||||
self.atcType = ""
|
||||
self.atc_active = False
|
||||
|
||||
|
||||
def _params_update(self):
|
||||
self.frame += 1
|
||||
self.params_count += 1
|
||||
if self.params_count % 10 == 0:
|
||||
myDrivingMode = DrivingMode(self.params.get_int("MyDrivingMode"))
|
||||
if myDrivingMode != self.myDrivingMode_last:
|
||||
self.myDrivingMode_disable_auto = True
|
||||
self.myDrivingMode_last = myDrivingMode
|
||||
|
||||
self.myDrivingModeAuto = self.params.get_int("MyDrivingModeAuto")
|
||||
if self.myDrivingModeAuto > 0 and not self.myDrivingMode_disable_auto:
|
||||
self.myDrivingMode = self.drivingModeDetector.get_mode()
|
||||
else:
|
||||
self.myDrivingMode = myDrivingMode
|
||||
|
||||
if self.params_count == 10:
|
||||
self.myHighModeFactor = 1.2 #float(self.params.get_int("MyHighModeFactor")) * 0.01
|
||||
self.trafficLightDetectMode = self.params.get_int("TrafficLightDetectMode") # 0: None, 1:Stop, 2:Stop&Go
|
||||
elif self.params_count == 20:
|
||||
self.tFollowGap1 = self.params.get_float("TFollowGap1") * 0.01
|
||||
self.tFollowGap2 = self.params.get_float("TFollowGap2") * 0.01
|
||||
self.tFollowGap3 = self.params.get_float("TFollowGap3") * 0.01
|
||||
self.tFollowGap4 = self.params.get_float("TFollowGap4") * 0.01
|
||||
self.dynamicTFollow = self.params.get_float("DynamicTFollow") * 0.01
|
||||
self.dynamicTFollowLC = self.params.get_float("DynamicTFollowLC") * 0.01
|
||||
elif self.params_count == 30:
|
||||
self.cruiseMaxVals0 = self.params.get_float("CruiseMaxVals0") * 0.01
|
||||
self.cruiseMaxVals1 = self.params.get_float("CruiseMaxVals1") * 0.01
|
||||
self.cruiseMaxVals2 = self.params.get_float("CruiseMaxVals2") * 0.01
|
||||
self.cruiseMaxVals3 = self.params.get_float("CruiseMaxVals3") * 0.01
|
||||
self.cruiseMaxVals4 = self.params.get_float("CruiseMaxVals4") * 0.01
|
||||
self.cruiseMaxVals5 = self.params.get_float("CruiseMaxVals5") * 0.01
|
||||
self.cruiseMaxVals6 = self.params.get_float("CruiseMaxVals6") * 0.01
|
||||
elif self.params_count == 40:
|
||||
self.stop_distance = self.params.get_float("StopDistanceCarrot") * 0.01
|
||||
self.j_lead_factor = self.params.get_float("JLeadFactor3") * 0.01
|
||||
self.eco_over_speed = self.params.get_int("CruiseEcoControl")
|
||||
self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
|
||||
self.aChangeCostStaring = self.params.get_float("AChangeCostStarting")
|
||||
self.trafficStopDistanceAdjust = self.params.get_float("TrafficStopDistanceAdjust") / 100.
|
||||
elif self.params_count >= 100:
|
||||
|
||||
self.params_count = 0
|
||||
|
||||
def get_carrot_accel(self, v_ego):
|
||||
cruiseMaxVals = [self.cruiseMaxVals0, self.cruiseMaxVals1, self.cruiseMaxVals2, self.cruiseMaxVals3, self.cruiseMaxVals4, self.cruiseMaxVals5, self.cruiseMaxVals6]
|
||||
factor = self.myHighModeFactor if self.myDrivingMode == DrivingMode.High else self.mySafeFactor
|
||||
return np.interp(v_ego, A_CRUISE_MAX_BP_CARROT, cruiseMaxVals) * factor
|
||||
|
||||
def get_T_FOLLOW(self, personality=log.LongitudinalPersonality.standard):
|
||||
if personality==log.LongitudinalPersonality.moreRelaxed:
|
||||
self.jerk_factor = 1.0
|
||||
return self.tFollowGap4
|
||||
elif personality==log.LongitudinalPersonality.relaxed:
|
||||
self.jerk_factor = 1.0
|
||||
return self.tFollowGap3
|
||||
elif personality==log.LongitudinalPersonality.standard:
|
||||
self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.7
|
||||
return self.tFollowGap2
|
||||
elif personality==log.LongitudinalPersonality.aggressive:
|
||||
self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.5
|
||||
return self.tFollowGap1
|
||||
else:
|
||||
raise NotImplementedError("Longitudinal personality not supported")
|
||||
|
||||
def _update_model_desire(self, sm):
|
||||
meta = sm['modelV2'].meta
|
||||
carState = sm['carState']
|
||||
if meta.laneChangeState == LaneChangeState.laneChangeStarting: # laneChangig
|
||||
self.desireState = meta.desireState[3] if carState.leftBlinker else meta.desireState[4]
|
||||
self.desireStateCount += 1
|
||||
else:
|
||||
self.desireState = 0.0
|
||||
self.desireStateCount = 0
|
||||
|
||||
def dynamic_t_follow(self, t_follow, lead, desired_follow_distance, prev_a):
|
||||
|
||||
self.jerk_factor_apply = self.jerk_factor
|
||||
if self.desireState > 0.9 and self.desireStateCount < int(1.5 / DT_MDL): # lane change state, 1.5초동안만.
|
||||
t_follow *= self.dynamicTFollowLC # 차선변경시 t_follow를 줄임.
|
||||
self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC # 차선변경시 jerk factor를 줄여 aggresive하게
|
||||
elif lead.status:
|
||||
t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.1, 0.0])
|
||||
if self.dynamicTFollow > 0.0:
|
||||
gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1
|
||||
t_follow += gap_dist_adjust
|
||||
if gap_dist_adjust < 0:
|
||||
self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게.
|
||||
#self.jerk_factor_apply = np.interp(abs(lead.jLead), [0, 2], [self.jerk_factor, self.jerk_factor * self.j_lead_factor])
|
||||
|
||||
return t_follow
|
||||
|
||||
def update_stop_dist(self, stop_x):
|
||||
stop_x = self.xStopFilter.process(stop_x, median = True)
|
||||
stop_x = self.xStopFilter2.process(stop_x)
|
||||
return stop_x
|
||||
|
||||
def check_model_stopping(self, v_cruise, v, v_ego, a_ego, model_x, y, d_rel):
|
||||
v_ego_kph = v_ego * CV.MS_TO_KPH
|
||||
model_v = self.vFilter.process(v[-1])
|
||||
startSign = model_v > 5.0 or model_v > (v[0] + 2)
|
||||
|
||||
if v_ego_kph < 1.0:
|
||||
stopSign = model_x < 20.0 and model_v < 10.0
|
||||
elif v_ego_kph < 82.0:
|
||||
stopSign = (model_x < d_rel - 3.0 and
|
||||
model_x < np.interp(v[0] * 3.6, [60, 80], [120.0, 150]) and
|
||||
((model_v < 3.0) or (model_v < v[0] * 0.7)) and
|
||||
abs(y[-1]) < 5.0)
|
||||
# 정상주행중 감속하는 경우(카메라 감속등), 오감지가 많음.
|
||||
# 회생감속시:v_cruise=0에는 신호호감지하도록함.
|
||||
if v_cruise != 0 and (self.xState == XState.e2eCruise and a_ego < -1.0):
|
||||
stopSign = False
|
||||
else:
|
||||
stopSign = False
|
||||
|
||||
# self.stopSignCount = (
|
||||
# self.stopSignCount + 1
|
||||
# if (
|
||||
# stopSign
|
||||
# and (
|
||||
# model_x > get_safe_obstacle_distance(
|
||||
# v_ego,
|
||||
# t_follow=0,
|
||||
# comfort_brake=COMFORT_BRAKE,
|
||||
# stop_distance=-1.0,
|
||||
# )
|
||||
# )
|
||||
# )
|
||||
# else 0
|
||||
# )
|
||||
self.stopSignCount = self.stopSignCount + 1 if stopSign else 0
|
||||
self.startSignCount = self.startSignCount + 1 if startSign and not stopSign else 0
|
||||
|
||||
if self.stopSignCount * DT_MDL > 0.0:
|
||||
self.trafficState = TrafficState.red
|
||||
elif self.startSignCount * DT_MDL > 0.2:
|
||||
self.trafficState = TrafficState.green
|
||||
else:
|
||||
self.trafficState = TrafficState.off
|
||||
|
||||
def _update_carrot_man(self, sm, v_ego_kph, v_cruise_kph):
|
||||
atc_active = False
|
||||
if sm.alive['carrotMan']:
|
||||
carrot_man = sm['carrotMan']
|
||||
atc_turn_left = carrot_man.atcType in ["turn left", "atc left"]
|
||||
trigger_start = self.carrot_staty_stop = False
|
||||
if atc_turn_left or sm['carState'].leftBlinker:
|
||||
if self.trafficState_carrot == 1 and carrot_man.trafficState == 3: # red -> left triggered
|
||||
trigger_start = True
|
||||
elif carrot_man.trafficState in [1, 2]:
|
||||
self.carrot_stay_stop = True
|
||||
elif self.trafficState_carrot == 1 and carrot_man.trafficState == 2: # red -> green triggered
|
||||
trigger_start = True
|
||||
else:
|
||||
trigger_start = False
|
||||
self.trafficState_carrot = carrot_man.trafficState
|
||||
|
||||
if trigger_start:
|
||||
if self.soft_hold_active > 0:
|
||||
self.events.add(EventName.trafficSignChanged)
|
||||
elif self.xState in [XState.e2eStop, XState.e2eStopped]:
|
||||
self.xState = XState.e2eCruise
|
||||
self.traffic_starting_count = 10.0 / DT_MDL
|
||||
|
||||
self.activeCarrot = carrot_man.activeCarrot
|
||||
self.xDistToTurn = carrot_man.xDistToTurn
|
||||
atc_active = self.activeCarrot > 1 and 0 < self.xDistToTurn < 100
|
||||
self.atcType = carrot_man.atcType
|
||||
|
||||
v_cruise_kph = min(v_cruise_kph, carrot_man.desiredSpeed)
|
||||
|
||||
return v_cruise_kph, atc_active
|
||||
|
||||
def cruise_eco_control(self, v_ego_kph, v_cruise_kph):
|
||||
v_cruise_kph_apply = v_cruise_kph
|
||||
if self.eco_over_speed > 0:
|
||||
if self.eco_target_speed > 0:
|
||||
if self.eco_target_speed < v_cruise_kph:
|
||||
self.eco_target_speed = v_cruise_kph
|
||||
elif self.eco_target_speed > v_cruise_kph:
|
||||
self.eco_target_speed = 0
|
||||
elif self.eco_target_speed == 0 and v_ego_kph + 3 < v_cruise_kph and v_cruise_kph > 20.0: # 주행중 속도가 떨어지면 다시 크루즈연비제어 시작.
|
||||
self.eco_target_speed = v_cruise_kph
|
||||
|
||||
if self.eco_target_speed != 0: ## 크루즈 연비 제어모드 작동중일때: 연비제어 종료지점
|
||||
if v_ego_kph > self.eco_target_speed: # 설정속도를 초과하면..
|
||||
self.eco_target_speed = 0
|
||||
else:
|
||||
v_cruise_kph_apply = self.eco_target_speed + self.eco_over_speed # + 설정 속도로 설정함.
|
||||
else:
|
||||
self.eco_target_speed = 0
|
||||
|
||||
return v_cruise_kph_apply
|
||||
|
||||
def update(self, sm, v_cruise_kph, mode):
|
||||
self._params_update()
|
||||
self._update_model_desire(sm)
|
||||
|
||||
self.events = Events()
|
||||
carstate = sm['carState']
|
||||
vCluRatio = carstate.vCluRatio
|
||||
#controlsState = sm['controlsState']
|
||||
radarstate = sm['radarState']
|
||||
model = sm['modelV2']
|
||||
|
||||
#self.soft_hold_active = sm['carControl'].hudControl.softHoldActive # carrot 1
|
||||
self.soft_hold_active = sm['carState'].softHoldActive # carrot 2
|
||||
|
||||
self.comfort_brake = self.comfortBrake
|
||||
|
||||
v_ego = carstate.vEgo
|
||||
a_ego = carstate.aEgo
|
||||
v_ego_kph = v_ego * CV.MS_TO_KPH
|
||||
v_ego_cluster = carstate.vEgoCluster
|
||||
v_ego_cluster_kph = v_ego_cluster * CV.MS_TO_KPH
|
||||
|
||||
leadOne = radarstate.leadOne
|
||||
self.mySafeFactor = 1.0
|
||||
if leadOne.status and leadOne.radar and leadOne.vLead < 5 and leadOne.aLead < 0.2 and v_ego > 1.0: # 앞차가 매우 느리거나 정지한경우
|
||||
self.myDrivingMode = DrivingMode.Safe
|
||||
if self.myDrivingMode == DrivingMode.Eco: # eco
|
||||
self.mySafeFactor = self.myEcoModeFactor
|
||||
elif self.myDrivingMode == DrivingMode.Safe: #safe
|
||||
self.mySafeFactor = self.mySafeModeFactor
|
||||
|
||||
if self.frame % 20 == 0: # every 1 sec
|
||||
vLead = 0
|
||||
aLead = 0
|
||||
dRel = 200
|
||||
if leadOne.status:
|
||||
vLead = leadOne.vLead * CV.MS_TO_KPH
|
||||
aLead = leadOne.aLead
|
||||
dRel = leadOne.dRel
|
||||
|
||||
self.drivingModeDetector.update_data(v_ego_kph, vLead, carstate.aEgo, aLead, dRel)
|
||||
|
||||
v_cruise_kph = self.cruise_eco_control(v_ego_cluster_kph, v_cruise_kph)
|
||||
v_cruise_kph, atc_active = self._update_carrot_man(sm, v_ego_kph, v_cruise_kph)
|
||||
|
||||
#if atc_active and not self.atc_active and self.xState not in [XState.e2eStop, XState.e2eStopped, XState.lead]:
|
||||
# if self.atcType in ["turn left", "turn right", "atc left", "atc right"]:
|
||||
# self.xState = XState.e2ePrepare
|
||||
self.atc_active = atc_active
|
||||
|
||||
v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
||||
if vCluRatio > 0.5:
|
||||
v_cruise *= vCluRatio
|
||||
|
||||
x = model.position.x
|
||||
y = model.position.y
|
||||
v = model.velocity.x
|
||||
|
||||
self.fakeCruiseDistance = 0.0
|
||||
lead_detected = radarstate.leadOne.status # & radarstate.leadOne.radar
|
||||
|
||||
self.xStop = self.update_stop_dist(x[31])
|
||||
stop_model_x = self.xStop
|
||||
|
||||
trafficState_last = self.trafficState
|
||||
#self.check_model_stopping(v, v_ego, self.xStop, y)
|
||||
self.check_model_stopping(v_cruise, v, v_ego, a_ego, x[-1], y, radarstate.leadOne.dRel if lead_detected else 1000)
|
||||
|
||||
if self.myDrivingMode == DrivingMode.High or self.trafficLightDetectMode == 0:
|
||||
self.trafficState = TrafficState.off
|
||||
if abs(carstate.steeringAngleDeg) > 20:
|
||||
self.trafficState = TrafficState.off
|
||||
|
||||
#self.update_user_control()
|
||||
|
||||
if carstate.gasPressed or carstate.brakePressed:
|
||||
self.user_stop_distance = -1
|
||||
|
||||
if self.soft_hold_active > 0:
|
||||
self.xState = XState.e2eStopped
|
||||
if trafficState_last in [TrafficState.off, TrafficState.red] and self.trafficState == TrafficState.green:
|
||||
self.events.add(EventName.trafficSignChanged)
|
||||
elif self.xState == XState.e2eStopped:
|
||||
if carstate.gasPressed:
|
||||
self.xState = XState.e2eCruise #XState.e2ePrepare
|
||||
elif lead_detected and (radarstate.leadOne.dRel - stop_model_x) < 2.0:
|
||||
self.xState = XState.lead
|
||||
elif self.stopping_count == 0:
|
||||
if self.trafficState == TrafficState.green and not self.carrot_stay_stop and not carstate.leftBlinker and self.trafficLightDetectMode != 1:
|
||||
#self.xState = XState.e2ePrepare
|
||||
self.xState = XState.e2eCruise # 실험모드를 거치지 않고 바로 출발.
|
||||
self.events.add(EventName.trafficSignGreen)
|
||||
self.stopping_count = max(0, self.stopping_count - 1)
|
||||
v_cruise = 0
|
||||
elif self.xState == XState.e2eStop:
|
||||
self.stopping_count = 0
|
||||
if carstate.gasPressed: # Stop detecting traffic signal for 10 seconds
|
||||
#self.xState = XState.e2ePrepare
|
||||
self.xState = XState.e2eCruise
|
||||
self.traffic_starting_count = 10.0 / DT_MDL
|
||||
elif lead_detected and (radarstate.leadOne.dRel - stop_model_x) < 2.0:
|
||||
self.xState = XState.lead
|
||||
else:
|
||||
if self.trafficState == TrafficState.green:
|
||||
self.events.add(EventName.trafficSignGreen)
|
||||
self.xState = XState.e2eCruise
|
||||
else:
|
||||
self.comfort_brake = self.comfortBrake * 0.9
|
||||
#self.comfort_brake = COMFORT_BRAKE
|
||||
self.trafficStopAdjustRatio = np.interp(v_ego_kph, [0, 100], [1.0, 0.7])
|
||||
stop_dist = self.xStop * np.interp(self.xStop, [0, 50], [1.0, self.trafficStopAdjustRatio]) ##<23><><EFBFBD><EFBFBD><EFBFBD>Ÿ<EFBFBD><C5B8><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>Ÿ<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if stop_dist > 10.0: ### 10M<30>̻<EFBFBD><CCBB>϶<EFBFBD><CFB6><EFBFBD>, self.actual_stop_distance<63><65> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʈ<EFBFBD><C6AE>.
|
||||
self.actual_stop_distance = stop_dist
|
||||
stop_model_x = 0
|
||||
self.fakeCruiseDistance = 0 if self.actual_stop_distance > 10.0 else 10.0
|
||||
if v_ego < 0.3:
|
||||
self.stopping_count = 0.5 / DT_MDL
|
||||
self.xState = XState.e2eStopped
|
||||
elif self.xState == XState.e2ePrepare:
|
||||
if lead_detected:
|
||||
self.xState = XState.lead
|
||||
elif self.atc_active:
|
||||
if carstate.gasPressed:
|
||||
self.xState = XState.e2eCruise
|
||||
elif v_ego_kph < 5.0 and self.trafficState != TrafficState.green:
|
||||
self.xState = XState.e2eStop
|
||||
self.actual_stop_distance = 5.0 #2.0
|
||||
elif v_ego_kph > 5.0: # and stop_model_x > 30.0:
|
||||
self.xState = XState.e2eCruise
|
||||
else: #XState.lead, XState.cruise, XState.e2eCruise
|
||||
self.traffic_starting_count = max(0, self.traffic_starting_count - 1)
|
||||
if lead_detected:
|
||||
self.xState = XState.lead
|
||||
elif self.trafficState == TrafficState.red and abs(carstate.steeringAngleDeg) < 30 and self.traffic_starting_count == 0:
|
||||
self.events.add(EventName.trafficStopping)
|
||||
self.xState = XState.e2eStop
|
||||
self.actual_stop_distance = self.xStop
|
||||
else:
|
||||
self.xState = XState.e2eCruise
|
||||
|
||||
if self.trafficState in [TrafficState.off, TrafficState.green] or self.xState not in [XState.e2eStop, XState.e2eStopped]:
|
||||
stop_model_x = 1000.0
|
||||
|
||||
if self.user_stop_distance >= 0:
|
||||
self.user_stop_distance = max(0, self.user_stop_distance - v_ego * DT_MDL)
|
||||
self.actual_stop_distance = self.user_stop_distance
|
||||
self.xState = XState.e2eStop if self.user_stop_distance > 0 else XState.e2eStopped
|
||||
|
||||
if mode == 'acc':
|
||||
mode = 'blended' if self.xState in [XState.e2ePrepare] else 'acc'
|
||||
|
||||
self.comfort_brake *= self.mySafeFactor
|
||||
self.actual_stop_distance = max(0, self.actual_stop_distance - (v_ego * DT_MDL))
|
||||
|
||||
if stop_model_x == 1000.0: ## e2eCruise, lead<61>ΰ<EFBFBD><CEB0>
|
||||
self.actual_stop_distance = 0.0
|
||||
elif self.actual_stop_distance > 0: ## e2eStop, e2eStopped<65>ΰ<EFBFBD><CEB0>..
|
||||
stop_model_x = 0.0
|
||||
|
||||
# self.debugLongText = (
|
||||
# f"XState({str(self.xState)})," +
|
||||
# f"stop_x={stop_x:.1f}," +
|
||||
# f"stopDist={self.actual_stop_distance:.1f}," +
|
||||
# f"Traffic={str(self.trafficState)}"
|
||||
# )
|
||||
#<23><>ȣ<EFBFBD><C8A3> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> self.xState.value
|
||||
|
||||
stop_dist = stop_model_x + self.actual_stop_distance
|
||||
stop_dist = max(stop_dist, v_ego ** 2 / (self.comfort_brake * 2))
|
||||
|
||||
self.v_cruise = v_cruise
|
||||
self.stop_dist = stop_dist
|
||||
self.mode = mode
|
||||
#return v_cruise, stop_dist, mode
|
||||
|
||||
return v_cruise_kph
|
||||
|
||||
|
||||
class DrivingModeDetector:
|
||||
def __init__(self):
|
||||
self.congested = False
|
||||
self.speed_threshold = 2 # (km/h)
|
||||
self.accel_threshold = 1.5 # (m/s^2)
|
||||
self.distance_threshold = 12 # (m)
|
||||
self.lead_speed_exit_threshold = 35 # (km/h)
|
||||
|
||||
def update_data(self, my_speed, lead_speed, my_accel, lead_accel, distance):
|
||||
# 1. 정체 조건: 앞차가 가까이 있고 정지된 상황
|
||||
if distance <= self.distance_threshold and lead_speed <= self.speed_threshold:
|
||||
self.congested = True
|
||||
|
||||
# 2. 주행 조건: 앞차가 가속하거나 빠르게 이동
|
||||
if lead_accel > self.accel_threshold or my_speed > self.lead_speed_exit_threshold or distance >= 200:
|
||||
self.congested = False
|
||||
|
||||
def get_mode(self):
|
||||
return DrivingMode.Safe if self.congested else DrivingMode.Normal
|
||||
1036
selfdrive/carrot/carrot_man.py
Normal file
1036
selfdrive/carrot/carrot_man.py
Normal file
File diff suppressed because it is too large
Load Diff
1328
selfdrive/carrot/carrot_serv.py
Normal file
1328
selfdrive/carrot/carrot_serv.py
Normal file
File diff suppressed because it is too large
Load Diff
401
selfdrive/carrot/carrot_speed.py
Normal file
401
selfdrive/carrot/carrot_speed.py
Normal file
@@ -0,0 +1,401 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
CarrotSpeedTable v2.1 (Params backend, JSON+gzip, 1e-4° grid, 8 buckets)
|
||||
- 저장 키: "CarrotSpeedTable"
|
||||
- 포맷(JSON): {"format":"v5","dir_buckets":8,"cells":{"gy,gx":[[v,ts],...]} }
|
||||
- gzip 저장/로드 지원 (기본 on). 기존 비압축 v2도 로드 가능.
|
||||
- 격자: 위/경도 각 1e-4° 스냅(한국 위도에서 약 9~11m)
|
||||
- 저장: 단일 speed(부호 포함)만 해당 셀 1곳에 기록
|
||||
* 입력 > 0: 기존 None/음수/더 작은 양수면 갱신(더 큰 +)
|
||||
* 입력 < 0: 기존 None/양수/덜 음수면 갱신(더 작은 -)
|
||||
- 조회: 전방 lookahead 셀 → 없으면 이웃 탐색(ring=1)
|
||||
* 본셀: 시간 필터 없음
|
||||
* 이웃: 오래된 데이터만 사용(age ≥ 120s)
|
||||
- 정리(청소) 없음: 오래된 데이터도 유지
|
||||
"""
|
||||
|
||||
import json, math, threading, time, gzip
|
||||
from typing import Optional, Tuple, Dict, List
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
# ---------- 지오/도우미 ----------
|
||||
|
||||
def quantize_1e4(lat: float, lon: float) -> Tuple[int, int]:
|
||||
gy = int(math.floor(lat * 1e4 + 0.5))
|
||||
gx = int(math.floor(lon * 1e4 + 0.5))
|
||||
return gy, gx
|
||||
|
||||
def heading_to_bucket(heading_deg: float) -> int:
|
||||
# 8 버킷 고정
|
||||
step = 45.0 # 360/8
|
||||
i = int((heading_deg % 360.0) // step)
|
||||
if i < 0: return 0
|
||||
if i > 7: return 7
|
||||
return i
|
||||
|
||||
DIR_8 = {
|
||||
0: ( 1, 0), # 북
|
||||
1: ( 1, 1), # 북동
|
||||
2: ( 0, 1), # 동
|
||||
3: (-1, 1), # 남동
|
||||
4: (-1, 0), # 남
|
||||
5: (-1, -1), # 남서
|
||||
6: ( 0, -1), # 서
|
||||
7: ( 1, -1), # 북서
|
||||
}
|
||||
|
||||
def project_point(lat: float, lon: float, heading_deg: float, distance_m: float) -> Tuple[float, float]:
|
||||
if distance_m <= 0.0:
|
||||
return lat, lon
|
||||
R = 6_371_000.0
|
||||
h = math.radians(heading_deg)
|
||||
dlat = (distance_m * math.cos(h)) / R
|
||||
dlon = (distance_m * math.sin(h)) / (R * math.cos(math.radians(lat)))
|
||||
return lat + math.degrees(dlat), lon + math.degrees(dlon)
|
||||
|
||||
def _is_gzip(data: bytes) -> bool:
|
||||
return len(data) >= 2 and data[0] == 0x1F and data[1] == 0x8B
|
||||
|
||||
|
||||
# ---------- 메인 클래스 ----------
|
||||
|
||||
class CarrotSpeed:
|
||||
KEY = "CarrotSpeedTable"
|
||||
|
||||
def __init__(self,
|
||||
neighbor_ring: int = 1,
|
||||
neighbor_old_threshold_s: int = 120,
|
||||
use_gzip: bool = True,
|
||||
gzip_level: int = 5):
|
||||
# 고정 사양
|
||||
self.buckets = 8
|
||||
|
||||
# 파라미터
|
||||
self.neighbor_ring = max(0, int(neighbor_ring))
|
||||
self.neighbor_old_threshold_s = int(neighbor_old_threshold_s)
|
||||
self.use_gzip = bool(use_gzip)
|
||||
self.gzip_level = int(gzip_level)
|
||||
|
||||
# 내부 상태
|
||||
self._lock = threading.RLock()
|
||||
# _cells[(gy,gx)] = [[value or None, ts(int seconds) or None] * 8]
|
||||
self._cells: Dict[Tuple[int, int], List[List[Optional[float]]]] = {}
|
||||
self._dirty = False
|
||||
self._last_save = 0
|
||||
self._params = Params()
|
||||
|
||||
self._load_from_params_if_exists()
|
||||
|
||||
self._last_hit = None # (gy, gx, b, ts_when_read)
|
||||
self._last_hit_read_ms = 0 # 밀리초
|
||||
|
||||
# ----- 내부 유틸 -----
|
||||
|
||||
def _ensure_cell(self, gy: int, gx: int) -> List[List[Optional[float]]]:
|
||||
arr = self._cells.get((gy, gx))
|
||||
if arr is None:
|
||||
arr = [[None, None] for _ in range(self.buckets)] # [v, ts]
|
||||
self._cells[(gy, gx)] = arr
|
||||
return arr
|
||||
|
||||
def _now(self) -> int:
|
||||
# int 초
|
||||
return int(time.time())
|
||||
|
||||
def _age(self, ts: Optional[float]) -> Optional[int]:
|
||||
if ts is None:
|
||||
return None
|
||||
return self._now() - int(ts)
|
||||
|
||||
def _neighbor_indices(self, gy: int, gx: int) -> List[Tuple[int, int]]:
|
||||
r = self.neighbor_ring
|
||||
if r <= 0:
|
||||
return []
|
||||
out = []
|
||||
for dy in range(-r, r + 1):
|
||||
for dx in range(-r, r + 1):
|
||||
if dy == 0 and dx == 0:
|
||||
continue
|
||||
out.append((gy + dy, gx + dx))
|
||||
return out
|
||||
|
||||
def _neighbors_8(self, gy, gx):
|
||||
for dy in (-1, 0, 1):
|
||||
for dx in (-1, 0, 1):
|
||||
if dy == 0 and dx == 0:
|
||||
continue
|
||||
yield gy + dy, gx + dx
|
||||
|
||||
def _try_cell_bucket_old(self, arr, b):
|
||||
v, ts = arr[b]
|
||||
if v is None or ts is None:
|
||||
return None, None
|
||||
if self._now() - int(ts) < self.neighbor_old_threshold_s:
|
||||
return None, None
|
||||
return float(v), b
|
||||
# ----- 공용 API -----
|
||||
def export_cells_around(self, lat: float, lon: float,
|
||||
heading_deg: float,
|
||||
ring: int = 1, max_points: int = 64) -> str:
|
||||
"""
|
||||
현재 lat, lon 기준 주변 그리드(ring 범위)에서
|
||||
값이 있는 셀들을 (lat, lon, speed) 리스트로 JSON으로 반환.
|
||||
Params("CarrotSpeedViz")에 그대로 넣을 용도.
|
||||
"""
|
||||
gy0, gx0 = quantize_1e4(lat, lon)
|
||||
b0 = heading_to_bucket(heading_deg)
|
||||
pts = []
|
||||
|
||||
with self._lock:
|
||||
for dy in range(-ring, ring + 1):
|
||||
for dx in range(-ring, ring + 1):
|
||||
gy = gy0 + dy
|
||||
gx = gx0 + dx
|
||||
arr = self._cells.get((gy, gx))
|
||||
if not arr:
|
||||
continue
|
||||
|
||||
# 먼저 exact bucket(b0)
|
||||
v, ts = arr[b0]
|
||||
if v is not None:
|
||||
cell_lat = (gy + 0.5) * 1e-4
|
||||
cell_lon = (gx + 0.5) * 1e-4
|
||||
pts.append([cell_lat, cell_lon, float(v)])
|
||||
if len(pts) >= max_points:
|
||||
return json.dumps({"pts": pts}, separators=(",",":"))
|
||||
|
||||
# 없다면 좌/우
|
||||
for b in ((b0 - 1) % self.buckets, (b0 + 1) % self.buckets):
|
||||
v, ts = arr[b]
|
||||
if v is None:
|
||||
continue
|
||||
cell_lat = (gy + 0.5) * 1e-4
|
||||
cell_lon = (gx + 0.5) * 1e-4
|
||||
pts.append([cell_lat, cell_lon, float(v)])
|
||||
if len(pts) >= max_points:
|
||||
return json.dumps({"pts": pts}, separators=(",",":"))
|
||||
|
||||
return json.dumps({"pts": pts}, separators=(",",":"))
|
||||
|
||||
def add_sample(self, lat: float, lon: float, heading_deg: float, speed_signed: float):
|
||||
"""
|
||||
단일 speed(부호 포함) 저장.
|
||||
- 기준 셀(현재 위치) + heading 기준 좌/우 1셀, 2셀까지 동일 speed 기록
|
||||
- 각 셀 안에서는 heading 버킷 b와 b±1 세 개 버킷 모두 같은 값으로 갱신.
|
||||
- >0: 기존 음수/None도 교체, 기존 양수면 평균으로 완만하게 갱신.
|
||||
- <0: 항상 새 음수로 덮어쓰기(돌발 감속 우선).
|
||||
==0: 무시
|
||||
"""
|
||||
v_in = round(float(speed_signed), 1)
|
||||
if v_in == 0.0:
|
||||
return
|
||||
|
||||
# 현재 위치를 그리드로
|
||||
gy0, gx0 = quantize_1e4(lat, lon)
|
||||
b = heading_to_bucket(heading_deg)
|
||||
now = self._now()
|
||||
|
||||
# bucket에 해당하는 전진 방향 그리드 벡터
|
||||
dy_f, dx_f = DIR_8[b]
|
||||
|
||||
# heading 기준 좌/우 1셀, 2셀 (project_point 사용 X)
|
||||
# 좌 = 전진벡터를 90° 회전 (dy,dx) -> (dx,-dy)
|
||||
# 우 = 전진벡터를 -90° 회전 (dy,dx) -> (-dx,dy)
|
||||
dy_l1, dx_l1 = dx_f, -dy_f
|
||||
dy_r1, dx_r1 = -dx_f, dy_f
|
||||
|
||||
dy_l2, dx_l2 = 2 * dy_l1, 2 * dx_l1
|
||||
dy_r2, dx_r2 = 2 * dy_r1, 2 * dx_r1
|
||||
|
||||
# 기록할 셀들: 중앙 + 좌/우 1칸 + 좌/우 2칸
|
||||
target_cells = {
|
||||
(gy0, gx0),
|
||||
(gy0 + dy_l1, gx0 + dx_l1),
|
||||
(gy0 + dy_r1, gx0 + dx_r1),
|
||||
(gy0 + dy_l2, gx0 + dx_l2),
|
||||
(gy0 + dy_r2, gx0 + dx_r2),
|
||||
}
|
||||
|
||||
with self._lock:
|
||||
for gy, gx in target_cells:
|
||||
arr = self._ensure_cell(gy, gx)
|
||||
|
||||
# b, b-1, b+1 세 버킷 모두 같은 정책으로 업데이트
|
||||
for off in (0, -1, +1):
|
||||
bi = (b + off) % self.buckets
|
||||
v_old, ts_old = arr[bi]
|
||||
|
||||
if v_old is None:
|
||||
# 처음 쓰는 버킷
|
||||
arr[bi] = [v_in, now]
|
||||
else:
|
||||
if v_in > 0.0:
|
||||
# 가속 정보: 기존 양수면 평균, 음수면 교체
|
||||
if v_old < 0.0:
|
||||
# 음수 -> 양수로 바뀌면 새 양수로 교체 (ts는 기존 유지)
|
||||
arr[bi] = [v_in, ts_old]
|
||||
else:
|
||||
new_val = round((v_old + v_in) / 2.0, 1)
|
||||
arr[bi] = [new_val, ts_old]
|
||||
else:
|
||||
# 감속 정보: 항상 새 음수로 덮어쓰기, ts는 기존 유지
|
||||
arr[bi] = [v_in, ts_old]
|
||||
|
||||
self._dirty = True
|
||||
|
||||
|
||||
def query_target(self, lat: float, lon: float, heading_deg: float, v_ego: float,
|
||||
lookahead_s: float = 2.0) -> float:
|
||||
dist = max(0.0, float(v_ego) * float(lookahead_s))
|
||||
return self.query_target_dist(lat, lon, heading_deg, dist)
|
||||
|
||||
def query_target_dist(self, lat: float, lon: float, heading_deg: float, dist: float) -> float:
|
||||
b = heading_to_bucket(heading_deg)
|
||||
|
||||
cand_ds = [dist]
|
||||
for off in (3.0, -3.0):
|
||||
d2 = dist + off
|
||||
if d2 >= 0.0:
|
||||
cand_ds.append(d2)
|
||||
|
||||
with self._lock:
|
||||
for d in cand_ds:
|
||||
y, x = project_point(lat, lon, heading_deg, d)
|
||||
gy, gx = quantize_1e4(y, x)
|
||||
|
||||
arr = self._cells.get((gy, gx))
|
||||
if not arr:
|
||||
continue
|
||||
|
||||
v, b_sel = self._try_cell_bucket_old(arr, b)
|
||||
if v is not None:
|
||||
now_sec = int(time.time())
|
||||
self._last_hit = (gy, gx, b_sel, now_sec)
|
||||
self._last_hit_read_ms = int(time.time() * 1000)
|
||||
return v
|
||||
|
||||
return 0.0
|
||||
|
||||
def invalidate_last_hit(self, window_s: float = 2.0, action: str = "clear") -> bool:
|
||||
if self._last_hit is None:
|
||||
return False
|
||||
gy, gx, b, read_ts = self._last_hit
|
||||
now = int(time.time())
|
||||
if (now - int(read_ts)) > window_s:
|
||||
return False
|
||||
|
||||
with self._lock:
|
||||
arr = self._cells.get((gy, gx))
|
||||
if not arr:
|
||||
return False
|
||||
|
||||
# b, b-1, b+1 모두 invalidate
|
||||
for off in (0, -1, +1):
|
||||
bi = (b + off) % self.buckets
|
||||
v, ts = arr[bi]
|
||||
|
||||
if action == "clear":
|
||||
if v is not None and v < 0.0:
|
||||
arr[bi] = [None, None]
|
||||
else: # "age_bump"
|
||||
if v is not None:
|
||||
arr[bi] = [v, now]
|
||||
else:
|
||||
# 값이 없으면 넘어가기만 (그 버킷만 skip)
|
||||
pass
|
||||
|
||||
self._dirty = True
|
||||
|
||||
return True
|
||||
|
||||
def maybe_save(self, interval_s: int = 60) -> None:
|
||||
now = self._now()
|
||||
if (not self._dirty) or (now - self._last_save < interval_s):
|
||||
return
|
||||
self.save()
|
||||
|
||||
def save(self) -> None:
|
||||
payload = self._encode_payload()
|
||||
self._params.put_nonblocking(self.KEY, payload)
|
||||
self._last_save = self._now()
|
||||
self._dirty = False
|
||||
|
||||
def close(self) -> None:
|
||||
try:
|
||||
if self._dirty:
|
||||
self.save()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ----- 직렬화 -----
|
||||
|
||||
def _encode_payload(self) -> bytes:
|
||||
with self._lock:
|
||||
cells = {}
|
||||
for (gy, gx), arr in self._cells.items():
|
||||
key = f"{gy},{gx}"
|
||||
# arr: [[v, ts], ...] (ts는 int 또는 None)
|
||||
cells[key] = [[None if v is None else float(v),
|
||||
None if ts is None else int(ts)] for (v, ts) in arr]
|
||||
obj = {"format": "v5", "dir_buckets": self.buckets, "cells": cells}
|
||||
raw = json.dumps(obj, separators=(",", ":")).encode("utf-8")
|
||||
if self.use_gzip:
|
||||
return gzip.compress(raw, compresslevel=self.gzip_level)
|
||||
return raw
|
||||
|
||||
def _load_from_params_if_exists(self) -> None:
|
||||
raw = self._params.get(self.KEY)
|
||||
if not raw:
|
||||
return
|
||||
try:
|
||||
data_bytes = raw
|
||||
if _is_gzip(data_bytes):
|
||||
data_bytes = gzip.decompress(data_bytes)
|
||||
data = json.loads(data_bytes.decode("utf-8"))
|
||||
|
||||
# v3 아니면 삭제/초기화
|
||||
if data.get("format") != "v5":
|
||||
self._params.remove(self.KEY)
|
||||
with self._lock:
|
||||
self._cells = {}
|
||||
self._dirty = False
|
||||
return
|
||||
|
||||
buckets = int(data.get("dir_buckets", 8))
|
||||
if buckets != 8:
|
||||
# 버킷 불일치도 삭제/초기화
|
||||
self._params.remove(self.KEY)
|
||||
with self._lock:
|
||||
self._cells = {}
|
||||
self._dirty = False
|
||||
return
|
||||
|
||||
restored: Dict[Tuple[int, int], List[List[Optional[float]]]] = {}
|
||||
for key, arr in data.get("cells", {}).items():
|
||||
gy, gx = map(int, key.split(","))
|
||||
fixed: List[List[Optional[float]]] = []
|
||||
if isinstance(arr, list) and len(arr) == 8:
|
||||
for pair in arr:
|
||||
if isinstance(pair, list) and len(pair) == 2:
|
||||
v, ts = pair
|
||||
v2 = None if v is None else float(v)
|
||||
# ts는 int로 강제
|
||||
ts2 = None if ts is None else int(ts)
|
||||
fixed.append([v2, ts2])
|
||||
else:
|
||||
fixed.append([None, None])
|
||||
else:
|
||||
fixed = [[None, None] for _ in range(8)]
|
||||
restored[(gy, gx)] = fixed
|
||||
|
||||
with self._lock:
|
||||
self._cells = restored
|
||||
self._dirty = False
|
||||
|
||||
except Exception:
|
||||
# 파싱 실패 시 안전 초기화
|
||||
self._params.delete(self.KEY)
|
||||
with self._lock:
|
||||
self._cells = {}
|
||||
self._dirty = False
|
||||
Reference in New Issue
Block a user