forked from IQ.Lvbs/IQ.Pilot
564 lines
22 KiB
Python
564 lines
22 KiB
Python
import time
|
|
import math
|
|
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.volkswagen.values import VolkswagenFlags
|
|
from iqdbc.car.lateral import ISO_LATERAL_ACCEL
|
|
|
|
NOT_SET = 0
|
|
SPEED_SUGGESTED_MAX_HIGHWAY_GER_KPH = 130 # 130 kph in germany
|
|
STREET_TYPE_URBAN = 1
|
|
STREET_TYPE_NONURBAN = 2
|
|
STREET_TYPE_HIGHWAY = 3
|
|
SANITY_CHECK_DIFF_PERCENT_LOWER = 30
|
|
SPEED_LIMIT_UNLIMITED_VZE_KPH = int(round(144 * CV.MS_TO_KPH))
|
|
DECELERATION_PREDICATIVE = 1.0
|
|
SEGMENT_DECAY = 10
|
|
PSD_TYPE_SPEED_LIMIT = 1
|
|
PSD_TYPE_CURV_SPEED = 2
|
|
PSD_CURV_SPEED_DECAY = 4
|
|
PSD_UNIT_KPH = 0
|
|
PSD_UNIT_MPH = 1
|
|
|
|
|
|
class SpeedLimitManager:
|
|
def __init__(self, car_params, speed_limit_max_kph=SPEED_SUGGESTED_MAX_HIGHWAY_GER_KPH, predicative=False, predicative_speed_limit=False, predicative_curve=False):
|
|
self.CP = car_params
|
|
self.v_limit_psd = NOT_SET
|
|
self.v_limit_psd_next = NOT_SET
|
|
self.v_limit_psd_legal = NOT_SET
|
|
self.v_limit_psd_next_type = NOT_SET
|
|
self.v_limit_vze = NOT_SET
|
|
self.v_limit_speed_unit_psd = PSD_UNIT_KPH
|
|
self.v_limit_vze_sanity_error = False
|
|
self.v_limit_output_last = NOT_SET
|
|
self.v_limit_max = speed_limit_max_kph
|
|
self.predicative = predicative
|
|
self.predicative_speed_limit = predicative_speed_limit
|
|
self.predicative_curve = predicative_curve
|
|
self.predicative_segments = {}
|
|
self.current_predicative_segment = {"ID": NOT_SET, "Length": NOT_SET, "Speed": NOT_SET, "StreetType": NOT_SET, "OnRampExit": NOT_SET}
|
|
self.v_limit_psd_next_last_timestamp = 0
|
|
self.v_limit_psd_next_last = NOT_SET
|
|
self.v_limit_psd_next_decay_time = NOT_SET
|
|
self.v_limit_changed = False
|
|
|
|
def _reset_predicative(self):
|
|
self.v_limit_psd_next = NOT_SET
|
|
self.v_limit_psd_next_type = NOT_SET
|
|
self.v_limit_psd_next_last_timestamp = 0
|
|
self.v_limit_psd_next_last = NOT_SET
|
|
self.v_limit_psd_next_decay_time = NOT_SET
|
|
|
|
def enable_predicative_speed_limit(self, predicative=False, reaction_to_speed_limits=False, reaction_to_curves=False):
|
|
if self.predicative == predicative and self.predicative_speed_limit == reaction_to_speed_limits and self.predicative_curve == reaction_to_curves:
|
|
return # perf
|
|
|
|
if not predicative or (not reaction_to_speed_limits and not reaction_to_curves):
|
|
self._reset_predicative()
|
|
self.predicative = False # fully disable execution
|
|
else:
|
|
self.predicative = predicative
|
|
|
|
if (not reaction_to_speed_limits and self.predicative_speed_limit) or (not reaction_to_curves and self.predicative_curve):
|
|
self._reset_predicative() # force reset when disabling
|
|
|
|
self.predicative_speed_limit = reaction_to_speed_limits
|
|
self.predicative_curve = reaction_to_curves
|
|
|
|
|
|
def update(self, current_speed_ms, psd_04, psd_05, psd_06, vze, raining, time_car):
|
|
# try reading speed form traffic sign recognition
|
|
if vze and self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
self._receive_speed_limit_vze_meb(vze)
|
|
|
|
# read speed unit from PSD_06 and also use it for traffic sign recognition if present (for now always seen on bus 0)
|
|
# the vze location/unit flag is not stable and no corresponding flag has been found yet
|
|
if psd_06:
|
|
self._receive_speed_unit_psd(psd_06)
|
|
|
|
# try reading speed from predicative street data
|
|
if psd_04 and psd_05 and psd_06:
|
|
self._receive_current_segment_psd(psd_05)
|
|
self._refresh_current_segment()
|
|
self._build_predicative_segments(psd_04, psd_06, raining, time_car)
|
|
self._receive_speed_limit_psd_legal(psd_06)
|
|
self._get_speed_limit_psd()
|
|
if self.predicative:
|
|
self._get_speed_limit_psd_next(current_speed_ms) # this is very cpu heavy
|
|
|
|
def get_speed_limit_predicative(self):
|
|
v_limit_output = self.v_limit_psd_next if self.predicative and self.v_limit_psd_next != NOT_SET and self.v_limit_psd_next < self.v_limit_output_last else NOT_SET
|
|
return v_limit_output * CV.KPH_TO_MS
|
|
|
|
def get_speed_limit_predicative_type(self):
|
|
return self.v_limit_psd_next_type
|
|
|
|
def get_speed_limit(self):
|
|
candidates = {
|
|
"vze": self.v_limit_vze if self.v_limit_vze != NOT_SET and not self.v_limit_vze_sanity_error else NOT_SET,
|
|
"psd": self.v_limit_psd if self.v_limit_psd != NOT_SET else NOT_SET,
|
|
"legal": self.v_limit_psd_legal
|
|
}
|
|
|
|
v_limit_output = NOT_SET
|
|
for source in ["vze", "psd", "legal"]:
|
|
v = candidates[source]
|
|
if v != NOT_SET:
|
|
v_limit_output = v
|
|
break
|
|
|
|
if v_limit_output > self.v_limit_max:
|
|
v_limit_output = self.v_limit_max
|
|
|
|
self.v_limit_changed = True if self.v_limit_output_last != v_limit_output else False
|
|
self.v_limit_output_last = v_limit_output
|
|
|
|
return v_limit_output * CV.KPH_TO_MS
|
|
|
|
def _speed_limit_vze_sanitiy_check(self, speed_limit_vze_new):
|
|
if self.v_limit_output_last == NOT_SET:
|
|
self.v_limit_vze_sanity_error = False
|
|
return
|
|
|
|
diff_p = 100 * speed_limit_vze_new / self.v_limit_output_last
|
|
self.v_limit_vze_sanity_error = True if diff_p < SANITY_CHECK_DIFF_PERCENT_LOWER else False
|
|
if speed_limit_vze_new > SPEED_LIMIT_UNLIMITED_VZE_KPH: # unlimited sign detected: use psd logic for setting maximum speed
|
|
self.v_limit_vze_sanity_error = True
|
|
|
|
def _receive_speed_unit_psd(self, psd_06):
|
|
# keep it simple for now, the unit is supplied shortly before the corresponding speed limits are supplied for given segment ID
|
|
if psd_06["PSD_06_Mux"] == 0 and psd_06["PSD_Sys_Segment_ID"] > 1:
|
|
self.v_limit_speed_unit_psd = psd_06["PSD_Sys_Geschwindigkeit_Einheit"]
|
|
|
|
def _convert_raw_speed_psd(self, raw_speed, street_type):
|
|
speed = NOT_SET
|
|
|
|
if self.v_limit_speed_unit_psd == PSD_UNIT_KPH:
|
|
if 0 < raw_speed < 11: # 0 - 45 kph
|
|
speed = (raw_speed - 1) * 5
|
|
elif 11 <= raw_speed < 23: # 50 - 160 kph
|
|
speed = 50 + (raw_speed - 11) * 10
|
|
elif raw_speed == 23: # explicitly no legal speed limit
|
|
if street_type == STREET_TYPE_HIGHWAY:
|
|
speed = self.v_limit_max
|
|
|
|
elif self.v_limit_speed_unit_psd == PSD_UNIT_MPH:
|
|
if 3 < raw_speed < 18: # 0 - 70 mph
|
|
speed = (5 * (raw_speed - 3)) * CV.MPH_TO_KPH
|
|
elif 18 <= raw_speed < 23: # 75 - 105 mph
|
|
speed = ((5 * (raw_speed - 3)) + 10) * CV.MPH_TO_KPH
|
|
elif raw_speed == 23: # explicitly no legal speed limit
|
|
if street_type == STREET_TYPE_HIGHWAY:
|
|
speed = self.v_limit_max
|
|
|
|
return speed
|
|
|
|
def _receive_speed_limit_vze_meb(self, vze):
|
|
v_limit_vze = vze["VZE_Verkehrszeichen_1"] # main traffic sign
|
|
# "VZE_Anzeigemodus" has been seen not being stable for different drives in same USA mph car, no other flag has been found yet
|
|
# for now additionally assume a car with mph speed unit nav data sgements is also supplied with mph converted vze data ("PSD_06" is available on bus 0)
|
|
# mph speed unit set in signal "Einheiten_01" does not mean the speed data being supplied in mph unit!
|
|
v_limit_vze = v_limit_vze * CV.MPH_TO_KPH if vze["VZE_Anzeigemodus"] == 1 or self.v_limit_speed_unit_psd == PSD_UNIT_MPH else v_limit_vze
|
|
self._speed_limit_vze_sanitiy_check(v_limit_vze)
|
|
self.v_limit_vze = v_limit_vze
|
|
|
|
def _receive_current_segment_psd(self, psd_05):
|
|
if psd_05["PSD_Pos_Standort_Eindeutig"] == 1 and psd_05["PSD_Pos_Segment_ID"] != NOT_SET:
|
|
self.current_predicative_segment["Length"] = psd_05["PSD_Pos_Segmentlaenge"]
|
|
|
|
if self.current_predicative_segment["ID"] != psd_05["PSD_Pos_Segment_ID"]:
|
|
self.current_predicative_segment["ID"] = psd_05["PSD_Pos_Segment_ID"]
|
|
self.current_predicative_segment["Speed"] = NOT_SET
|
|
self.current_predicative_segment["StreetType"] = NOT_SET
|
|
self.current_predicative_segment["OnRampExit"] = False
|
|
|
|
def _get_segment_curvature_psd(self, psd_curvature, psd_sign, scale=2e-5):
|
|
if psd_curvature in (0, 255):
|
|
return NOT_SET
|
|
|
|
curvature = (255 - psd_curvature) * scale
|
|
if psd_sign == 1:
|
|
curvature *= -1
|
|
|
|
return curvature
|
|
|
|
def _calculate_curve_speed(self, curvature):
|
|
if curvature == NOT_SET:
|
|
return NOT_SET
|
|
|
|
curv_speed_ms = math.sqrt(ISO_LATERAL_ACCEL / abs(curvature))
|
|
|
|
if self.v_limit_speed_unit_psd == PSD_UNIT_MPH:
|
|
curv_speed = int((curv_speed_ms * CV.MS_TO_MPH) // 5 * 5) * CV.MPH_TO_KPH
|
|
else:
|
|
curv_speed = int((curv_speed_ms * CV.MS_TO_KPH) // 5 * 5)
|
|
|
|
return curv_speed
|
|
|
|
def _refresh_current_segment(self):
|
|
current_segment = self.current_predicative_segment["ID"]
|
|
if current_segment != NOT_SET:
|
|
seg = self.predicative_segments.get(current_segment)
|
|
if seg:
|
|
self.current_predicative_segment["Speed"] = self.predicative_segments[current_segment]["Speed"]
|
|
self.current_predicative_segment["StreetType"] = self.predicative_segments[current_segment]["StreetType"]
|
|
self.current_predicative_segment["OnRampExit"] = self.predicative_segments[current_segment]["OnRampExit"]
|
|
|
|
def _build_predicative_segments(self, psd_04, psd_06, raining, time_car):
|
|
now = time.time()
|
|
|
|
# Segment erfassen/aktualisieren
|
|
if (psd_04["PSD_ADAS_Qualitaet"] == 1 and
|
|
psd_04["PSD_wahrscheinlichster_Pfad"] == 1 and
|
|
psd_04["PSD_Segment_ID"] != NOT_SET):
|
|
|
|
segment_id = psd_04["PSD_Segment_ID"]
|
|
seg = self.predicative_segments.get(segment_id)
|
|
if seg:
|
|
seg["Length"] = psd_04["PSD_Segmentlaenge"]
|
|
seg["Curvature_Begin"] = self._get_segment_curvature_psd(psd_04["PSD_Anfangskruemmung"], psd_04["PSD_Anfangskruemmung_Vorz"])
|
|
seg["Curvature_End"] = self._get_segment_curvature_psd(psd_04["PSD_Endkruemmung"], psd_04["PSD_Endkruemmung_Vorz"])
|
|
seg["StreetType"] = self._get_street_type(psd_04["PSD_Strassenkategorie"], psd_04["PSD_Bebauung"])
|
|
seg["OnRampExit"] = psd_04["PSD_Rampe"] in (1, 2)
|
|
seg["ID_Prev"] = psd_04["PSD_Vorgaenger_Segment_ID"]
|
|
seg["Curve_Speed_Begin"] = self._calculate_curve_speed(seg["Curvature_Begin"])
|
|
seg["Curve_Speed_End"] = self._calculate_curve_speed(seg["Curvature_End"])
|
|
seg["Timestamp"] = now
|
|
else:
|
|
self.predicative_segments[segment_id] = {
|
|
"ID": segment_id,
|
|
"Length": psd_04["PSD_Segmentlaenge"],
|
|
"Curvature_Begin": self._get_segment_curvature_psd(psd_04["PSD_Anfangskruemmung"], psd_04["PSD_Anfangskruemmung_Vorz"]),
|
|
"Curvature_End": self._get_segment_curvature_psd(psd_04["PSD_Endkruemmung"], psd_04["PSD_Endkruemmung_Vorz"]),
|
|
"StreetType": self._get_street_type(psd_04["PSD_Strassenkategorie"], psd_04["PSD_Bebauung"]),
|
|
"OnRampExit": psd_04["PSD_Rampe"] in (1, 2),
|
|
"ID_Prev": psd_04["PSD_Vorgaenger_Segment_ID"],
|
|
"Speed": NOT_SET,
|
|
"Curve_Speed_Begin": NOT_SET,
|
|
"Curve_Speed_End": NOT_SET,
|
|
"QualityFlag": False,
|
|
"Timestamp": now
|
|
}
|
|
seg = self.predicative_segments.get(segment_id)
|
|
seg["Curve_Speed_Begin"] = self._calculate_curve_speed(seg["Curvature_Begin"])
|
|
seg["Curve_Speed_End"] = self._calculate_curve_speed(seg["Curvature_End"])
|
|
|
|
# Schritt 2: Alte Segmente bereinigen
|
|
current_id = self.current_predicative_segment["ID"]
|
|
if current_id != NOT_SET:
|
|
self.predicative_segments = {
|
|
sid: seg for sid, seg in self.predicative_segments.items()
|
|
if now - seg.get("Timestamp", 0) <= SEGMENT_DECAY
|
|
}
|
|
|
|
# Geschwindigkeit setzen (speed limits seen for changed limits only)
|
|
if (psd_06["PSD_06_Mux"] == 2 and
|
|
psd_06["PSD_Ges_Typ"] == 1 and
|
|
psd_06["PSD_Ges_Gesetzlich_Kategorie"] == 0 and
|
|
psd_06["PSD_Ges_Segment_ID"] != NOT_SET):
|
|
|
|
raw_speed = psd_06["PSD_Ges_Geschwindigkeit"] if self._speed_limit_is_valid_now_psd(psd_06, raining, time_car) else NOT_SET
|
|
segment_id = psd_06["PSD_Ges_Segment_ID"]
|
|
|
|
if segment_id in self.predicative_segments:
|
|
speed = self._convert_raw_speed_psd(raw_speed, self.predicative_segments[segment_id]["StreetType"])
|
|
self.predicative_segments[segment_id]["Speed"] = speed
|
|
self.predicative_segments[segment_id]["Speed_Type"] = PSD_TYPE_SPEED_LIMIT
|
|
self.predicative_segments[segment_id]["QualityFlag"] = True
|
|
|
|
def _get_time_from_vw_datetime(self, time_car):
|
|
if time_car:
|
|
try:
|
|
t = (time_car["UH_Jahr"], time_car["UH_Monat"], time_car["UH_Tag"], time_car["UH_Stunde"], time_car["UH_Minute"], time_car["UH_Sekunde"], 0, 0, -1)
|
|
local_time = time.mktime(t)
|
|
return time.localtime(local_time)
|
|
except Exception:
|
|
return time.localtime()
|
|
else:
|
|
return time.localtime()
|
|
|
|
def _speed_limit_is_valid_now_psd(self, psd_06, raining, time_car):
|
|
local_time = self._get_time_from_vw_datetime(time_car)
|
|
|
|
# by day
|
|
day_start = psd_06["PSD_Ges_Geschwindigkeit_Tag_Anf"]
|
|
day_end = psd_06["PSD_Ges_Geschwindigkeit_Tag_Ende"]
|
|
now_weekday = (local_time.tm_wday + 1) # Python: 0=Mon -> PSD: 1=Mon
|
|
|
|
if 1 <= day_start <= 7 and 1 <= day_end <= 7:
|
|
if day_start <= day_end:
|
|
is_valid_by_day = day_start <= now_weekday <= day_end
|
|
else:
|
|
is_valid_by_day = now_weekday >= day_start or now_weekday <= day_end
|
|
else:
|
|
is_valid_by_day = True
|
|
|
|
# by time
|
|
hour_start = psd_06["PSD_Ges_Geschwindigkeit_Std_Anf"]
|
|
hour_end = psd_06["PSD_Ges_Geschwindigkeit_Std_Ende"]
|
|
now_hour = local_time.tm_hour
|
|
|
|
if (hour_start != 25 and hour_end != 25):
|
|
if hour_start <= hour_end:
|
|
is_valid_by_time = hour_start <= now_hour < hour_end
|
|
else:
|
|
is_valid_by_time = now_hour >= hour_start or now_hour < hour_end
|
|
else:
|
|
is_valid_by_time = True
|
|
|
|
# by weather condition
|
|
weather_condition = psd_06["PSD_Ges_Geschwindigkeit_Witter"]
|
|
is_valid_by_weather_conditions = weather_condition == 0 or ( raining and weather_condition == 1 )
|
|
|
|
checks = [
|
|
is_valid_by_time,
|
|
is_valid_by_day,
|
|
is_valid_by_weather_conditions,
|
|
]
|
|
|
|
return all(checks)
|
|
|
|
def _speed_limit_curve_allowed(self, seg):
|
|
currently_on_ramp = self.current_predicative_segment.get("OnRampExit", False)
|
|
seg_on_ramp = seg.get("OnRampExit", False)
|
|
|
|
ramp_allowed_on_ramp = currently_on_ramp and seg_on_ramp
|
|
ramp_allowed = ramp_allowed_on_ramp or not seg_on_ramp
|
|
|
|
# on ramp is probably type 2 TODO
|
|
# for now only allow non urban, there are problems with highway curvature data
|
|
street_type = seg.get("StreetType", NOT_SET)
|
|
street_type_allowed = True if street_type == STREET_TYPE_NONURBAN or (street_type == STREET_TYPE_HIGHWAY and ramp_allowed_on_ramp) else False
|
|
|
|
speed_curve = seg.get("Curve_Speed", NOT_SET)
|
|
|
|
if NOT_SET not in (self.v_limit_output_last, speed_curve):
|
|
diff_p = 100 * speed_curve / self.v_limit_output_last
|
|
sanity_error = True if diff_p < SANITY_CHECK_DIFF_PERCENT_LOWER else False
|
|
else:
|
|
sanity_error = False
|
|
|
|
checks = [
|
|
ramp_allowed,
|
|
street_type_allowed,
|
|
not sanity_error,
|
|
]
|
|
|
|
return all(checks)
|
|
|
|
def _hit_linear_profile(self, v0_ms, a, total_dist_m, L_m, v_b_kmh, v_e_kmh):
|
|
if L_m is None or L_m <= 0:
|
|
return None, None
|
|
|
|
v_b = v_b_kmh * CV.KPH_TO_MS
|
|
v_e = v_e_kmh * CV.KPH_TO_MS
|
|
r = (v_e - v_b) / L_m # m/s pro m
|
|
|
|
# Flaches Profil => Standard-Bremsweg gegen v_b am Beginn
|
|
if abs(r) < 1e-9:
|
|
if v0_ms <= v_b:
|
|
return None, None
|
|
brake_dist = (v0_ms**2 - v_b**2) / (2*a)
|
|
if brake_dist >= total_dist_m:
|
|
# Aktivierung am Segmentbeginn
|
|
return 0.0, v_b_kmh
|
|
return None, None
|
|
|
|
A = r*r
|
|
B = 2*v_b*r + 2*a
|
|
C = v_b*v_b + 2*a*total_dist_m - v0_ms*v0_ms
|
|
|
|
D = B*B - 4*A*C
|
|
if D < 0:
|
|
return None, None
|
|
|
|
sqrtD = math.sqrt(D)
|
|
# Smallest non-negative solution
|
|
s1 = (-B - sqrtD) / (2*A)
|
|
s2 = (-B + sqrtD) / (2*A)
|
|
s_candidates = [s for s in (s1, s2) if s >= 0.0]
|
|
|
|
if not s_candidates:
|
|
return None, None
|
|
|
|
s_hit = min(s_candidates)
|
|
if s_hit > L_m:
|
|
return None, None
|
|
|
|
v_req_ms = v_b + r * s_hit
|
|
return float(s_hit), (v_req_ms * CV.MS_TO_KPH)
|
|
|
|
def _dfs(self, seg_id, total_dist, visited, current_speed_ms, best_result, path):
|
|
if seg_id in visited or seg_id not in path:
|
|
return
|
|
visited.add(seg_id)
|
|
|
|
seg = self.predicative_segments.get(seg_id)
|
|
if not seg:
|
|
return
|
|
|
|
candidates = []
|
|
|
|
length = seg.get("Length", NOT_SET)
|
|
|
|
# Speed-Limit
|
|
if self.predicative_speed_limit:
|
|
sl = seg.get("Speed", NOT_SET)
|
|
if sl != NOT_SET:
|
|
qual_ok = seg.get("QualityFlag", False)
|
|
candidates.append((sl, PSD_TYPE_SPEED_LIMIT, 0.0, qual_ok))
|
|
|
|
# Curve-Speed
|
|
if self.predicative_curve and self._speed_limit_curve_allowed(seg):
|
|
cs_begin = seg.get("Curve_Speed_Begin", NOT_SET)
|
|
cs_end = seg.get("Curve_Speed_End", NOT_SET)
|
|
|
|
if cs_begin != NOT_SET:
|
|
candidates.append((cs_begin, PSD_TYPE_CURV_SPEED, 0.0, True))
|
|
|
|
if cs_end != NOT_SET and length > 0:
|
|
candidates.append((cs_end, PSD_TYPE_CURV_SPEED, length, True))
|
|
|
|
# Curve-Speed Profile
|
|
if cs_begin != NOT_SET and cs_end != NOT_SET and length > 0:
|
|
s_hit, v_req_kmh = self._hit_linear_profile(v0_ms=current_speed_ms, a=DECELERATION_PREDICATIVE,
|
|
total_dist_m=total_dist, L_m=length, v_b_kmh=cs_begin, v_e_kmh=cs_end)
|
|
if s_hit is not None and v_req_kmh is not None:
|
|
candidates.append((v_req_kmh, PSD_TYPE_CURV_SPEED, s_hit, True))
|
|
|
|
# check candidates
|
|
for cand_speed_kmh, cand_type, activation_offset, qual_ok in candidates:
|
|
if cand_speed_kmh == NOT_SET:
|
|
continue
|
|
|
|
if not qual_ok:
|
|
continue
|
|
|
|
v_target_ms = cand_speed_kmh * CV.KPH_TO_MS
|
|
if not (v_target_ms < current_speed_ms and (cand_speed_kmh < self.v_limit_output_last or self.v_limit_output_last == NOT_SET)):
|
|
continue
|
|
|
|
brake_dist = (current_speed_ms**2 - v_target_ms**2) / (2 * DECELERATION_PREDICATIVE)
|
|
if brake_dist <= 0:
|
|
continue
|
|
|
|
dist_to_activation = total_dist + activation_offset
|
|
if dist_to_activation <= brake_dist:
|
|
# choose lowest limit or shorter length when equal
|
|
better = False
|
|
if cand_speed_kmh < best_result["limit"]:
|
|
better = True
|
|
elif cand_speed_kmh == best_result["limit"] and dist_to_activation < best_result["dist"]:
|
|
better = True
|
|
|
|
if better:
|
|
best_result["limit"] = cand_speed_kmh
|
|
best_result["type"] = cand_type
|
|
best_result["dist"] = dist_to_activation # represents distance to limit
|
|
best_result["length"] = length - activation_offset # represents remaining distance with limit for curves
|
|
|
|
children = [sid for sid, s in self.predicative_segments.items() if s.get("ID_Prev") == seg_id]
|
|
if len(children) > 1:
|
|
return # Split detected, can not decide unique limit on current path
|
|
|
|
for next_id in children:
|
|
if seg_id == self.current_predicative_segment.get("ID"):
|
|
next_length = self.current_predicative_segment.get("Length", 0)
|
|
else:
|
|
next_length = seg.get("Length", 0)
|
|
|
|
self._dfs(next_id, total_dist + next_length, visited.copy(), current_speed_ms, best_result, path)
|
|
|
|
def _build_path_psd(self, start_seg_id):
|
|
path = []
|
|
current_id = start_seg_id
|
|
visited = set()
|
|
|
|
segment_children_map = {}
|
|
for sid, seg in self.predicative_segments.items():
|
|
parent = seg.get("ID_Prev")
|
|
if parent not in (None, NOT_SET):
|
|
segment_children_map.setdefault(parent, []).append(sid)
|
|
|
|
while current_id != NOT_SET:
|
|
if current_id in visited:
|
|
break
|
|
visited.add(current_id)
|
|
path.append(current_id)
|
|
children = segment_children_map.get(current_id, [])
|
|
if len(children) != 1:
|
|
break
|
|
current_id = children[0]
|
|
|
|
return path
|
|
|
|
def _get_speed_limit_psd_next(self, current_speed_ms):
|
|
current_id = self.current_predicative_segment.get("ID")
|
|
self.v_limit_psd_next = NOT_SET
|
|
|
|
if current_id == NOT_SET:
|
|
return
|
|
|
|
path = self._build_path_psd(current_id)
|
|
|
|
if len(path) <= 1:
|
|
return
|
|
|
|
best_result = {"limit": float('inf'), "type": NOT_SET, "dist": float('inf'), "length": float('inf')}
|
|
self._dfs(current_id, 0, set(), current_speed_ms, best_result, path)
|
|
|
|
now = time.time()
|
|
if best_result["limit"] != float('inf'):
|
|
self.v_limit_psd_next = best_result["limit"]
|
|
self.v_limit_psd_next_type = best_result["type"]
|
|
self.v_limit_psd_next_last = best_result["limit"]
|
|
self.v_limit_psd_next_last_timestamp = now
|
|
self.v_limit_psd_next_decay_time = math.sqrt(2 * best_result["dist"] / DECELERATION_PREDICATIVE)
|
|
if self.v_limit_psd_next_type == PSD_TYPE_CURV_SPEED:
|
|
self.v_limit_psd_next_decay_time += max((best_result["length"] / (self.v_limit_psd_next * CV.KPH_TO_MS)), PSD_CURV_SPEED_DECAY)
|
|
else:
|
|
if now - self.v_limit_psd_next_last_timestamp <= self.v_limit_psd_next_decay_time and self.v_limit_output_last > self.v_limit_psd_next_last and not self.v_limit_changed:
|
|
self.v_limit_psd_next = self.v_limit_psd_next_last
|
|
else:
|
|
self.v_limit_psd_next_last = NOT_SET
|
|
self.v_limit_psd_next_decay_time = NOT_SET
|
|
self.v_limit_psd_next_type = NOT_SET
|
|
|
|
def _get_speed_limit_psd(self):
|
|
seg_id = self.current_predicative_segment.get("ID")
|
|
if seg_id == NOT_SET:
|
|
self.v_limit_psd = NOT_SET
|
|
return
|
|
|
|
seg = self.predicative_segments.get(seg_id)
|
|
if seg and seg.get("Speed") != NOT_SET:
|
|
self.v_limit_psd = seg.get("Speed")
|
|
|
|
def _get_street_type(self, strassenkategorie, bebauung):
|
|
street_type = NOT_SET
|
|
|
|
if strassenkategorie == 1: # base type: urban
|
|
street_type = STREET_TYPE_URBAN
|
|
|
|
elif strassenkategorie in (2, 3, 4): # base type: non urban
|
|
if bebauung == 1:
|
|
street_type = STREET_TYPE_URBAN
|
|
else:
|
|
street_type = STREET_TYPE_NONURBAN
|
|
|
|
elif strassenkategorie == 5: # base type: highway
|
|
street_type = STREET_TYPE_HIGHWAY
|
|
|
|
return street_type
|
|
|
|
def _receive_speed_limit_psd_legal(self, psd_06):
|
|
if psd_06["PSD_06_Mux"] == 2:
|
|
if psd_06["PSD_Ges_Typ"] == 2:
|
|
street_type = self.current_predicative_segment["StreetType"]
|
|
if ((psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_URBAN and street_type == STREET_TYPE_URBAN) or
|
|
(psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_NONURBAN and street_type == STREET_TYPE_NONURBAN) or
|
|
(psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_HIGHWAY and street_type == STREET_TYPE_HIGHWAY)):
|
|
raw_speed = psd_06["PSD_Ges_Geschwindigkeit"]
|
|
self.v_limit_psd_legal = self._convert_raw_speed_psd(raw_speed, street_type)
|