1329 lines
50 KiB
Python
1329 lines
50 KiB
Python
import fcntl
|
||
import json
|
||
import math
|
||
import os
|
||
import socket
|
||
import struct
|
||
import subprocess
|
||
import threading
|
||
import time
|
||
import numpy as np
|
||
from datetime import datetime
|
||
|
||
from ftplib import FTP
|
||
from cereal import log
|
||
import cereal.messaging as messaging
|
||
from openpilot.common.realtime import Ratekeeper
|
||
from openpilot.common.params import Params
|
||
from openpilot.common.filter_simple import MyMovingAverage
|
||
from openpilot.system.hardware import PC, TICI
|
||
from openpilot.selfdrive.navd.helpers import Coordinate
|
||
from opendbc.car.common.conversions import Conversions as CV
|
||
from openpilot.common.gps import get_gps_location_service
|
||
|
||
nav_type_mapping = {
|
||
12: ("turn", "left", 1),
|
||
16: ("turn", "sharp left", 1),
|
||
1000: ("turn", "slight left", 1),
|
||
1001: ("turn", "slight right", 2),
|
||
1002: ("fork", "slight left", 3),
|
||
1003: ("fork", "slight right", 4),
|
||
1006: ("off ramp", "left", 3),
|
||
1007: ("off ramp", "right", 4),
|
||
13: ("turn", "right", 2),
|
||
19: ("turn", "sharp right", 2),
|
||
102: ("off ramp", "slight left", 3),
|
||
105: ("off ramp", "slight left", 3),
|
||
112: ("off ramp", "slight left", 3),
|
||
115: ("off ramp", "slight left", 3),
|
||
101: ("off ramp", "slight right", 4),
|
||
104: ("off ramp", "slight right", 4),
|
||
111: ("off ramp", "slight right", 4),
|
||
114: ("off ramp", "slight right", 4),
|
||
7: ("fork", "left", 3),
|
||
44: ("fork", "left", 3),
|
||
17: ("fork", "left", 3),
|
||
75: ("fork", "left", 3),
|
||
76: ("fork", "left", 3),
|
||
118: ("fork", "left", 3),
|
||
6: ("fork", "right", 4),
|
||
43: ("fork", "right", 4),
|
||
73: ("fork", "right", 4),
|
||
74: ("fork", "right", 4),
|
||
123: ("fork", "right", 4),
|
||
124: ("fork", "right", 4),
|
||
117: ("fork", "right", 4),
|
||
131: ("rotary", "slight right", 5),
|
||
132: ("rotary", "slight right", 5),
|
||
140: ("rotary", "slight left", 5),
|
||
141: ("rotary", "slight left", 5),
|
||
133: ("rotary", "right", 5),
|
||
134: ("rotary", "sharp right", 5),
|
||
135: ("rotary", "sharp right", 5),
|
||
136: ("rotary", "sharp left", 5),
|
||
137: ("rotary", "sharp left", 5),
|
||
138: ("rotary", "sharp left", 5),
|
||
139: ("rotary", "left", 5),
|
||
142: ("rotary", "straight", 5),
|
||
14: ("turn", "uturn", 5),
|
||
201: ("arrive", "straight", 5),
|
||
51: ("notification", "straight", None),
|
||
52: ("notification", "straight", None),
|
||
53: ("notification", "straight", None),
|
||
54: ("notification", "straight", None),
|
||
55: ("notification", "straight", None),
|
||
153: ("", "", 6), #TG
|
||
154: ("", "", 6), #TG
|
||
249: ("", "", 6) #TG
|
||
}
|
||
|
||
import collections
|
||
class CarrotServ:
|
||
def __init__(self):
|
||
self.params = Params()
|
||
self.params_memory = Params("/dev/shm/params")
|
||
|
||
self.nRoadLimitSpeed = 30
|
||
self.nRoadLimitSpeed_last = 30
|
||
self.nRoadLimitSpeed_counter = 0
|
||
|
||
self.active_carrot = 0 ## 1: CarrotMan Active, 2: sdi active , 3: speed decel active, 4: section active, 5: bump active, 6: speed limit active
|
||
self.active_count = 0
|
||
self.active_sdi_count = 0
|
||
self.active_sdi_count_max = 200 # 20 sec
|
||
|
||
self.active_kisa_count = 0
|
||
|
||
self.nSdiType = -1
|
||
self.nSdiSpeedLimit = 0
|
||
self.nSdiSection = 0
|
||
self.nSdiDist = 0
|
||
self.nSdiBlockType = -1
|
||
self.nSdiBlockSpeed = 0
|
||
self.nSdiBlockDist = 0
|
||
|
||
self.nTBTDist = 0
|
||
self.nTBTTurnType = -1
|
||
self.szTBTMainText = ""
|
||
self.szNearDirName = ""
|
||
self.szFarDirName = ""
|
||
self.nTBTNextRoadWidth = 0
|
||
|
||
self.nTBTDistNext = 0
|
||
self.nTBTTurnTypeNext = -1
|
||
self.szTBTMainTextNext = ""
|
||
|
||
self.nGoPosDist = 0
|
||
self.nGoPosTime = 0
|
||
self.szPosRoadName = ""
|
||
self.nSdiPlusType = -1
|
||
self.nSdiPlusSpeedLimit = 0
|
||
self.nSdiPlusDist = 0
|
||
self.nSdiPlusBlockType = -1
|
||
self.nSdiPlusBlockSpeed = 0
|
||
self.nSdiPlusBlockDist = 0
|
||
|
||
self.goalPosX = 0.0
|
||
self.goalPosY = 0.0
|
||
self.szGoalName = ""
|
||
self.vpPosPointLatNavi = 0.0
|
||
self.vpPosPointLonNavi = 0.0
|
||
self.vpPosPointLat = 0.0
|
||
self.vpPosPointLon = 0.0
|
||
self.roadcate = 8
|
||
|
||
self.nPosSpeed = 0.0
|
||
self.nPosAngle = 0.0
|
||
self.nPosAnglePhone = 0.0
|
||
|
||
self.diff_angle_count = 0
|
||
self.last_calculate_gps_time = 0
|
||
self.last_update_gps_time = 0
|
||
self.last_update_gps_time_phone = 0
|
||
self.last_update_gps_time_navi = 0
|
||
self.bearing_offset = 0.0
|
||
self.bearing_measured = 0.0
|
||
self.bearing = 0.0
|
||
self.gps_valid = False
|
||
|
||
self.phone_gps_accuracy = 0.0
|
||
self.gps_accuracy_device = 0.0
|
||
self.phone_latitude = 0.0
|
||
self.phone_longitude = 0.0
|
||
self.phone_gps_frame = 0
|
||
|
||
self.totalDistance = 0
|
||
self.xSpdLimit = 0
|
||
self.xSpdDist = 0
|
||
self.xSpdType = -1
|
||
|
||
self.xTurnInfo = -1
|
||
self.xDistToTurn = 0
|
||
self.xTurnInfoNext = -1
|
||
self.xDistToTurnNext = 0
|
||
|
||
self.navType, self.navModifier = "invalid", ""
|
||
self.navTypeNext, self.navModifierNext = "invalid", ""
|
||
|
||
self.carrotIndex = 0
|
||
self.carrotCmdIndex = 0
|
||
self.carrotCmd = ""
|
||
self.carrotArg = ""
|
||
self.carrotCmdIndex_last = 0
|
||
|
||
self.traffic_light_q = collections.deque(maxlen=int(2.0/0.1)) # 2 secnods
|
||
self.traffic_light_count = -1
|
||
self.traffic_state = 0
|
||
|
||
self.left_spd_sec = 0
|
||
self.left_tbt_sec = 0
|
||
self.left_sec = 100
|
||
self.max_left_sec = 100
|
||
self.carrot_left_sec = 100
|
||
self.sdi_inform = False
|
||
|
||
|
||
self.atc_paused = False
|
||
self.atc_activate_count = 0
|
||
self.gas_override_speed = 0
|
||
self.gas_pressed_state = False
|
||
self.source_last = "none"
|
||
|
||
self.debugText = ""
|
||
|
||
# 默认语言，稍后在 update_params 中从 Params 读取覆盖，
|
||
# 规则：main_ko -> 韩语；main_zh-CHS -> 中文；其他 -> 英文
|
||
self.lang = "en"
|
||
|
||
self.update_params()
|
||
|
||
def update_params(self):
|
||
self.autoNaviSpeedBumpSpeed = float(self.params.get_int("AutoNaviSpeedBumpSpeed"))
|
||
self.autoNaviSpeedBumpTime = float(self.params.get_int("AutoNaviSpeedBumpTime"))
|
||
self.autoNaviSpeedCtrlEnd = float(self.params.get_int("AutoNaviSpeedCtrlEnd"))
|
||
self.autoNaviSpeedCtrlMode = self.params.get_int("AutoNaviSpeedCtrlMode")
|
||
self.autoNaviSpeedSafetyFactor = float(self.params.get_int("AutoNaviSpeedSafetyFactor")) * 0.01
|
||
self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
|
||
self.autoNaviCountDownMode = self.params.get_int("AutoNaviCountDownMode")
|
||
self.turnSpeedControlMode= self.params.get_int("TurnSpeedControlMode")
|
||
self.mapTurnSpeedFactor= self.params.get_float("MapTurnSpeedFactor") * 0.01
|
||
|
||
self.autoTurnControlSpeedTurn = self.params.get_int("AutoTurnControlSpeedTurn")
|
||
self.autoTurnMapChange = self.params.get_int("AutoTurnMapChange")
|
||
self.autoTurnControl = self.params.get_int("AutoTurnControl")
|
||
self.autoTurnControlTurnEnd = self.params.get_int("AutoTurnControlTurnEnd")
|
||
#self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
|
||
self.autoCurveSpeedLowerLimit = int(self.params.get("AutoCurveSpeedLowerLimit"))
|
||
self.is_metric = self.params.get_bool("IsMetric")
|
||
self.autoRoadSpeedLimitOffset = self.params.get_int("AutoRoadSpeedLimitOffset")
|
||
|
||
# 读取语言设置：优先使用 LanguageSetting，与 UI 保持一致；回退读取可能存在的 "lang"
|
||
try:
|
||
lang_val = self.params.get('LanguageSetting', encoding='utf8') or self.params.get('lang', encoding='utf8')
|
||
except Exception:
|
||
lang_val = None
|
||
if isinstance(lang_val, bytes):
|
||
try:
|
||
lang_val = lang_val.decode('utf8')
|
||
except Exception:
|
||
lang_val = None
|
||
if lang_val == "main_ko":
|
||
self.lang = "ko"
|
||
elif lang_val == "main_zh-CHS":
|
||
self.lang = "zh"
|
||
else:
|
||
self.lang = "en"
|
||
|
||
|
||
def _update_cmd(self):
|
||
if self.carrotCmdIndex != self.carrotCmdIndex_last:
|
||
self.carrotCmdIndex_last = self.carrotCmdIndex
|
||
command_handlers = {
|
||
"DETECT": self._handle_detect_command,
|
||
}
|
||
|
||
handler = command_handlers.get(self.carrotCmd)
|
||
if handler:
|
||
handler(self.carrotArg)
|
||
|
||
self.traffic_light_q.append((-1, -1, "none", 0.0))
|
||
self.traffic_light_count -= 1
|
||
if self.traffic_light_count < 0:
|
||
self.traffic_light_count = -1
|
||
self.traffic_state = 0
|
||
|
||
def _handle_detect_command(self, xArg):
|
||
elements = [e.strip() for e in xArg.split(',')]
|
||
if len(elements) >= 4:
|
||
try:
|
||
state = elements[0]
|
||
value1 = float(elements[1])
|
||
value2 = float(elements[2])
|
||
value3 = float(elements[3])
|
||
self.traffic_light(value1, value2, state, value3)
|
||
self.traffic_light_count = int(0.5 / 0.1)
|
||
except ValueError:
|
||
pass
|
||
|
||
def traffic_light(self, x, y, color, cnf):
|
||
traffic_red = 0
|
||
traffic_green = 0
|
||
traffic_left = 0
|
||
traffic_red_trig = 0
|
||
traffic_green_trig = 0
|
||
traffic_left_trig = 0
|
||
for pdata in self.traffic_light_q:
|
||
px, py, pcolor,pcnf = pdata
|
||
if abs(x - px) < 0.2 and abs(y - py) < 0.2:
|
||
if pcolor in ["Green Light", "Left turn"]:
|
||
if color in ["Red Light", "Yellow Light"]:
|
||
traffic_red_trig += cnf
|
||
traffic_red += cnf
|
||
elif color in ["Green Light", "Left turn"]:
|
||
traffic_green += cnf
|
||
elif pcolor in ["Red Light", "Yellow Light"]:
|
||
if color in ["Green Light"]: #, "Left turn"]:
|
||
traffic_green_trig += cnf
|
||
traffic_green += cnf
|
||
elif color in ["Left turn"]:
|
||
traffic_left_trig += cnf
|
||
traffic_left += cnf
|
||
elif color in ["Red Light", "Yellow Light"]:
|
||
traffic_red += cnf
|
||
|
||
#print(self.traffic_light_q)
|
||
if traffic_red_trig > 0:
|
||
self.traffic_state = 1
|
||
#self._add_log("Red light triggered")
|
||
#print("Red light triggered")
|
||
elif traffic_green_trig > 0 and traffic_green > traffic_red: #주변에 red light의 cnf보다 더 크면 출발... 감지오류로 출발하는경우가 생김.
|
||
self.traffic_state = 2
|
||
#self._add_log("Green light triggered")
|
||
#print("Green light triggered")
|
||
elif traffic_left_trig > 0:
|
||
self.traffic_state = 3
|
||
elif traffic_red > 0:
|
||
self.traffic_state = 1
|
||
#self._add_log("Red light continued")
|
||
#print("Red light continued")
|
||
elif traffic_green > 0:
|
||
self.traffic_state = 2
|
||
#self._add_log("Green light continued")
|
||
#print("Green light continued")
|
||
else:
|
||
self.traffic_state = 0
|
||
#print("TrafficLight none")
|
||
|
||
self.traffic_light_q.append((x,y,color,cnf))
|
||
|
||
|
||
def calculate_current_speed(self, left_dist, safe_speed_kph, safe_time, safe_decel_rate):
|
||
safe_speed = safe_speed_kph / 3.6
|
||
safe_dist = safe_speed * safe_time
|
||
decel_dist = left_dist - safe_dist
|
||
|
||
if decel_dist <= 0:
|
||
return safe_speed_kph
|
||
|
||
# v_i^2 = v_f^2 + 2ad
|
||
temp = safe_speed**2 + 2 * safe_decel_rate * decel_dist # 공식에서 감속 적용
|
||
|
||
if temp < 0:
|
||
speed_mps = safe_speed
|
||
else:
|
||
speed_mps = math.sqrt(temp)
|
||
return max(safe_speed_kph, min(250, speed_mps * 3.6))
|
||
|
||
def _update_tbt(self):
|
||
#xTurnInfo : 1: left turn, 2: right turn, 3: left lane change, 4: right lane change, 5: rotary, 6: tg, 7: arrive or uturn
|
||
turn_type_mapping = {
|
||
12: ("turn", "left", 1),
|
||
16: ("turn", "sharp left", 1),
|
||
13: ("turn", "right", 2),
|
||
19: ("turn", "sharp right", 2),
|
||
102: ("off ramp", "slight left", 3),
|
||
105: ("off ramp", "slight left", 3),
|
||
112: ("off ramp", "slight left", 3),
|
||
115: ("off ramp", "slight left", 3),
|
||
101: ("off ramp", "slight right", 4),
|
||
104: ("off ramp", "slight right", 4),
|
||
111: ("off ramp", "slight right", 4),
|
||
114: ("off ramp", "slight right", 4),
|
||
7: ("fork", "left", 3),
|
||
44: ("fork", "left", 3),
|
||
17: ("fork", "left", 3),
|
||
75: ("fork", "left", 3),
|
||
76: ("fork", "left", 3),
|
||
118: ("fork", "left", 3),
|
||
6: ("fork", "right", 4),
|
||
43: ("fork", "right", 4),
|
||
73: ("fork", "right", 4),
|
||
74: ("fork", "right", 4),
|
||
123: ("fork", "right", 4),
|
||
124: ("fork", "right", 4),
|
||
117: ("fork", "right", 4),
|
||
131: ("rotary", "slight right", 5),
|
||
132: ("rotary", "slight right", 5),
|
||
140: ("rotary", "slight left", 5),
|
||
141: ("rotary", "slight left", 5),
|
||
133: ("rotary", "right", 5),
|
||
134: ("rotary", "sharp right", 5),
|
||
135: ("rotary", "sharp right", 5),
|
||
136: ("rotary", "sharp left", 5),
|
||
137: ("rotary", "sharp left", 5),
|
||
138: ("rotary", "sharp left", 5),
|
||
139: ("rotary", "left", 5),
|
||
142: ("rotary", "straight", 5),
|
||
14: ("turn", "uturn", 7),
|
||
201: ("arrive", "straight", 8),
|
||
51: ("notification", "straight", 0),
|
||
52: ("notification", "straight", 0),
|
||
53: ("notification", "straight", 0),
|
||
54: ("notification", "straight", 0),
|
||
55: ("notification", "straight", 0),
|
||
153: ("", "", 6), #TG
|
||
154: ("", "", 6), #TG
|
||
249: ("", "", 6) #TG
|
||
}
|
||
|
||
if self.nTBTTurnType in turn_type_mapping:
|
||
self.navType, self.navModifier, self.xTurnInfo = turn_type_mapping[self.nTBTTurnType]
|
||
else:
|
||
self.navType, self.navModifier, self.xTurnInfo = "invalid", "", -1
|
||
|
||
if self.nTBTTurnTypeNext in turn_type_mapping:
|
||
self.navTypeNext, self.navModifierNext, self.xTurnInfoNext = turn_type_mapping[self.nTBTTurnTypeNext]
|
||
else:
|
||
self.navTypeNext, self.navModifierNext, self.xTurnInfoNext = "invalid", "", -1
|
||
|
||
if self.nTBTDist > 0 and self.xTurnInfo > 0:
|
||
self.xDistToTurn = self.nTBTDist
|
||
if self.nTBTDistNext > 0 and self.xTurnInfoNext > 0:
|
||
self.xDistToTurnNext = self.nTBTDistNext + self.nTBTDist
|
||
|
||
def _get_sdi_descr(self, nSdiType):
|
||
# 多语言映射：ko（韩语，原始），zh（简体中文），en（英文）。
|
||
sdi_ko = {
|
||
0: "신호과속",
|
||
1: "과속 (고정식)",
|
||
2: "구간단속 시작",
|
||
3: "구간단속 끝",
|
||
4: "구간단속중",
|
||
5: "꼬리물기단속카메라",
|
||
6: "신호 단속",
|
||
7: "과속 (이동식)",
|
||
8: "고정식 과속위험 구간(박스형)",
|
||
9: "버스전용차로구간",
|
||
10: "가변 차로 단속",
|
||
11: "갓길 감시 지점",
|
||
12: "끼어들기 금지",
|
||
13: "교통정보 수집지점",
|
||
14: "방범용cctv",
|
||
15: "과적차량 위험구간",
|
||
16: "적재 불량 단속",
|
||
17: "주차단속 지점",
|
||
18: "일방통행도로",
|
||
19: "철길 건널목",
|
||
20: "어린이 보호구역(스쿨존 시작 구간)",
|
||
21: "어린이 보호구역(스쿨존 끝 구간)",
|
||
22: "과속방지턱",
|
||
23: "lpg충전소",
|
||
24: "터널 구간",
|
||
25: "휴게소",
|
||
26: "톨게이트",
|
||
27: "안개주의 지역",
|
||
28: "유해물질 지역",
|
||
29: "사고다발",
|
||
30: "급커브지역",
|
||
31: "급커브구간1",
|
||
32: "급경사구간",
|
||
33: "야생동물 교통사고 잦은 구간",
|
||
34: "우측시야불량지점",
|
||
35: "시야불량지점",
|
||
36: "좌측시야불량지점",
|
||
37: "신호위반다발구간",
|
||
38: "과속운행다발구간",
|
||
39: "교통혼잡지역",
|
||
40: "방향별차로선택지점",
|
||
41: "무단횡단사고다발지점",
|
||
42: "갓길 사고 다발 지점",
|
||
43: "과속 사발 다발 지점",
|
||
44: "졸음 사고 다발 지점",
|
||
45: "사고다발지점",
|
||
46: "보행자 사고다발지점",
|
||
47: "차량도난사고 상습발생지점",
|
||
48: "낙석주의지역",
|
||
49: "결빙주의지역",
|
||
50: "병목지점",
|
||
51: "합류 도로",
|
||
52: "추락주의지역",
|
||
53: "지하차도 구간",
|
||
54: "주택밀집지역(교통진정지역)",
|
||
55: "인터체인지",
|
||
56: "분기점",
|
||
57: "휴게소(lpg충전가능)",
|
||
58: "교량",
|
||
59: "제동장치사고다발지점",
|
||
60: "중앙선침범사고다발지점",
|
||
61: "통행위반사고다발지점",
|
||
62: "목적지 건너편 안내",
|
||
63: "졸음 쉼터 안내",
|
||
64: "노후경유차단속",
|
||
65: "터널내 차로변경단속",
|
||
66: "",
|
||
}
|
||
|
||
sdi_en = {
|
||
0: "Signal speed enforcement",
|
||
1: "Speed camera (fixed)",
|
||
2: "Section control start",
|
||
3: "Section control end",
|
||
4: "Under section control",
|
||
5: "Block-the-box camera",
|
||
6: "Signal violation enforcement",
|
||
7: "Speed camera (mobile)",
|
||
8: "Fixed speed camera zone (box)",
|
||
9: "Bus-only lane zone",
|
||
10: "Reversible/variable lane enforcement",
|
||
11: "Shoulder surveillance point",
|
||
12: "No cut-in",
|
||
13: "Traffic data collection point",
|
||
14: "Security CCTV",
|
||
15: "Overloaded vehicle risk zone",
|
||
16: "Improper loading enforcement",
|
||
17: "Parking enforcement point",
|
||
18: "One-way road",
|
||
19: "Railroad crossing",
|
||
20: "School zone start",
|
||
21: "School zone end",
|
||
22: "Speed bump",
|
||
23: "LPG station",
|
||
24: "Tunnel section",
|
||
25: "Rest area",
|
||
26: "Toll gate",
|
||
27: "Fog caution area",
|
||
28: "Hazardous materials area",
|
||
29: "Accident-prone section",
|
||
30: "Sharp curve area",
|
||
31: "Sharp curve section 1",
|
||
32: "Steep slope section",
|
||
33: "Wild animal crossing area",
|
||
34: "Poor visibility (right)",
|
||
35: "Poor visibility",
|
||
36: "Poor visibility (left)",
|
||
37: "Frequent signal violations",
|
||
38: "Frequent speeding",
|
||
39: "Traffic congestion area",
|
||
40: "Lane selection by direction",
|
||
41: "Frequent jaywalking accidents",
|
||
42: "Frequent shoulder accidents",
|
||
43: "Frequent speeding accidents",
|
||
44: "Frequent drowsy driving accidents",
|
||
45: "Accident-prone spot",
|
||
46: "Frequent pedestrian accidents",
|
||
47: "Frequent vehicle theft",
|
||
48: "Falling rock caution area",
|
||
49: "Icy road caution area",
|
||
50: "Bottleneck point",
|
||
51: "Merging road",
|
||
52: "Cliff/Drop caution area",
|
||
53: "Underpass section",
|
||
54: "Residential area (traffic calming)",
|
||
55: "Interchange",
|
||
56: "Junction",
|
||
57: "Rest area (LPG available)",
|
||
58: "Bridge",
|
||
59: "Frequent brake failure accidents",
|
||
60: "Center line invasion accidents",
|
||
61: "Violation-of-passage accidents",
|
||
62: "Destination on opposite side",
|
||
63: "Drowsy rest area",
|
||
64: "Old diesel control",
|
||
65: "Lane change enforcement in tunnel",
|
||
66: "",
|
||
}
|
||
|
||
sdi_zh = {
|
||
0: "信号测速/闯灯拍照",
|
||
1: "固定测速摄像头",
|
||
2: "区间测速开始",
|
||
3: "区间测速结束",
|
||
4: "区间测速中",
|
||
5: "路口压线摄像头",
|
||
6: "闯红灯拍照",
|
||
7: "流动测速摄像头",
|
||
8: "测速拍照",
|
||
9: "公交专用车道区间",
|
||
10: "可变/潮汐车道拍照",
|
||
11: "应急车道拍照",
|
||
12: "禁止加塞",
|
||
13: "交通信息采集点",
|
||
14: "治安监控",
|
||
15: "超载车辆风险区",
|
||
16: "装载不当拍照",
|
||
17: "违停拍照点",
|
||
18: "单行道",
|
||
19: "铁路道口",
|
||
20: "学校区域开始",
|
||
21: "学校区域结束",
|
||
22: "减速带",
|
||
23: "LPG加气站",
|
||
24: "隧道区间",
|
||
25: "服务区",
|
||
26: "ETC计费拍照",
|
||
27: "多雾路段",
|
||
28: "危险品区域",
|
||
29: "事故多发路段",
|
||
30: "急弯路段",
|
||
31: "急弯区段1",
|
||
32: "陡坡路段",
|
||
33: "野生动物出没路段",
|
||
34: "右侧视野不良点",
|
||
35: "视野不良点",
|
||
36: "左侧视野不良点",
|
||
37: "闯红灯多发",
|
||
38: "超速多发",
|
||
39: "交通拥堵区域",
|
||
40: "按方向选择车道点",
|
||
41: "行人乱穿马路多发处",
|
||
42: "应急车道事故多发",
|
||
43: "超速事故多发",
|
||
44: "疲劳驾驶事故多发",
|
||
45: "事故多发点",
|
||
46: "行人事故多发点",
|
||
47: "车辆盗窃多发点",
|
||
48: "落石危险路段",
|
||
49: "路面结冰危险",
|
||
50: "瓶颈路段",
|
||
51: "汇入道路",
|
||
52: "坠落危险路段",
|
||
53: "地下车道区间",
|
||
54: "居民区（交通缓和）",
|
||
55: "立交",
|
||
56: "分岔点",
|
||
57: "服务区（可加气）",
|
||
58: "桥梁",
|
||
59: "制动故障事故多发点",
|
||
60: "越线事故多发点",
|
||
61: "违法通行事故多发点",
|
||
62: "目的地在对面",
|
||
63: "瞌睡停车区",
|
||
64: "老旧柴油车管制",
|
||
65: "隧道内变道拍照",
|
||
66: "",
|
||
}
|
||
|
||
sdi_map = sdi_en
|
||
if self.lang == "ko":
|
||
sdi_map = sdi_ko
|
||
elif self.lang == "zh":
|
||
sdi_map = sdi_zh
|
||
|
||
return sdi_map.get(nSdiType, "")
|
||
|
||
def _update_sdi(self):
|
||
#sdiBlockType
|
||
# 1: startOSEPS: 구간단속시작
|
||
# 2: inOSEPS: 구간단속중
|
||
# 3: endOSEPS: 구간단속종료
|
||
# 0:감속안함,1:과속카메라,2:+사고방지턱,3:+이동식카메라
|
||
if self.nSdiType in [0,1,2,3,4,7,8, 75, 76] and self.nSdiSpeedLimit > 0 and self.autoNaviSpeedCtrlMode > 0:
|
||
self.xSpdLimit = self.nSdiSpeedLimit * self.autoNaviSpeedSafetyFactor
|
||
self.xSpdDist = self.nSdiDist
|
||
self.xSpdType = self.nSdiType
|
||
if self.nSdiBlockType in [2,3]:
|
||
self.xSpdDist = self.nSdiBlockDist
|
||
self.xSpdType = 4
|
||
elif self.nSdiType == 7 and self.autoNaviSpeedCtrlMode < 3: #이동식카메라
|
||
self.xSpdLimit = self.xSpdDist = 0
|
||
elif (self.nSdiPlusType == 22 or self.nSdiType == 22) and self.roadcate > 1 and self.autoNaviSpeedCtrlMode >= 2: # speed bump, roadcate:0,1: highway
|
||
self.xSpdLimit = self.autoNaviSpeedBumpSpeed
|
||
self.xSpdDist = self.nSdiPlusDist if self.nSdiPlusType == 22 else self.nSdiDist
|
||
self.xSpdType = 22
|
||
else:
|
||
self.xSpdLimit = 0
|
||
self.xSpdType = -1
|
||
self.xSpdDist = 0
|
||
|
||
def _update_gps(self, v_ego, sm, gps_service):
|
||
gps = sm[gps_service]
|
||
#print(f"location = {sm.valid[llk]}, {sm.updated[llk]}, {sm.recv_frame[llk]}, {sm.recv_time[llk]}")
|
||
if not sm.updated['carState'] or not sm.updated['carControl']: # or not sm.updated[llk]:
|
||
return self.nPosAngle
|
||
CS = sm['carState']
|
||
CC = sm['carControl']
|
||
self.gps_valid = sm.updated[gps_service] and gps.hasFix
|
||
|
||
now = time.monotonic()
|
||
gps_updated_phone = (now - self.last_update_gps_time_phone) < 3
|
||
gps_updated_navi = (now - self.last_update_gps_time_navi) < 3
|
||
|
||
bearing = self.nPosAngle
|
||
if gps_updated_phone:
|
||
self.bearing_offset = 0.0
|
||
elif self.gps_valid:
|
||
bearing = self.nPosAngle = gps.bearingDeg
|
||
if self.gps_valid:
|
||
self.bearing_offset = 0.0
|
||
elif self.active_carrot > 0:
|
||
bearing = self.nPosAnglePhone
|
||
self.bearing_offset = 0.0
|
||
|
||
#print(f"bearing = {bearing:.1f}, posA=={self.nPosAngle:.1f}, posP=={self.nPosAnglePhone:.1f}, offset={self.bearing_offset:.1f}, {gps_updated_phone}, {gps_updated_navi}")
|
||
gpsDelayTimeAdjust = 0.0
|
||
if gps_updated_navi:
|
||
gpsDelayTimeAdjust = 0 #1.0
|
||
|
||
external_gps_update_timedout = not (gps_updated_phone or gps_updated_navi)
|
||
#print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}")
|
||
if self.gps_valid and external_gps_update_timedout: # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
|
||
self.vpPosPointLatNavi = gps.latitude
|
||
self.vpPosPointLonNavi = gps.longitude
|
||
self.last_calculate_gps_time = now #sm.recv_time[llk]
|
||
elif gps_updated_navi: # carrot navi로부터 gps신호가 수신되는 경우..
|
||
if abs(self.bearing_measured - bearing) < 0.1:
|
||
self.diff_angle_count += 1
|
||
else:
|
||
self.diff_angle_count = 0
|
||
self.bearing_measured = bearing
|
||
|
||
if self.diff_angle_count > 5: # 조향각도변화가 거의 없을때만 업데이트
|
||
diff_angle = (self.nPosAngle - bearing) % 360
|
||
if diff_angle > 180:
|
||
diff_angle -= 360
|
||
self.bearing_offset = self.bearing_offset * 0.9 + diff_angle * 0.1
|
||
|
||
bearing_calculated = (bearing + self.bearing_offset) % 360
|
||
|
||
dt = now - self.last_calculate_gps_time
|
||
#print(f"dt = {dt:.1f}, {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}")
|
||
if dt > 5.0:
|
||
self.vpPosPointLat, self.vpPosPointLon = 0.0, 0.0
|
||
elif dt == 0:
|
||
self.vpPosPointLat, self.vpPosPointLon = self.vpPosPointLatNavi, self.vpPosPointLonNavi
|
||
else:
|
||
self.vpPosPointLat, self.vpPosPointLon = self.estimate_position(float(self.vpPosPointLatNavi), float(self.vpPosPointLonNavi), v_ego, bearing_calculated, dt + gpsDelayTimeAdjust)
|
||
|
||
#self.debugText = " {} {:.1f},{:.1f}={:.1f}+{:.1f}".format(self.active_sdi_count, self.nPosAngle, bearing_calculated, bearing, self.bearing_offset)
|
||
#print("nPosAngle = {:.1f},{:.1f} = {:.1f}+{:.1f}".format(self.nPosAngle, bearing_calculated, bearing, self.bearing_offset))
|
||
|
||
return float(bearing_calculated)
|
||
|
||
|
||
def estimate_position(self, lat, lon, speed, angle, dt):
|
||
R = 6371000
|
||
angle_rad = math.radians(angle)
|
||
delta_d = speed * dt
|
||
delta_lat = delta_d * math.cos(angle_rad) / R
|
||
new_lat = lat + math.degrees(delta_lat)
|
||
delta_lon = delta_d * math.sin(angle_rad) / (R * math.cos(math.radians(lat)))
|
||
new_lon = lon + math.degrees(delta_lon)
|
||
|
||
return new_lat, new_lon
|
||
|
||
def update_auto_turn(self, v_ego_kph, sm, x_turn_info, x_dist_to_turn, check_steer=False):
|
||
turn_speed = self.autoTurnControlSpeedTurn
|
||
fork_speed = self.nRoadLimitSpeed
|
||
stop_speed = 1
|
||
turn_dist_for_speed = self.autoTurnControlTurnEnd * turn_speed / 3.6 # 5
|
||
fork_dist_for_speed = self.autoTurnControlTurnEnd * fork_speed / 3.6 # 5
|
||
stop_dist_for_speed = 5
|
||
start_fork_dist = np.interp(self.nRoadLimitSpeed, [30, 50, 100], [160, 200, 350])
|
||
start_turn_dist = np.interp(self.nTBTNextRoadWidth, [5, 10], [43, 60])
|
||
turn_info_mapping = {
|
||
1: {"type": "turn left", "speed": turn_speed, "dist": turn_dist_for_speed, "start": start_fork_dist},
|
||
2: {"type": "turn right", "speed": turn_speed, "dist": turn_dist_for_speed, "start": start_fork_dist},
|
||
5: {"type": "straight", "speed": turn_speed, "dist": turn_dist_for_speed, "start": start_turn_dist},
|
||
3: {"type": "fork left", "speed": fork_speed, "dist": fork_dist_for_speed, "start": start_fork_dist},
|
||
4: {"type": "fork right", "speed": fork_speed, "dist": fork_dist_for_speed, "start": start_fork_dist},
|
||
6: {"type": "straight", "speed": fork_speed, "dist": fork_dist_for_speed, "start": start_fork_dist},
|
||
7: {"type": "straight", "speed": stop_speed, "dist": stop_dist_for_speed, "start": 1000},
|
||
8: {"type": "straight", "speed": stop_speed, "dist": stop_dist_for_speed, "start": 1000},
|
||
}
|
||
|
||
default_mapping = {"type": "none", "speed": 0, "dist": 0, "start": 1000}
|
||
|
||
mapping = turn_info_mapping.get(x_turn_info, default_mapping)
|
||
|
||
atc_type = mapping["type"]
|
||
atc_speed = mapping["speed"]
|
||
atc_dist = mapping["dist"]
|
||
atc_start_dist = mapping["start"]
|
||
|
||
if x_dist_to_turn > atc_start_dist:
|
||
atc_type += " prepare"
|
||
if check_steer:
|
||
self.atc_activate_count = min(0, self.atc_activate_count - 1)
|
||
else:
|
||
if check_steer:
|
||
self.atc_activate_count = max(0, self.atc_activate_count + 1)
|
||
if atc_type in ["turn left", "turn right"] and x_dist_to_turn > start_turn_dist:
|
||
atc_type = "atc left" if atc_type == "turn left" else "atc right"
|
||
|
||
if self.autoTurnMapChange > 0 and check_steer:
|
||
#print(f"x_dist_to_turn: {x_dist_to_turn}, atc_start_dist: {atc_start_dist}")
|
||
#print(f"atc_activate_count: {self.atc_activate_count}")
|
||
if self.atc_activate_count == 2:
|
||
self.carrotCmdIndex += 100
|
||
self.carrotCmd = "DISPLAY";
|
||
self.carrotArg = "MAP";
|
||
elif self.atc_activate_count == -50:
|
||
self.carrotCmdIndex += 100
|
||
self.carrotCmd = "DISPLAY";
|
||
self.carrotArg = "ROAD";
|
||
|
||
if check_steer:
|
||
if 0 <= x_dist_to_turn < atc_start_dist and atc_type in ["fork left", "fork right"]:
|
||
if not self.atc_paused:
|
||
steering_pressed = sm["carState"].steeringPressed
|
||
steering_torque = sm["carState"].steeringTorque
|
||
if steering_pressed and steering_torque < 0 and atc_type in ["fork left", "atc left"]:
|
||
self.atc_paused = True
|
||
elif steering_pressed and steering_torque > 0 and atc_type in ["fork right", "atc right"]:
|
||
self.atc_paused = True
|
||
else:
|
||
self.atc_paused = False
|
||
|
||
if self.atc_paused:
|
||
atc_type += " canceled"
|
||
|
||
atc_desired = 250
|
||
if atc_speed > 0 and x_dist_to_turn > 0:
|
||
decel = self.autoNaviSpeedDecelRate
|
||
safe_sec = 2.0
|
||
atc_desired = min(atc_desired, self.calculate_current_speed(x_dist_to_turn - atc_dist, atc_speed, safe_sec, decel))
|
||
|
||
|
||
return atc_desired, atc_type, atc_speed, atc_dist
|
||
|
||
def update_nav_instruction(self, sm):
|
||
if sm.alive['navInstruction'] and sm.valid['navInstruction']:
|
||
msg_nav = sm['navInstruction']
|
||
|
||
self.nGoPosDist = int(msg_nav.distanceRemaining)
|
||
self.nGoPosTime = int(msg_nav.timeRemaining)
|
||
if self.active_kisa_count <= 0 and msg_nav.speedLimit > 0:
|
||
self.nRoadLimitSpeed = max(30, round(msg_nav.speedLimit * CV.MS_TO_KPH))
|
||
self.xDistToTurn = int(msg_nav.maneuverDistance)
|
||
self.szTBTMainText = msg_nav.maneuverPrimaryText
|
||
self.xTurnInfo = -1
|
||
for key, value in nav_type_mapping.items():
|
||
if value[0] == msg_nav.maneuverType and value[1] == msg_nav.maneuverModifier:
|
||
self.xTurnInfo = value[2]
|
||
break
|
||
|
||
self.debugText = f"{self.nRoadLimitSpeed if self.is_metric else self.nRoadLimitSpeed * CV.KPH_TO_MPH:.0f},{msg_nav.maneuverType},{msg_nav.maneuverModifier} "
|
||
#print(msg_nav)
|
||
#print(f"navInstruction: {self.xTurnInfo}, {self.xDistToTurn}, {self.szTBTMainText}")
|
||
|
||
def update_kisa(self, data):
|
||
self.active_kisa_count = 100
|
||
if "kisawazecurrentspd" in data:
|
||
pass
|
||
if "kisawazeroadspdlimit" in data:
|
||
road_limit_speed = data["kisawazeroadspdlimit"]
|
||
if road_limit_speed > 0:
|
||
print(f"kisawazeroadspdlimit: {road_limit_speed} km/h")
|
||
if not self.is_metric:
|
||
road_limit_speed *= CV.MPH_TO_KPH
|
||
self.nRoadLimitSpeed = road_limit_speed
|
||
if "kisawazealert" in data:
|
||
pass
|
||
if "kisawazeendalert" in data:
|
||
pass
|
||
if "kisawazeroadname" in data:
|
||
print(f"kisawazeroadname: {data['kisawazeroadname']}")
|
||
self.szPosRoadName = data["kisawazeroadname"]
|
||
if "kisawazereportid" in data and "kisawazealertdist" in data:
|
||
id_str = data["kisawazereportid"]
|
||
dist_str = data["kisawazealertdist"].lower()
|
||
import re
|
||
match = re.search(r'(\d+)', dist_str)
|
||
distance = int(match.group(1)) if match else 0
|
||
if not self.is_metric:
|
||
distance = int(distance * 0.3048)
|
||
print(f"{id_str}: {distance} m")
|
||
xSpdType = -1
|
||
if 'camera' in id_str:
|
||
xSpdType = 101 # 101: waze speed cam, 100: police
|
||
elif 'police' in id_str:
|
||
xSpdType = 100
|
||
|
||
if xSpdType >= 0:
|
||
offset = 5 if self.is_metric else 5 * CV.MPH_TO_KPH
|
||
self.xSpdLimit = self.nRoadLimitSpeed + offset
|
||
|
||
self.xSpdDist = distance
|
||
self.xSpdType =xSpdType
|
||
|
||
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed, gps_service):
|
||
|
||
self.debugText = ""
|
||
self.update_params()
|
||
if sm.alive['carState'] and sm.alive['selfdriveState']:
|
||
CS = sm['carState']
|
||
v_ego = CS.vEgo
|
||
v_ego_kph = v_ego * 3.6
|
||
distanceTraveled = sm['selfdriveState'].distanceTraveled
|
||
delta_dist = distanceTraveled - self.totalDistance
|
||
self.totalDistance = distanceTraveled
|
||
if CS.speedLimit > 0 and self.active_carrot <= 1:
|
||
self.nRoadLimitSpeed = CS.speedLimit
|
||
else:
|
||
v_ego = v_ego_kph = 0
|
||
delta_dist = 0
|
||
CS = None
|
||
|
||
road_speed_limit_changed = True if self.nRoadLimitSpeed != self.nRoadLimitSpeed_last else False
|
||
self.nRoadLimitSpeed_last = self.nRoadLimitSpeed
|
||
#self.bearing = self.nPosAngle #self._update_gps(v_ego, sm)
|
||
self.bearing = self._update_gps(v_ego, sm, gps_service)
|
||
|
||
self.xSpdDist = max(self.xSpdDist - delta_dist, -1000)
|
||
self.xDistToTurn = self.xDistToTurn - delta_dist
|
||
self.xDistToTurnNext = self.xDistToTurnNext - delta_dist
|
||
self.active_count = max(self.active_count - 1, 0)
|
||
self.active_sdi_count = max(self.active_sdi_count - 1, 0)
|
||
self.active_kisa_count = max(self.active_kisa_count - 1, 0)
|
||
if self.active_kisa_count > 0:
|
||
self.active_carrot = 2
|
||
|
||
elif self.active_count > 0:
|
||
self.active_carrot = 2 if self.active_sdi_count > 0 else 1
|
||
else:
|
||
self.active_carrot = 0
|
||
|
||
if self.autoRoadSpeedLimitOffset >= 0 and self.active_carrot>=2:
|
||
if self.nRoadLimitSpeed >= 30:
|
||
road_speed_limit_offset = self.autoRoadSpeedLimitOffset
|
||
if not self.is_metric:
|
||
road_speed_limit_offset *= CV.KPH_TO_MPH
|
||
limit_speed = self.nRoadLimitSpeed + road_speed_limit_offset
|
||
else:
|
||
limit_speed = 200
|
||
|
||
if self.active_carrot <= 1:
|
||
self.xSpdType = self.navType = self.xTurnInfo = self.xTurnInfoNext = -1
|
||
self.nSdiType = self.nSdiBlockType = self.nSdiPlusBlockType = -1
|
||
self.nTBTTurnType = self.nTBTTurnTypeNext = -1
|
||
self.roadcate = 8
|
||
self.nGoPosDist = 0
|
||
if self.active_carrot <= 1 or self.active_kisa_count > 0:
|
||
self.update_nav_instruction(sm)
|
||
|
||
if self.xSpdType < 0 or (self.xSpdType not in [100,101] and self.xSpdDist <= 0) or (self.xSpdType in [100,101] and self.xSpdDist < -250):
|
||
self.xSpdType = -1
|
||
self.xSpdDist = self.xSpdLimit = 0
|
||
if self.xTurnInfo < 0 or self.xDistToTurn < -50:
|
||
if self.xDistToTurn > 0:
|
||
self.xDistToTurn = 0
|
||
self.xTurnInfo = -1
|
||
self.xDistToTurnNext = 0
|
||
self.xTurnInfoNext = -1
|
||
|
||
sdi_speed = 250
|
||
hda_active = False
|
||
### 과속카메라, 사고방지턱
|
||
if (self.xSpdDist > 0 or self.xSpdType in [100, 101]) and self.active_carrot > 0:
|
||
safe_sec = self.autoNaviSpeedBumpTime if self.xSpdType == 22 else self.autoNaviSpeedCtrlEnd
|
||
decel = self.autoNaviSpeedDecelRate
|
||
sdi_speed = min(sdi_speed, self.calculate_current_speed(self.xSpdDist, self.xSpdLimit, safe_sec, decel))
|
||
self.active_carrot = 5 if self.xSpdType == 22 else 3
|
||
if self.xSpdType == 4 or (self.xSpdType in [100, 101] and self.xSpdDist <= 0):
|
||
sdi_speed = self.xSpdLimit
|
||
self.active_carrot = 4
|
||
elif CS is not None and CS.speedLimit > 0 and CS.speedLimitDistance > 0:
|
||
sdi_speed = min(sdi_speed,
|
||
self.calculate_current_speed(CS.speedLimitDistance,
|
||
CS.speedLimit * self.autoNaviSpeedSafetyFactor,
|
||
self.autoNaviSpeedCtrlEnd,
|
||
self.autoNaviSpeedDecelRate))
|
||
#self.active_carrot = 6
|
||
hda_active = True
|
||
|
||
#print(f"sdi_speed: {sdi_speed}, hda_active: {hda_active}, xSpdType: {self.xSpdType}, xSpdDist: {self.xSpdDist}, active_carrot: {self.active_carrot}, v_ego_kph: {v_ego_kph}, nRoadLimitSpeed: {self.nRoadLimitSpeed}")
|
||
### TBT 속도제어
|
||
atc_desired, self.atcType, self.atcSpeed, self.atcDist = self.update_auto_turn(v_ego*3.6, sm, self.xTurnInfo, self.xDistToTurn, True)
|
||
atc_desired_next, _, _, _ = self.update_auto_turn(v_ego*3.6, sm, self.xTurnInfoNext, self.xDistToTurnNext, False)
|
||
|
||
if self.nSdiType >= 0: # or self.active_carrot > 0:
|
||
pass
|
||
# self.debugText = (
|
||
# f"Atc:{atc_desired:.1f}," +
|
||
# f"{self.xTurnInfo}:{self.xDistToTurn:.1f}, " +
|
||
# f"I({self.nTBTNextRoadWidth},{self.roadcate}) " +
|
||
# f"Atc2:{atc_desired_next:.1f}," +
|
||
# f"{self.xTurnInfoNext},{self.xDistToTurnNext:.1f}"
|
||
# )
|
||
#self.debugText = "" #f" {self.nSdiType}/{self.nSdiSpeedLimit}/{self.nSdiDist},BLOCK:{self.nSdiBlockType}/{self.nSdiBlockSpeed}/{self.nSdiBlockDist}, PLUS:{self.nSdiPlusType}/{self.nSdiPlusSpeedLimit}/{self.nSdiPlusDist}"
|
||
#elif self.nGoPosDist > 0 and self.active_carrot > 1:
|
||
# self.debugText = " 목적지:{:.1f}km/{:.1f}분 남음".format(self.nGoPosDist/1000., self.nGoPosTime / 60)
|
||
else:
|
||
#self.debugText = ""
|
||
pass
|
||
|
||
if self.autoTurnControl not in [2, 3]: # auto turn speed control
|
||
atc_desired = atc_desired_next = 250
|
||
|
||
if self.autoTurnControl not in [1,2]: # auto turn control
|
||
self.atcType = "none"
|
||
|
||
|
||
speed_n_sources = [
|
||
(atc_desired, "atc"),
|
||
(atc_desired_next, "atc2"),
|
||
(sdi_speed, "hda" if hda_active else "bump" if self.xSpdType == 22 else "section" if self.xSpdType == 4 else "police" if self.xSpdType == 100 else "waze" if self.xSpdType == 101 else "cam"),
|
||
(limit_speed, "road"),
|
||
]
|
||
if self.turnSpeedControlMode in [1,2]:
|
||
speed_n_sources.append((max(abs(vturn_speed), self.autoCurveSpeedLowerLimit), "vturn"))
|
||
|
||
route_speed = max(route_speed * self.mapTurnSpeedFactor, self.autoCurveSpeedLowerLimit)
|
||
if self.turnSpeedControlMode == 2:
|
||
if -500 < self.xDistToTurn < 500:
|
||
speed_n_sources.append((route_speed, "route"))
|
||
elif self.turnSpeedControlMode in [3, 4]:
|
||
speed_n_sources.append((route_speed, "route"))
|
||
#speed_n_sources.append((self.calculate_current_speed(dist, speed * self.mapTurnSpeedFactor, 0, 1.2), "route"))
|
||
|
||
model_turn_speed = max(sm['modelV2'].meta.modelTurnSpeed, self.autoCurveSpeedLowerLimit)
|
||
if model_turn_speed < 200 and abs(vturn_speed) < 120:
|
||
speed_n_sources.append((model_turn_speed, "model"))
|
||
|
||
desired_speed, source = min(speed_n_sources, key=lambda x: x[0])
|
||
|
||
if CS is not None:
|
||
if source != self.source_last:
|
||
self.gas_override_speed = 0
|
||
self.gas_pressed_state = CS.gasPressed
|
||
if CS.vEgo < 0.1 or desired_speed > 150 or source in ["cam", "section", "police"] or CS.brakePressed or road_speed_limit_changed:
|
||
self.gas_override_speed = 0
|
||
elif CS.gasPressed and not self.gas_pressed_state:
|
||
self.gas_override_speed = max(v_ego_kph, self.gas_override_speed)
|
||
else:
|
||
self.gas_pressed_state = False
|
||
self.source_last = source
|
||
|
||
if desired_speed < self.gas_override_speed:
|
||
source = "gas"
|
||
desired_speed = self.gas_override_speed
|
||
|
||
self.debugText += f"route={route_speed:.1f}"#f"desired={desired_speed:.1f},{source},g={self.gas_override_speed:.0f}"
|
||
|
||
left_spd_sec = 100
|
||
left_tbt_sec = 100
|
||
if self.autoNaviCountDownMode > 0:
|
||
if self.xSpdType == 22 and self.autoNaviCountDownMode == 1: # speed bump
|
||
pass
|
||
else:
|
||
if self.xSpdDist > 0:
|
||
left_spd_sec = min(self.left_spd_sec, int(max(self.xSpdDist - v_ego, 1) / max(1, v_ego) + 0.5))
|
||
|
||
if self.xDistToTurn > 0:
|
||
left_tbt_sec = min(self.left_tbt_sec, int(max(self.xDistToTurn - v_ego, 1) / max(1, v_ego) + 0.5))
|
||
|
||
self.left_spd_sec = left_spd_sec
|
||
self.left_tbt_sec = left_tbt_sec
|
||
|
||
left_sec = min(left_spd_sec, left_tbt_sec)
|
||
|
||
if left_sec > 11:
|
||
self.left_sec = 100
|
||
self.max_left_sec = 100
|
||
else:
|
||
self.sdi_inform = True if source in ["cam", "hda"] else False
|
||
self.max_left_sec = min(11, max(6, int(v_ego_kph/10) + 1))
|
||
|
||
if left_sec != self.left_sec:
|
||
if left_sec == self.max_left_sec and self.sdi_inform:
|
||
self.carrot_left_sec = 11
|
||
elif 1 <= left_sec < self.max_left_sec:
|
||
self.carrot_left_sec = left_sec
|
||
elif left_sec == 0 and self.left_sec == 1:
|
||
self.carrot_left_sec = left_sec
|
||
|
||
self.left_sec = left_sec
|
||
|
||
|
||
self._update_cmd()
|
||
msg = messaging.new_message('carrotMan')
|
||
msg.valid = True
|
||
msg.carrotMan.activeCarrot = self.active_carrot
|
||
msg.carrotMan.nRoadLimitSpeed = int(self.nRoadLimitSpeed)
|
||
msg.carrotMan.remote = remote_ip
|
||
msg.carrotMan.xSpdType = int(self.xSpdType)
|
||
msg.carrotMan.xSpdLimit = int(self.xSpdLimit)
|
||
msg.carrotMan.xSpdDist = int(self.xSpdDist)
|
||
msg.carrotMan.xSpdCountDown = int(left_spd_sec)
|
||
msg.carrotMan.xTurnInfo = int(self.xTurnInfo)
|
||
msg.carrotMan.xDistToTurn = int(self.xDistToTurn)
|
||
msg.carrotMan.xTurnCountDown = int(left_tbt_sec)
|
||
msg.carrotMan.atcType = self.atcType
|
||
msg.carrotMan.vTurnSpeed = int(vturn_speed)
|
||
msg.carrotMan.szPosRoadName = self.szPosRoadName + self.debugText
|
||
msg.carrotMan.szTBTMainText = self.szTBTMainText
|
||
msg.carrotMan.desiredSpeed = int(desired_speed)
|
||
msg.carrotMan.desiredSource = source
|
||
msg.carrotMan.carrotCmdIndex = int(self.carrotCmdIndex)
|
||
msg.carrotMan.carrotCmd = self.carrotCmd
|
||
msg.carrotMan.carrotArg = self.carrotArg
|
||
msg.carrotMan.trafficState = self.traffic_state
|
||
|
||
msg.carrotMan.xPosSpeed = float(v_ego_kph) #float(self.nPosSpeed)
|
||
msg.carrotMan.xPosAngle = float(self.bearing)
|
||
msg.carrotMan.xPosLat = float(self.vpPosPointLat)
|
||
msg.carrotMan.xPosLon = float(self.vpPosPointLon)
|
||
|
||
msg.carrotMan.nGoPosDist = self.nGoPosDist
|
||
msg.carrotMan.nGoPosTime = self.nGoPosTime
|
||
msg.carrotMan.szSdiDescr = self._get_sdi_descr(-1 if self.nSdiType == 0 and self.nSdiDist == 0 else self.nSdiType)
|
||
|
||
#coords_str = ";".join([f"{x},{y}" for x, y in coords])
|
||
coords_str = ";".join([f"{x:.2f},{y:.2f},{d:.2f}" for (x, y), d in zip(coords, distances, strict=False)])
|
||
msg.carrotMan.naviPaths = coords_str
|
||
|
||
msg.carrotMan.leftSec = int(self.carrot_left_sec)
|
||
pm.send('carrotMan', msg)
|
||
|
||
inst = messaging.new_message('navInstructionCarrot')
|
||
if self.active_carrot > 1 and self.active_kisa_count <= 0:
|
||
inst.valid = True
|
||
|
||
instruction = inst.navInstructionCarrot
|
||
instruction.distanceRemaining = self.nGoPosDist
|
||
instruction.timeRemaining = self.nGoPosTime
|
||
instruction.speedLimit = self.nRoadLimitSpeed / 3.6 if self.nRoadLimitSpeed > 0 else 0
|
||
instruction.maneuverDistance = float(self.nTBTDist)
|
||
instruction.maneuverSecondaryText = self.szNearDirName
|
||
if self.szFarDirName and len(self.szFarDirName):
|
||
instruction.maneuverSecondaryText += "[{}]".format(self.szFarDirName)
|
||
instruction.maneuverPrimaryText = self.szTBTMainText
|
||
instruction.timeRemainingTypical = self.nGoPosTime
|
||
|
||
navType, navModifier, xTurnInfo1 = "invalid", "", -1
|
||
if self.nTBTTurnType in nav_type_mapping:
|
||
navType, navModifier, xTurnInfo1 = nav_type_mapping[self.nTBTTurnType]
|
||
navTypeNext, navModifierNext, xTurnInfoNext = "invalid", "", -1
|
||
if self.nTBTTurnTypeNext in nav_type_mapping:
|
||
navTypeNext, navModifierNext, xTurnInfoNext = nav_type_mapping[self.nTBTTurnTypeNext]
|
||
|
||
instruction.maneuverType = navType
|
||
instruction.maneuverModifier = navModifier
|
||
|
||
maneuvers = []
|
||
if self.nTBTTurnType >= 0:
|
||
maneuver = {}
|
||
maneuver['distance'] = float(self.xDistToTurn)
|
||
maneuver['type'] = navType
|
||
maneuver['modifier'] = navModifier
|
||
maneuvers.append(maneuver)
|
||
if self.nTBTDistNext >= self.nTBTDist:
|
||
maneuver = {}
|
||
maneuver['distance'] = float(self.nTBTDistNext)
|
||
maneuver['type'] = navTypeNext
|
||
maneuver['modifier'] = navModifierNext
|
||
maneuvers.append(maneuver)
|
||
|
||
instruction.allManeuvers = maneuvers
|
||
elif sm.alive['navInstruction'] and sm.valid['navInstruction']:
|
||
inst.navInstructionCarrot = sm['navInstruction']
|
||
|
||
pm.send('navInstructionCarrot', inst)
|
||
|
||
def _update_system_time(self, epoch_time_remote, timezone_remote):
|
||
epoch_time = int(time.time())
|
||
if epoch_time_remote > 0:
|
||
epoch_time_offset = epoch_time_remote - epoch_time
|
||
print(f"epoch_time_offset = {epoch_time_offset}")
|
||
if abs(epoch_time_offset) > 60:
|
||
os.system(f"sudo timedatectl set-timezone {timezone_remote}")
|
||
formatted_time = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(epoch_time_remote))
|
||
print(f"Setting system time to: {formatted_time}")
|
||
os.system(f'sudo date -s "{formatted_time}"')
|
||
|
||
def set_time(self, epoch_time, timezone):
|
||
import datetime
|
||
new_time = datetime.datetime.utcfromtimestamp(epoch_time)
|
||
localtime_path = "/data/etc/localtime"
|
||
|
||
no_timezone = False
|
||
try:
|
||
if os.path.getsize(localtime_path) == 0:
|
||
no_timezone = True
|
||
except (FileNotFoundError, OSError):
|
||
no_timezone = True
|
||
|
||
diff = datetime.datetime.utcnow() - new_time
|
||
if abs(diff) < datetime.timedelta(seconds=10) and not no_timezone:
|
||
#print(f"Time diff too small: {diff}")
|
||
return
|
||
|
||
print(f"Setting time to {new_time}, diff={diff}")
|
||
zoneinfo_path = f"/usr/share/zoneinfo/{timezone}"
|
||
if os.path.exists(localtime_path) or os.path.islink(localtime_path):
|
||
try:
|
||
subprocess.run(["sudo", "rm", "-f", localtime_path], check=True)
|
||
print(f"Removed existing file or link: {localtime_path}")
|
||
except subprocess.CalledProcessError as e:
|
||
print(f"Error removing {localtime_path}: {e}")
|
||
return
|
||
try:
|
||
subprocess.run(["sudo", "ln", "-s", zoneinfo_path, localtime_path], check=True)
|
||
print(f"Timezone successfully set to: {timezone}")
|
||
except subprocess.CalledProcessError as e:
|
||
print(f"Failed to set timezone to {timezone}: {e}")
|
||
|
||
|
||
try:
|
||
subprocess.run(f"TZ=UTC date -s '{new_time}'", shell=True, check=True)
|
||
#subprocess.run()
|
||
except subprocess.CalledProcessError:
|
||
print("timed.failed_setting_time")
|
||
|
||
def update(self, json):
|
||
if json is None:
|
||
return
|
||
if "carrotIndex" in json:
|
||
self.carrotIndex = int(json.get("carrotIndex"))
|
||
|
||
#print(json)
|
||
if self.carrotIndex % 60 == 0 and "epochTime" in json:
|
||
# op는 ntp를 사용하기때문에... 필요없는 루틴으로 보임.
|
||
timezone_remote = json.get("timezone", "Asia/Seoul")
|
||
|
||
if not PC:
|
||
self.set_time(int(json.get("epochTime")), timezone_remote)
|
||
|
||
#self._update_system_time(int(json.get("epochTime")), timezone_remote)
|
||
|
||
if "carrotCmd" in json:
|
||
#print(json.get("carrotCmd"), json.get("carrotArg"))
|
||
self.carrotCmdIndex = self.carrotIndex
|
||
self.carrotCmd = json.get("carrotCmd")
|
||
self.carrotArg = json.get("carrotArg")
|
||
print(f"carrotCmd = {self.carrotCmd}, {self.carrotArg}")
|
||
|
||
self.active_count = 80
|
||
now = time.monotonic()
|
||
|
||
if "goalPosX" in json:
|
||
self.goalPosX = float(json.get("goalPosX", self.goalPosX))
|
||
self.goalPosY = float(json.get("goalPosY", self.goalPosY))
|
||
self.szGoalName = json.get("szGoalName", self.szGoalName)
|
||
|
||
if "nRoadLimitSpeed" in json:
|
||
#print(json)
|
||
self.active_sdi_count = self.active_sdi_count_max
|
||
### roadLimitSpeed
|
||
nRoadLimitSpeed = int(json.get("nRoadLimitSpeed", 20))
|
||
if nRoadLimitSpeed > 0:
|
||
if nRoadLimitSpeed > 200:
|
||
nRoadLimitSpeed = (nRoadLimitSpeed - 20) / 10
|
||
elif nRoadLimitSpeed == 120:
|
||
nRoadLimitSpeed = 115 # 120 -> 115 fix bug
|
||
else:
|
||
nRoadLimitSpeed = 30
|
||
#self.nRoadLimitSpeed = nRoadLimitSpeed
|
||
if self.nRoadLimitSpeed != nRoadLimitSpeed:
|
||
self.nRoadLimitSpeed_counter += 1
|
||
if self.nRoadLimitSpeed_counter > 5:
|
||
self.nRoadLimitSpeed = nRoadLimitSpeed
|
||
else:
|
||
self.nRoadLimitSpeed_counter = 0
|
||
|
||
### SDI
|
||
self.nSdiType = int(json.get("nSdiType", -1))
|
||
self.nSdiSpeedLimit = int(json.get("nSdiSpeedLimit", 0))
|
||
self.nSdiSection = int(json.get("nSdiSection", -1))
|
||
self.nSdiDist = int(json.get("nSdiDist", -1))
|
||
self.nSdiBlockType = int(json.get("nSdiBlockType", -1))
|
||
self.nSdiBlockSpeed = int(json.get("nSdiBlockSpeed", 0))
|
||
self.nSdiBlockDist = int(json.get("nSdiBlockDist", 0))
|
||
|
||
self.nSdiPlusType = int(json.get("nSdiPlusType", -1))
|
||
self.nSdiPlusSpeedLimit = int(json.get("nSdiPlusSpeedLimit", 0))
|
||
self.nSdiPlusDist = int(json.get("nSdiPlusDist", 0))
|
||
self.nSdiPlusBlockType = int(json.get("nSdiPlusBlockType", -1))
|
||
self.nSdiPlusBlockSpeed = int(json.get("nSdiPlusBlockSpeed", 0))
|
||
self.nSdiPlusBlockDist = int(json.get("nSdiPlusBlockDist", 0))
|
||
self.roadcate = int(json.get("roadcate", 0))
|
||
|
||
## GuidePoint
|
||
self.nTBTDist = int(json.get("nTBTDist", 0))
|
||
self.nTBTTurnType = int(json.get("nTBTTurnType", -1))
|
||
self.szTBTMainText = json.get("szTBTMainText", "")
|
||
self.szNearDirName = json.get("szNearDirName", "")
|
||
self.szFarDirName = json.get("szFarDirName", "")
|
||
|
||
self.nTBTNextRoadWidth = int(json.get("nTBTNextRoadWidth", 0))
|
||
self.nTBTDistNext = int(json.get("nTBTDistNext", 0))
|
||
self.nTBTTurnTypeNext = int(json.get("nTBTTurnTypeNext", -1))
|
||
self.szTBTMainTextNext = json.get("szTBTMainText", "")
|
||
|
||
self.nGoPosDist = int(json.get("nGoPosDist", 0))
|
||
self.nGoPosTime = int(json.get("nGoPosTime", 0))
|
||
self.szPosRoadName = json.get("szPosRoadName", "")
|
||
if self.szPosRoadName == "null":
|
||
self.szPosRoadName = ""
|
||
|
||
self.vpPosPointLatNavi = float(json.get("vpPosPointLat", 0.0))
|
||
self.vpPosPointLonNavi = float(json.get("vpPosPointLon", 0.0))
|
||
if self.vpPosPointLatNavi != 0.0:
|
||
self.last_update_gps_time_navi = self.last_calculate_gps_time = now
|
||
self.nPosAngle = float(json.get("nPosAngle", self.nPosAngle))
|
||
|
||
self.nPosSpeed = float(json.get("nPosSpeed", self.nPosSpeed))
|
||
self._update_tbt()
|
||
self._update_sdi()
|
||
print(
|
||
f"sdi = {self.nSdiType}, {self.nSdiSpeedLimit}, {self.nSdiPlusType}, " +
|
||
f"tbt = {self.nTBTTurnType}, {self.nTBTDist}, " +
|
||
f"next = {self.nTBTTurnTypeNext}, {self.nTBTDistNext}"
|
||
)
|
||
#print(json)
|
||
else:
|
||
#print(json)
|
||
pass
|
||
|
||
# 3초간 navi 데이터가 없으면, phone gps로 업데이트
|
||
if "latitude" in json:
|
||
self.nPosAnglePhone = float(json.get("heading", self.nPosAngle))
|
||
self.phone_latitude = float(json.get("latitude", self.vpPosPointLatNavi))
|
||
self.phone_longitude = float(json.get("longitude", self.vpPosPointLonNavi))
|
||
self.phone_gps_accuracy = float(json.get("accuracy", 0))
|
||
if self.phone_gps_accuracy < 15.0:
|
||
self.phone_gps_frame += 1
|
||
if (now - self.last_update_gps_time_navi) > 3.0:
|
||
self.vpPosPointLatNavi = self.phone_latitude
|
||
self.vpPosPointLonNavi = self.phone_longitude
|
||
|
||
self.nPosAngle = self.nPosAnglePhone
|
||
# self.nPosSpeed = self.ve # TODO speed from v_ego
|
||
self.last_update_gps_time_phone = self.last_calculate_gps_time = now
|
||
self.nPosSpeed = float(json.get("gps_speed", 0))
|
||
print(f"phone gps: {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}, {self.phone_gps_accuracy}, {self.nPosSpeed}")
|
||
|
||
|
||
import traceback
|
||
|
||
def main():
|
||
print("CarrotManager Started")
|
||
#print("Carrot GitBranch = {}, {}".format(Params().get("GitBranch"), Params().get("GitCommitDate")))
|
||
# 延迟导入，避免与 carrot_man 中导入 CarrotServ 的循环依赖
|
||
from openpilot.selfdrive.carrot.carrot_man import CarrotMan
|
||
carrot_man = CarrotMan()
|
||
|
||
print(f"CarrotMan {carrot_man}")
|
||
threading.Thread(target=carrot_man.kisa_app_thread).start()
|
||
while True:
|
||
try:
|
||
carrot_man.carrot_man_thread()
|
||
except Exception as e:
|
||
print(f"carrot_man error...: {e}")
|
||
traceback.print_exc()
|
||
time.sleep(10)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|