Update251203 (#233)
This commit is contained in:
carrot
@@ -247,6 +247,7 @@ class VCruiseCarrot:
|
||||
self.autoGasSyncSpeed = self.params.get_bool("AutoGasSyncSpeed") * unit_factor
|
||||
self.autoSpeedUptoRoadSpeedLimit = self.params.get_float("AutoSpeedUptoRoadSpeedLimit") * 0.01
|
||||
self.autoRoadSpeedAdjust = self.params.get_float("AutoRoadSpeedAdjust") * 0.01
|
||||
self.smartSpeedControl = self.params.get_int("CarrotSmartSpeedControl")
|
||||
|
||||
useLaneLineSpeed = self.params.get_int("UseLaneLineSpeed") * unit_factor
|
||||
if self.useLaneLineSpeed != useLaneLineSpeed:
|
||||
@@ -441,6 +442,20 @@ class VCruiseCarrot:
|
||||
return button_kph, button_type, self.long_pressed
|
||||
|
||||
def _carrot_command(self, v_cruise_kph, button_type, long_pressed):
|
||||
carrot_speed = self.params_memory.get_int("CarrotSpeed")
|
||||
if carrot_speed != 0:
|
||||
if carrot_speed > 0:
|
||||
if self.smartSpeedControl in [1,3]:
|
||||
v_cruise_kph = max(carrot_speed, v_cruise_kph)
|
||||
else:
|
||||
if self.smartSpeedControl == 3:
|
||||
v_cruise_kph = -carrot_speed
|
||||
#elif self.smartSpeedControl == 1:
|
||||
# v_cruise_kph = max(-carrot_speed, v_cruise_kph)
|
||||
elif self.smartSpeedControl == 2:
|
||||
v_cruise_kph = min(-carrot_speed, v_cruise_kph)
|
||||
self.params_memory.put_int_nonblocking("CarrotSpeed", 0)
|
||||
self._add_log(f"Carrot speed set to {v_cruise_kph}")
|
||||
if self.carrot_cmd_index_last != self.carrot_cmd_index:
|
||||
self.carrot_cmd_index_last = self.carrot_cmd_index
|
||||
print(f"Carrot command(cruise.py): {self.carrot_cmd} {self.carrot_arg}")
|
||||
|
||||
@@ -22,6 +22,9 @@ from openpilot.selfdrive.navd.helpers import Coordinate
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
|
||||
from openpilot.selfdrive.carrot.carrot_serv import CarrotServ
|
||||
from openpilot.selfdrive.carrot.carrot_speed import CarrotSpeed
|
||||
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
|
||||
try:
|
||||
from shapely.geometry import LineString
|
||||
@@ -185,7 +188,8 @@ class CarrotMan:
|
||||
print("************************************************CarrotMan init************************************************")
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman', 'navInstruction'])
|
||||
self.gps_location_service = get_gps_location_service(self.params)
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'radarState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', self.gps_location_service, 'navInstruction'])
|
||||
self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
|
||||
|
||||
self.carrot_serv = CarrotServ()
|
||||
@@ -258,9 +262,22 @@ class CarrotMan:
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
|
||||
frame = 0
|
||||
self.save_toggle_values()
|
||||
rk = Ratekeeper(10, print_delay_threshold=None)
|
||||
|
||||
carrot_speed = CarrotSpeed(neighbor_ring=2)
|
||||
self.params_memory.put_int_nonblocking("CarrotSpeed", 0)
|
||||
|
||||
rk = Ratekeeper(20, print_delay_threshold=None)
|
||||
|
||||
carrotIndex_last = self.carrot_serv.carrotIndex
|
||||
phone_gps_frame = self.carrot_serv.phone_gps_frame
|
||||
carrot_speed_active_count = 0
|
||||
self.v_cruise_last = 0
|
||||
self.long_active = False
|
||||
self.v_cruise_change = 0
|
||||
self._last_vt = 0.0
|
||||
self.gas_pressed_count = 0
|
||||
self._last_viz_t = 0.0
|
||||
|
||||
while self.is_running:
|
||||
try:
|
||||
self.sm.update(0)
|
||||
@@ -273,7 +290,16 @@ class CarrotMan:
|
||||
|
||||
#print("coords=", coords)
|
||||
#print("curvatures=", curvatures)
|
||||
self.carrot_serv.update_navi(remote_ip, self.sm, self.pm, vturn_speed, coords, distances, route_speed)
|
||||
self.carrot_serv.update_navi(remote_ip, self.sm, self.pm, vturn_speed, coords, distances, route_speed, self.gps_location_service)
|
||||
|
||||
if phone_gps_frame != self.carrot_serv.phone_gps_frame:
|
||||
phone_gps_frame = self.carrot_serv.phone_gps_frame
|
||||
carrot_speed_active_count = 10
|
||||
else:
|
||||
carrot_speed_active_count -= 1
|
||||
|
||||
if carrot_speed_active_count > 0:
|
||||
self.carrot_speed_serv(carrot_speed, frame)
|
||||
|
||||
if frame % 20 == 0 or remote_addr is not None:
|
||||
try:
|
||||
@@ -318,6 +344,71 @@ class CarrotMan:
|
||||
traceback.print_exc()
|
||||
time.sleep(1)
|
||||
|
||||
def carrot_speed_serv(self, carrot_speed, frame):
|
||||
v_ego = a_ego = 0.0
|
||||
gas_pressed = False
|
||||
if self.sm.alive['carState'] and self.sm.alive['carControl']:
|
||||
CS = self.sm['carState']
|
||||
CC = self.sm['carControl']
|
||||
v_ego = CS.vEgo
|
||||
a_ego = CS.aEgo
|
||||
gas_pressed = CS.gasPressed
|
||||
v_ego_kph = v_ego * 3.6
|
||||
if gas_pressed:
|
||||
self.gas_pressed_count = 200
|
||||
self.v_cruise_change = 0
|
||||
elif self._last_vt == CS.vCruise:
|
||||
self.v_cruise_last = CS.vCruise
|
||||
elif self.long_active and CC.longActive and self.gas_pressed_count == 0:
|
||||
if self.v_cruise_last < CS.vCruise: # 속도가 증가하면
|
||||
self.v_cruise_change = 100
|
||||
elif self.v_cruise_last > CS.vCruise: # 속도가 감소하면
|
||||
if v_ego_kph < CS.vCruise: # 주행속도가 느리면
|
||||
self.v_cruise_change = 100
|
||||
else: # 주행속도가 빠르면
|
||||
self.v_cruise_change = -100
|
||||
|
||||
if self.v_cruise_change != 0:
|
||||
self.gas_pressed_count = 0
|
||||
else:
|
||||
self.v_cruise_change = 0
|
||||
self.long_active = CC.longActive
|
||||
self.v_cruise_last = CS.vCruise
|
||||
else:
|
||||
self.v_cruise_change = 0
|
||||
|
||||
now = time.monotonic()
|
||||
heading = self.carrot_serv.bearing #nPosAnglePhone
|
||||
lat, lon = self.carrot_serv.vpPosPointLat, self.carrot_serv.vpPosPointLon #self.carrot_serv.estimate_position(self.carrot_serv.phone_latitude, self.carrot_serv.phone_longitude, heading, v_ego, now - self.carrot_serv.last_update_gps_time_phone)
|
||||
vt = carrot_speed.query_target_dist(lat, lon, heading, 0.0)
|
||||
if self.v_cruise_change != 0:
|
||||
carrot_speed.add_sample(lat, lon, heading, self.v_cruise_last if self.v_cruise_change > 0 else (- self.v_cruise_last))
|
||||
if self.v_cruise_change > 0:
|
||||
self.v_cruise_change -= 1
|
||||
if self.v_cruise_change < 0:
|
||||
self.v_cruise_change += 1
|
||||
else:
|
||||
if self.gas_pressed_count > 0:
|
||||
vt = max(vt, self.v_cruise_last)
|
||||
carrot_speed.add_sample(lat, lon, heading, vt)
|
||||
|
||||
self.params_memory.put_int_nonblocking("CarrotSpeed", int(vt))
|
||||
|
||||
self._last_vt = vt
|
||||
if gas_pressed and a_ego < -0.5: #self._last_vt < 0.0:
|
||||
carrot_speed.invalidate_last_hit(window_s=2.0, action="clear")
|
||||
self.gas_pressed_count = max(0, self.gas_pressed_count - 1)
|
||||
|
||||
if now - self._last_viz_t > 0.5: # 2Hz
|
||||
self._last_viz_t = now
|
||||
viz_json = carrot_speed.export_cells_around(lat, lon, heading, ring=2, max_points=64)
|
||||
# 메모리 Params에 쓰는 게 좋음 (디스크 말고)
|
||||
self.params_memory.put_nonblocking("CarrotSpeedViz", viz_json)
|
||||
|
||||
carrot_speed.maybe_save()
|
||||
|
||||
|
||||
|
||||
def carrot_navi_route(self):
|
||||
|
||||
if self.carrot_serv.active_carrot > 1:
|
||||
|
||||
@@ -19,6 +19,7 @@ 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),
|
||||
@@ -145,8 +146,11 @@ class CarrotServ:
|
||||
self.bearing = 0.0
|
||||
self.gps_valid = False
|
||||
|
||||
self.gps_accuracy_phone = 0.0
|
||||
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
|
||||
@@ -641,15 +645,14 @@ class CarrotServ:
|
||||
self.xSpdType = -1
|
||||
self.xSpdDist = 0
|
||||
|
||||
def _update_gps(self, v_ego, sm):
|
||||
llk = 'liveLocationKalman'
|
||||
location = sm[llk]
|
||||
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 = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
self.gps_valid = sm.updated[gps_service] and gps.hasFix
|
||||
|
||||
now = time.monotonic()
|
||||
gps_updated_phone = (now - self.last_update_gps_time_phone) < 3
|
||||
@@ -658,8 +661,8 @@ class CarrotServ:
|
||||
bearing = self.nPosAngle
|
||||
if gps_updated_phone:
|
||||
self.bearing_offset = 0.0
|
||||
elif sm.valid[llk]:
|
||||
bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
elif self.gps_valid:
|
||||
bearing = self.nPosAngle = gps.bearingDeg
|
||||
if self.gps_valid:
|
||||
self.bearing_offset = 0.0
|
||||
elif self.active_carrot > 0:
|
||||
@@ -669,13 +672,13 @@ class CarrotServ:
|
||||
#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 = 1.0
|
||||
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 = location.positionGeodetic.value[0]
|
||||
self.vpPosPointLonNavi = location.positionGeodetic.value[1]
|
||||
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:
|
||||
@@ -853,7 +856,7 @@ class CarrotServ:
|
||||
self.xSpdDist = distance
|
||||
self.xSpdType =xSpdType
|
||||
|
||||
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed):
|
||||
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed, gps_service):
|
||||
|
||||
self.debugText = ""
|
||||
self.update_params()
|
||||
@@ -874,7 +877,7 @@ class CarrotServ:
|
||||
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)
|
||||
self.bearing = self._update_gps(v_ego, sm, gps_service)
|
||||
|
||||
self.xSpdDist = max(self.xSpdDist - delta_dist, -1000)
|
||||
self.xDistToTurn = self.xDistToTurn - delta_dist
|
||||
@@ -1281,15 +1284,20 @@ class CarrotServ:
|
||||
# 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 = float(json.get("latitude", self.vpPosPointLatNavi))
|
||||
self.vpPosPointLonNavi = float(json.get("longitude", self.vpPosPointLonNavi))
|
||||
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.gps_accuracy_phone = float(json.get("accuracy", 0))
|
||||
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.gps_accuracy_phone}, {self.nPosSpeed}")
|
||||
print(f"phone gps: {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}, {self.phone_gps_accuracy}, {self.nPosSpeed}")
|
||||
|
||||
|
||||
import traceback
|
||||
|
||||
@@ -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
|
||||
@@ -1744,6 +1744,19 @@
|
||||
"default": 1,
|
||||
"unit": 1
|
||||
},
|
||||
{
|
||||
"group": "감속제어",
|
||||
"name": "CarrotSmartSpeedControl",
|
||||
"title": "스마트속도제어(속도재생)",
|
||||
"descr": "0: 속도제어안함\n 1: 가속만, 2: 감속만, 3: 모두",
|
||||
"egroup": "SPEED",
|
||||
"etitle": "Smart Speed Control(Replay)",
|
||||
"edescr": "0:not use, 1:accel, 2:decel, 3:all",
|
||||
"min": 0,
|
||||
"max": 3,
|
||||
"default": 0,
|
||||
"unit": 1
|
||||
},
|
||||
{
|
||||
"group": "감속제어",
|
||||
"name": "MapTurnSpeedFactor",
|
||||
|
||||
@@ -48,7 +48,7 @@ class Controls:
|
||||
|
||||
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'carrotMan', 'lateralPlan', 'radarState', 'liveLocationKalman',
|
||||
'carrotMan', 'lateralPlan', 'radarState',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
@@ -180,7 +180,7 @@ class Controls:
|
||||
actuators.curvature = float(self.desired_curvature)
|
||||
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited_by_controls, self.desired_curvature,
|
||||
self.sm['liveLocationKalman'], curvature_limited,
|
||||
CC, curvature_limited,
|
||||
model_data=self.sm['modelV2'])
|
||||
actuators.torque = float(steer)
|
||||
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
||||
@@ -237,23 +237,16 @@ class Controls:
|
||||
|
||||
# Orientation and angle rates can be useful for carcontroller
|
||||
# Only calibrated (car) frame is relevant for the carcontroller
|
||||
#if self.calibrated_pose is not None:
|
||||
# CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
||||
# CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
||||
if self.calibrated_pose is not None:
|
||||
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
||||
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
||||
|
||||
orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
|
||||
if len(orientation_value) > 2:
|
||||
CC.orientationNED = orientation_value
|
||||
angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
|
||||
if len(angular_rate_value) > 2:
|
||||
CC.angularVelocity = angular_rate_value
|
||||
|
||||
acceleration_value = list(self.sm['liveLocationKalman'].accelerationCalibrated.value)
|
||||
if len(acceleration_value) > 2:
|
||||
if abs(acceleration_value[0]) > 16.0:
|
||||
print("Collision detected. disable openpilot, restart")
|
||||
self.params.put_bool("OpenpilotEnabledToggle", False)
|
||||
self.params.put_int("SoftRestartTriggered", 1)
|
||||
#acceleration_value = list(self.sm['liveLocationKalman'].accelerationCalibrated.value)
|
||||
#if len(acceleration_value) > 2:
|
||||
# if abs(acceleration_value[0]) > 16.0:
|
||||
# print("Collision detected. disable openpilot, restart")
|
||||
# self.params.put_bool("OpenpilotEnabledToggle", False)
|
||||
# self.params.put_int("SoftRestartTriggered", 1)
|
||||
|
||||
CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
|
||||
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
|
||||
|
||||
@@ -17,7 +17,7 @@ class LatControl(ABC):
|
||||
self.steer_max = 1.0
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
|
||||
@@ -17,7 +17,7 @@ class LatControlAngle(LatControl):
|
||||
#self.factor = 0.5
|
||||
#print("Angle factor", self.factor)
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if not active:
|
||||
|
||||
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
||||
@@ -139,7 +139,7 @@ class LatControlTorque(LatControl):
|
||||
self.torque_params.latAccelOffset = latAccelOffset
|
||||
self.torque_params.friction = friction
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
|
||||
self.frame += 1
|
||||
if self.frame % 10 == 0:
|
||||
lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
|
||||
@@ -181,7 +181,7 @@ class LatControlTorque(LatControl):
|
||||
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
|
||||
actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
|
||||
else:
|
||||
actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
|
||||
actual_curvature_llk = CC.angularVelocity[2] / CS.vEgo #llk.angularVelocityCalibrated.value[2] / CS.vEgo
|
||||
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
|
||||
curvature_deadzone = 0.0
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
@@ -219,8 +219,8 @@ class LatControlTorque(LatControl):
|
||||
# update past data
|
||||
pitch = 0
|
||||
roll = params.roll
|
||||
if len(llk.calibratedOrientationNED.value) > 1:
|
||||
pitch = self.pitch.update(llk.calibratedOrientationNED.value[1])
|
||||
if len(CC.orientionNED) > 1:
|
||||
pitch = self.pitch.update(CC.orientationNED[1])
|
||||
roll = roll_pitch_adjust(roll, pitch)
|
||||
self.roll_deque.append(roll)
|
||||
self.lateral_accel_desired_deque.append(desired_lateral_accel)
|
||||
|
||||
@@ -95,12 +95,16 @@ class LateralPlanner:
|
||||
|
||||
# clip speed , lateral planning is not possible at 0 speed
|
||||
measured_curvature = sm['controlsState'].curvature
|
||||
v_ego_car = sm['carState'].vEgo
|
||||
v_ego_car = max(sm['carState'].vEgo, MIN_SPEED)
|
||||
speed_kph = v_ego_car * 3.6
|
||||
self.v_ego = v_ego_car
|
||||
self.curve_speed = sm['carrotMan'].vTurnSpeed
|
||||
|
||||
# Parse model predictions
|
||||
md = sm['modelV2']
|
||||
model_active = False
|
||||
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
|
||||
model_active = True
|
||||
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
|
||||
self.t_idxs = np.array(md.position.t)
|
||||
self.plan_yaw = np.array(md.orientation.z)
|
||||
@@ -125,9 +129,9 @@ class LateralPlanner:
|
||||
|
||||
if self.useLaneLineSpeedApply == 0 or self.laneless_only:
|
||||
self.useLaneLineMode = False
|
||||
elif self.v_ego*3.6 >= self.useLaneLineSpeedApply + 2:
|
||||
elif speed_kph >= self.useLaneLineSpeedApply + 2:
|
||||
self.useLaneLineMode = True
|
||||
elif self.v_ego*3.6 < self.useLaneLineSpeedApply - 2:
|
||||
elif speed_kph < self.useLaneLineSpeedApply - 2:
|
||||
self.useLaneLineMode = False
|
||||
|
||||
# Turn off lanes during lane change
|
||||
@@ -143,10 +147,15 @@ class LateralPlanner:
|
||||
self.LP.lane_width_left = md.meta.laneWidthLeft
|
||||
self.LP.lane_width_right = md.meta.laneWidthRight
|
||||
self.LP.curvature = measured_curvature
|
||||
self.path_xyz, self.lanelines_active = self.LP.get_d_path(sm['carState'], self.v_ego, self.t_idxs, self.path_xyz, self.curve_speed)
|
||||
|
||||
#if self.LP.lanefull_mode:
|
||||
# self.plan_yaw, self.plan_yaw_rate = self.LP.calculate_plan_yaw_and_yaw_rate(self.path_xyz)
|
||||
self.path_xyz, self.lanelines_active = self.LP.get_d_path(sm['carState'], v_ego_car, self.t_idxs, self.path_xyz, self.curve_speed)
|
||||
|
||||
if self.lanelines_active:
|
||||
self.plan_yaw, self.plan_yaw_rate = yaw_from_path_no_scipy(
|
||||
self.path_xyz, self.v_plan,
|
||||
smooth_window=5,
|
||||
clip_rate=2.0,
|
||||
align_first_yaw=None #md.orientation.z[0] # 초기 정렬
|
||||
)
|
||||
|
||||
self.latDebugText = self.LP.debugText
|
||||
#self.lanelines_active = True if self.LP.d_prob > 0.3 and self.LP.lanefull_mode else False
|
||||
@@ -212,12 +221,14 @@ class LateralPlanner:
|
||||
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
|
||||
lateralPlan.distances = self.lat_mpc.x_sol[0:CONTROL_N, 0].tolist()
|
||||
|
||||
v_div = np.maximum(self.v_plan[:CONTROL_N], 6.0)
|
||||
if len(self.v_plan) == TRAJECTORY_SIZE:
|
||||
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_plan[0:CONTROL_N]).tolist()
|
||||
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / v_div).tolist()
|
||||
else:
|
||||
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_ego).tolist()
|
||||
|
||||
lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
|
||||
v_div2 = max(self.v_ego, 6.0)
|
||||
lateralPlan.curvatureRates = [float(x.item() / v_div2) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
|
||||
|
||||
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
|
||||
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
|
||||
@@ -263,3 +274,74 @@ class LateralPlanner:
|
||||
pm.send('lateralPlan', plan_send)
|
||||
|
||||
|
||||
def smooth_moving_avg(arr, window=5):
|
||||
if window < 2:
|
||||
return arr
|
||||
if window % 2 == 0:
|
||||
window += 1
|
||||
pad = window // 2
|
||||
arr_pad = np.pad(arr, (pad, pad), mode='edge')
|
||||
kernel = np.ones(window) / window
|
||||
return np.convolve(arr_pad, kernel, mode='same')[pad:-pad]
|
||||
|
||||
def yaw_from_path_no_scipy(path_xyz, v_plan, smooth_window=5,
|
||||
clip_rate=2.0, align_first_yaw=None):
|
||||
|
||||
v0 = float(np.asarray(v_plan)[0]) if len(v_plan) else 0.0
|
||||
# 저속(≤6 m/s)에서는 창을 크게
|
||||
if v0 <= 6.0:
|
||||
smooth_window = max(smooth_window, 9) # 9~11 권장
|
||||
|
||||
N = path_xyz.shape[0]
|
||||
x = path_xyz[:, 0].astype(float)
|
||||
y = path_xyz[:, 1].astype(float)
|
||||
|
||||
if N < 5:
|
||||
return np.zeros(N, np.float32), np.zeros(N, np.float32)
|
||||
|
||||
# 1) s(호길이) 계산
|
||||
dx = np.diff(x)
|
||||
dy = np.diff(y)
|
||||
ds_seg = np.sqrt(dx*dx + dy*dy)
|
||||
ds_seg[ds_seg < 0.05] = 0.05
|
||||
s = np.zeros(N, float)
|
||||
s[1:] = np.cumsum(ds_seg)
|
||||
if s[-1] < 0.5: # 총 호길이 < 0.5m면 미분 결과 의미가 약함
|
||||
return np.zeros(N, np.float32), np.zeros(N, np.float32)
|
||||
|
||||
# 2) smoothing (이동평균)
|
||||
x_smooth = smooth_moving_avg(x, smooth_window)
|
||||
y_smooth = smooth_moving_avg(y, smooth_window)
|
||||
|
||||
# 3) 1·2차 도함수(s축 미분)
|
||||
dx_ds = np.gradient(x_smooth, s)
|
||||
dy_ds = np.gradient(y_smooth, s)
|
||||
d2x_ds2 = np.gradient(dx_ds, s)
|
||||
d2y_ds2 = np.gradient(dy_ds, s)
|
||||
|
||||
# 4) yaw = atan2(dy/ds, dx/ds)
|
||||
yaw = np.unwrap(np.arctan2(dy_ds, dx_ds))
|
||||
|
||||
# 5) 곡률 kappa = ...
|
||||
denom = (dx_ds*dx_ds + dy_ds*dy_ds)**1.5
|
||||
denom[denom < 1e-9] = 1e-9
|
||||
kappa = (dx_ds * d2y_ds2 - dy_ds * d2x_ds2) / denom
|
||||
|
||||
# 6) yaw_rate = kappa * v
|
||||
v = np.asarray(v_plan, float)
|
||||
yaw_rate = kappa * v
|
||||
if v0 <= 6.0:
|
||||
# 이동평균으로 미세 요동 감쇄(창 5~7)
|
||||
yaw_rate = smooth_moving_avg(yaw_rate, window=7)
|
||||
|
||||
# 7) 초기 yaw 정렬 (선택)
|
||||
if align_first_yaw is not None:
|
||||
bias = yaw[0] - float(align_first_yaw)
|
||||
yaw = yaw - bias
|
||||
|
||||
# 8) 안정화
|
||||
yaw = np.where(np.isfinite(yaw), yaw, 0.0)
|
||||
yaw_rate = np.where(np.isfinite(yaw_rate), yaw_rate, 0.0)
|
||||
yaw_rate = np.clip(yaw_rate, -abs(clip_rate), abs(clip_rate))
|
||||
|
||||
return yaw.astype(np.float32), yaw_rate.astype(np.float32)
|
||||
|
||||
@@ -1,3 +1,2 @@
|
||||
params_learner
|
||||
paramsd
|
||||
locationd
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
|
||||
|
||||
loc_libs = [messaging, common, 'pthread', 'dl']
|
||||
Import('env', 'rednose')
|
||||
|
||||
# build ekf models
|
||||
rednose_gen_dir = 'models/generated'
|
||||
@@ -14,13 +12,6 @@ pose_ekf = env.RednoseCompileFilter(
|
||||
extra_gen_artifacts=[],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
live_ekf = env.RednoseCompileFilter(
|
||||
target='live',
|
||||
filter_gen_script='models/live_kf.py',
|
||||
output_dir=rednose_gen_dir,
|
||||
extra_gen_artifacts=['live_kf_constants.h'],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
car_ekf = env.RednoseCompileFilter(
|
||||
target='car',
|
||||
filter_gen_script='models/car_kf.py',
|
||||
@@ -28,17 +19,3 @@ car_ekf = env.RednoseCompileFilter(
|
||||
extra_gen_artifacts=[],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
|
||||
# locationd build
|
||||
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
|
||||
|
||||
lenv = env.Clone()
|
||||
# ekf filter libraries need to be linked, even if no symbols are used
|
||||
if arch != "Darwin":
|
||||
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
|
||||
|
||||
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
|
||||
lenv.Depends(locationd, rednose)
|
||||
lenv.Depends(locationd, live_ekf)
|
||||
|
||||
@@ -1,774 +0,0 @@
|
||||
#include "selfdrive/locationd/locationd.h"
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
ExitHandler do_exit;
|
||||
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
|
||||
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
|
||||
const double TRANS_SANITY_CHECK = 200.0; // m/s
|
||||
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
|
||||
const double ALTITUDE_SANITY_CHECK = 10000; // m
|
||||
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
|
||||
const double VALID_TIME_SINCE_RESET = 1.0; // s
|
||||
const double VALID_POS_STD = 50.0; // m
|
||||
const double MAX_RESET_TRACKER = 5.0;
|
||||
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
|
||||
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
|
||||
const double RESET_TRACKER_DECAY = 0.99995;
|
||||
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
|
||||
const double MAX_FILTER_REWIND_TIME = 0.8; // s
|
||||
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
|
||||
|
||||
// TODO: GPS sensor time offsets are empirically calculated
|
||||
// They should be replaced with synced time from a real clock
|
||||
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
|
||||
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
|
||||
const float GPS_POS_STD_THRESHOLD = 50.0;
|
||||
const float GPS_VEL_STD_THRESHOLD = 5.0;
|
||||
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
|
||||
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
|
||||
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
|
||||
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
|
||||
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
|
||||
|
||||
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
|
||||
|
||||
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
|
||||
VectorXd res(floatlist.size());
|
||||
for (int i = 0; i < floatlist.size(); i++) {
|
||||
res[i] = floatlist[i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
static Vector4d quat2vector(const Quaterniond& quat) {
|
||||
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
|
||||
}
|
||||
|
||||
static Quaterniond vector2quat(const VectorXd& vec) {
|
||||
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
|
||||
}
|
||||
|
||||
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
|
||||
meas.setValue(kj::arrayPtr(val.data(), val.size()));
|
||||
meas.setStd(kj::arrayPtr(std.data(), std.size()));
|
||||
meas.setValid(valid);
|
||||
}
|
||||
|
||||
|
||||
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
|
||||
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
|
||||
return ((rot_matrix * cov_in) * rot_matrix.transpose());
|
||||
}
|
||||
|
||||
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
|
||||
// Stds cannot be rotated like values, only covariances can be rotated
|
||||
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
|
||||
}
|
||||
|
||||
Localizer::Localizer(LocalizerGnssSource gnss_source) {
|
||||
this->kf = std::make_unique<LiveKalman>();
|
||||
this->reset_kalman();
|
||||
|
||||
this->calib = Vector3d(0.0, 0.0, 0.0);
|
||||
this->device_from_calib = MatrixXdr::Identity(3, 3);
|
||||
this->calib_from_device = MatrixXdr::Identity(3, 3);
|
||||
|
||||
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
|
||||
this->posenet_stds.push_back(10.0);
|
||||
}
|
||||
|
||||
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
|
||||
this->configure_gnss_source(gnss_source);
|
||||
}
|
||||
|
||||
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
|
||||
VectorXd predicted_state = this->kf->get_x();
|
||||
MatrixXdr predicted_cov = this->kf->get_P();
|
||||
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
|
||||
|
||||
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
|
||||
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
|
||||
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
|
||||
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
|
||||
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
|
||||
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
|
||||
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
|
||||
|
||||
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
|
||||
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
|
||||
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
|
||||
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
|
||||
|
||||
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
|
||||
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
|
||||
|
||||
VectorXd vel_device = device_from_ecef * vel_ecef;
|
||||
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
|
||||
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
|
||||
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
|
||||
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
|
||||
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
|
||||
H_input << device_from_ecef_eul, vel_ecef;
|
||||
MatrixXdr HH = this->kf->H(H_input);
|
||||
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
|
||||
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
|
||||
|
||||
VectorXd vel_calib = this->calib_from_device * vel_device;
|
||||
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
|
||||
|
||||
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
|
||||
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
|
||||
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
|
||||
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
|
||||
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
|
||||
|
||||
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
|
||||
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
|
||||
|
||||
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
|
||||
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
|
||||
|
||||
Vector3d nans = Vector3d(NAN, NAN, NAN);
|
||||
|
||||
// TODO fill in NED and Calibrated stds
|
||||
// write measurements to msg
|
||||
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
|
||||
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
|
||||
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
|
||||
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
|
||||
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
|
||||
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
|
||||
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
|
||||
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
|
||||
if (DEBUG) {
|
||||
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
|
||||
}
|
||||
|
||||
double old_mean = 0.0, new_mean = 0.0;
|
||||
int i = 0;
|
||||
for (double x : this->posenet_stds) {
|
||||
if (i < POSENET_STD_HIST_HALF) {
|
||||
old_mean += x;
|
||||
} else {
|
||||
new_mean += x;
|
||||
}
|
||||
i++;
|
||||
}
|
||||
old_mean /= POSENET_STD_HIST_HALF;
|
||||
new_mean /= POSENET_STD_HIST_HALF;
|
||||
// experimentally found these values, no false positives in 20k minutes of driving
|
||||
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
|
||||
|
||||
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
|
||||
fix.setDeviceStable(!this->device_fell);
|
||||
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
|
||||
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
|
||||
this->device_fell = false;
|
||||
|
||||
//fix.setGpsWeek(this->time.week);
|
||||
//fix.setGpsTimeOfWeek(this->time.tow);
|
||||
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
|
||||
|
||||
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
|
||||
fix.setTimeSinceReset(time_since_reset);
|
||||
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
|
||||
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
|
||||
} else {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
|
||||
}
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_position_geodetic() {
|
||||
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
|
||||
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
|
||||
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_state() {
|
||||
return this->kf->get_x();
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_stdev() {
|
||||
return this->kf->get_P().diagonal().array().sqrt();
|
||||
}
|
||||
|
||||
bool Localizer::are_inputs_ok() {
|
||||
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
|
||||
}
|
||||
|
||||
void Localizer::observation_timings_invalid_reset(){
|
||||
this->observation_timings_invalid = false;
|
||||
}
|
||||
|
||||
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
|
||||
// TODO does not yet account for double sensor readings in the log
|
||||
|
||||
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
|
||||
if (log.getTimestamp() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = 1e-9 * log.getTimestamp();
|
||||
|
||||
// sensor time and log time should be close
|
||||
if (std::abs(current_time - sensor_time) > 0.1) {
|
||||
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
} else if (!this->is_timestamp_valid(sensor_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO: handle messages from two IMUs at the same time
|
||||
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Gyro Uncalibrated
|
||||
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
|
||||
auto v = log.getGyroUncalibrated().getV();
|
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]);
|
||||
|
||||
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
|
||||
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
|
||||
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
|
||||
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
|
||||
|
||||
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
|
||||
this->observation_values_invalid["gyroscope"] *= DECAY;
|
||||
} else {
|
||||
this->observation_values_invalid["gyroscope"] += 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Accelerometer
|
||||
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
|
||||
auto v = log.getAcceleration().getV();
|
||||
|
||||
// TODO: reduce false positives and re-enable this check
|
||||
// check if device fell, estimate 10 for g
|
||||
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
|
||||
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
|
||||
|
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]);
|
||||
if (meas.norm() < ACCEL_SANITY_CHECK) {
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
|
||||
this->observation_values_invalid["accelerometer"] *= DECAY;
|
||||
} else {
|
||||
this->observation_values_invalid["accelerometer"] += 1.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::input_fake_gps_observations(double current_time) {
|
||||
// This is done to make sure that the error estimate of the position does not blow up
|
||||
// when the filter is in no-gps mode
|
||||
// Steps : first predict -> observe current obs with reasonable STD
|
||||
this->kf->predict(current_time);
|
||||
|
||||
VectorXd current_x = this->kf->get_x();
|
||||
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
|
||||
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
|
||||
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
|
||||
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
|
||||
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
|
||||
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
|
||||
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
|
||||
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
|
||||
|
||||
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
|
||||
//this->gps_valid = false;
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = current_time - sensor_time_offset;
|
||||
|
||||
// Process message
|
||||
//this->gps_valid = true;
|
||||
this->gps_mode = true;
|
||||
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
|
||||
this->converter = std::make_unique<LocalCoord>(geodetic);
|
||||
|
||||
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
|
||||
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
|
||||
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
|
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
|
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
|
||||
|
||||
this->unix_timestamp_millis = log.getUnixTimestampMillis();
|
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
|
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
|
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
|
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
|
||||
for (int i = 0; i < orientation_error.size(); i++) {
|
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
|
||||
if (orientation_error(i) < 0.0) {
|
||||
orientation_error(i) += 2.0 * M_PI;
|
||||
}
|
||||
orientation_error(i) -= M_PI;
|
||||
}
|
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
|
||||
|
||||
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
|
||||
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
|
||||
} else if (gps_est_error > 100.0) {
|
||||
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
}
|
||||
|
||||
this->last_gps_msg = sensor_time;
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
|
||||
|
||||
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = log.getMeasTime() * 1e-9;
|
||||
sensor_time -= this->gps_time_offset;
|
||||
|
||||
auto ecef_pos_v = log.getPositionECEF().getValue();
|
||||
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
|
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
|
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
|
||||
|
||||
auto ecef_vel_v = log.getVelocityECEF().getValue();
|
||||
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
|
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
|
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
|
||||
|
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
|
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
|
||||
|
||||
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
|
||||
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
|
||||
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
|
||||
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
|
||||
|
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
|
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
|
||||
for (int i = 0; i < orientation_error.size(); i++) {
|
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
|
||||
if (orientation_error(i) < 0.0) {
|
||||
orientation_error(i) += 2.0 * M_PI;
|
||||
}
|
||||
orientation_error(i) -= M_PI;
|
||||
}
|
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
|
||||
|
||||
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
// prevent jumping gnss measurements (covered lots, standstill...)
|
||||
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
|
||||
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
|
||||
orientation_reset &= !this->standstill;
|
||||
if (orientation_reset) {
|
||||
this->orientation_reset_count++;
|
||||
} else {
|
||||
this->orientation_reset_count = 0;
|
||||
}
|
||||
|
||||
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
|
||||
// always reset on first gps message and if the location is off but the accuracy is high
|
||||
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
|
||||
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
|
||||
this->orientation_reset_count = 0;
|
||||
}
|
||||
|
||||
this->gps_mode = true;
|
||||
this->last_gps_msg = sensor_time;
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
|
||||
this->car_speed = std::abs(log.getVEgo());
|
||||
this->standstill = log.getStandstill();
|
||||
if (this->standstill) {
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
|
||||
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
|
||||
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
|
||||
|
||||
if (!this->is_timestamp_valid(current_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
|
||||
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
|
||||
|
||||
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
this->posenet_stds.pop_front();
|
||||
this->posenet_stds.push_back(trans_calib_std[0]);
|
||||
|
||||
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
|
||||
trans_calib_std *= 10.0;
|
||||
rot_calib_std *= 10.0;
|
||||
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
|
||||
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
|
||||
{ rot_device }, { rot_device_cov });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
|
||||
{ trans_device }, { trans_device_cov });
|
||||
this->observation_values_invalid["cameraOdometry"] *= DECAY;
|
||||
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
|
||||
}
|
||||
|
||||
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
|
||||
if (!this->is_timestamp_valid(current_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (log.getRpyCalib().size() > 0) {
|
||||
auto live_calib = floatlist2vector(log.getRpyCalib());
|
||||
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["liveCalibration"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
this->calib = live_calib;
|
||||
this->device_from_calib = euler2rot(this->calib);
|
||||
this->calib_from_device = this->device_from_calib.transpose();
|
||||
this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
|
||||
this->observation_values_invalid["liveCalibration"] *= DECAY;
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time) {
|
||||
const VectorXd &init_x = this->kf->get_initial_x();
|
||||
const MatrixXdr &init_P = this->kf->get_initial_P();
|
||||
this->reset_kalman(current_time, init_x, init_P);
|
||||
}
|
||||
|
||||
void Localizer::finite_check(double current_time) {
|
||||
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
|
||||
if (!all_finite) {
|
||||
LOGE("Non-finite values detected, kalman reset");
|
||||
this->reset_kalman(current_time);
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::time_check(double current_time) {
|
||||
if (std::isnan(this->last_reset_time)) {
|
||||
this->last_reset_time = current_time;
|
||||
}
|
||||
if (std::isnan(this->first_valid_log_time)) {
|
||||
this->first_valid_log_time = current_time;
|
||||
}
|
||||
double filter_time = this->kf->get_filter_time();
|
||||
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
|
||||
if (big_time_gap) {
|
||||
LOGE("Time gap of over 10s detected, kalman reset");
|
||||
this->reset_kalman(current_time);
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::update_reset_tracker() {
|
||||
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
|
||||
if (this->is_gps_ok()) {
|
||||
this->reset_tracker *= RESET_TRACKER_DECAY;
|
||||
} else {
|
||||
this->reset_tracker = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
|
||||
// too nonlinear to init on completely wrong
|
||||
VectorXd current_x = this->kf->get_x();
|
||||
MatrixXdr current_P = this->kf->get_P();
|
||||
MatrixXdr init_P = this->kf->get_initial_P();
|
||||
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
|
||||
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
|
||||
|
||||
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
|
||||
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
|
||||
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
|
||||
|
||||
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
|
||||
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
|
||||
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
|
||||
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
|
||||
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
|
||||
|
||||
this->reset_kalman(current_time, current_x, init_P);
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
|
||||
this->kf->init_state(init_x, init_P, current_time);
|
||||
this->last_reset_time = current_time;
|
||||
this->reset_tracker += 1.0;
|
||||
}
|
||||
|
||||
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
|
||||
AlignedBuffer aligned_buf;
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
this->handle_msg(event);
|
||||
}
|
||||
|
||||
void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
||||
double t = log.getLogMonoTime() * 1e-9;
|
||||
this->time_check(t);
|
||||
if (log.isAccelerometer()) {
|
||||
this->handle_sensor(t, log.getAccelerometer());
|
||||
} else if (log.isGyroscope()) {
|
||||
this->handle_sensor(t, log.getGyroscope());
|
||||
} else if (log.isGpsLocation()) {
|
||||
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
|
||||
} else if (log.isGpsLocationExternal()) {
|
||||
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
|
||||
//} else if (log.isGnssMeasurements()) {
|
||||
// this->handle_gnss(t, log.getGnssMeasurements());
|
||||
} else if (log.isCarState()) {
|
||||
this->handle_car_state(t, log.getCarState());
|
||||
} else if (log.isCameraOdometry()) {
|
||||
this->handle_cam_odo(t, log.getCameraOdometry());
|
||||
} else if (log.isLiveCalibration()) {
|
||||
this->handle_live_calib(t, log.getLiveCalibration());
|
||||
}
|
||||
this->finite_check();
|
||||
this->update_reset_tracker();
|
||||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
|
||||
bool sensorsOK, bool gpsOK, bool msgValid) {
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setValid(msgValid);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
bool Localizer::is_gps_ok() {
|
||||
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
|
||||
}
|
||||
|
||||
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
|
||||
for (auto &kv : critical_services){
|
||||
if (kv.second >= INPUT_INVALID_THRESHOLD){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Localizer::is_timestamp_valid(double current_time) {
|
||||
double filter_time = this->kf->get_filter_time();
|
||||
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
|
||||
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Localizer::determine_gps_mode(double current_time) {
|
||||
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
|
||||
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
|
||||
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
|
||||
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
|
||||
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
|
||||
if (this->gps_mode){
|
||||
this->gps_mode = false;
|
||||
this->reset_kalman(current_time);
|
||||
} else {
|
||||
this->input_fake_gps_observations(current_time);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
|
||||
this->gnss_source = source;
|
||||
if (source == LocalizerGnssSource::UBLOX) {
|
||||
this->gps_std_factor = 10.0;
|
||||
this->gps_variance_factor = 1.0;
|
||||
this->gps_vertical_variance_factor = 1.0;
|
||||
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
|
||||
} else {
|
||||
this->gps_std_factor = 2.0;
|
||||
this->gps_variance_factor = 0.0;
|
||||
this->gps_vertical_variance_factor = 3.0;
|
||||
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
|
||||
}
|
||||
}
|
||||
|
||||
int Localizer::locationd_thread() {
|
||||
Params params;
|
||||
LocalizerGnssSource source;
|
||||
const char* gps_location_socket;
|
||||
if (params.getBool("UbloxAvailable")) {
|
||||
source = LocalizerGnssSource::UBLOX;
|
||||
gps_location_socket = "gpsLocationExternal";
|
||||
} else {
|
||||
source = LocalizerGnssSource::QCOM;
|
||||
gps_location_socket = "gpsLocation";
|
||||
}
|
||||
|
||||
this->configure_gnss_source(source);
|
||||
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
|
||||
"carState", "accelerometer", "gyroscope"};
|
||||
|
||||
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
|
||||
PubMaster pm({"liveLocationKalman"});
|
||||
|
||||
uint64_t cnt = 0;
|
||||
bool filterInitialized = false;
|
||||
const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
|
||||
for (std::string service : critical_input_services) {
|
||||
this->observation_values_invalid.insert({service, 0.0});
|
||||
}
|
||||
|
||||
bool ignore_gps = true;
|
||||
while (!do_exit) {
|
||||
sm.update();
|
||||
if (filterInitialized){
|
||||
this->observation_timings_invalid_reset();
|
||||
for (const char* service : service_list) {
|
||||
if (sm.updated(service) && sm.valid(service)){
|
||||
const cereal::Event::Reader log = sm[service];
|
||||
this->handle_msg(log);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
//filterInitialized = sm.allAliveAndValid();
|
||||
bool allValid = true;
|
||||
for (const char* service : service_list) {
|
||||
if (service != gps_location_socket && !sm.valid(service)) {
|
||||
allValid = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
filterInitialized = allValid;
|
||||
}
|
||||
|
||||
const char* trigger_msg = "cameraOdometry";
|
||||
if (sm.updated(trigger_msg)) {
|
||||
bool inputsOK = sm.allValid() && this->are_inputs_ok();
|
||||
if (ignore_gps) {
|
||||
inputsOK = this->are_inputs_ok();
|
||||
}
|
||||
bool gpsOK = this->is_gps_ok();
|
||||
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
|
||||
|
||||
/*
|
||||
if (!sm.allValid()) {
|
||||
for (const char* service : service_list) {
|
||||
if (!sm.valid(service)) {
|
||||
printf("Service %s is INVALID! (Alive: %d)\n", service, sm.alive(service));
|
||||
}
|
||||
}
|
||||
}
|
||||
printf("InputsOK: %d, SensorsOK: %d, GPSOK: %d, FilterInitialized: %d\n", inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
*/
|
||||
|
||||
// Log time to first fix
|
||||
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
|
||||
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
|
||||
}
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
//ignore_gps = false;
|
||||
VectorXd posGeo = this->get_position_geodetic();
|
||||
std::string lastGPSPosJSON = util::string_format(
|
||||
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
|
||||
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
|
||||
}
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main() {
|
||||
util::set_realtime_priority(5);
|
||||
|
||||
Localizer localizer;
|
||||
return localizer.locationd_thread();
|
||||
}
|
||||
@@ -1,100 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <deque>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/transformations/coordinates.hpp"
|
||||
#include "common/transformations/orientation.hpp"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#include "system/sensord/sensors/constants.h"
|
||||
#define VISION_DECIMATION 2
|
||||
#define SENSOR_DECIMATION 10
|
||||
#include "selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
#define POSENET_STD_HIST_HALF 20
|
||||
|
||||
enum LocalizerGnssSource {
|
||||
UBLOX, QCOM
|
||||
};
|
||||
|
||||
class Localizer {
|
||||
public:
|
||||
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
|
||||
|
||||
int locationd_thread();
|
||||
|
||||
void reset_kalman(double current_time = NAN);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
|
||||
void finite_check(double current_time = NAN);
|
||||
void time_check(double current_time = NAN);
|
||||
void update_reset_tracker();
|
||||
bool is_gps_ok();
|
||||
bool critical_services_valid(const std::map<std::string, double> &critical_services);
|
||||
bool is_timestamp_valid(double current_time);
|
||||
void determine_gps_mode(double current_time);
|
||||
bool are_inputs_ok();
|
||||
void observation_timings_invalid_reset();
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
Eigen::VectorXd get_state();
|
||||
Eigen::VectorXd get_stdev();
|
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size);
|
||||
void handle_msg(const cereal::Event::Reader& log);
|
||||
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
|
||||
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
|
||||
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
|
||||
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
|
||||
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
|
||||
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
|
||||
|
||||
void input_fake_gps_observations(double current_time);
|
||||
|
||||
private:
|
||||
std::unique_ptr<LiveKalman> kf;
|
||||
|
||||
Eigen::VectorXd calib;
|
||||
MatrixXdr device_from_calib;
|
||||
MatrixXdr calib_from_device;
|
||||
bool calibrated = false;
|
||||
|
||||
double car_speed = 0.0;
|
||||
double last_reset_time = NAN;
|
||||
std::deque<double> posenet_stds;
|
||||
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
int64_t unix_timestamp_millis = 0;
|
||||
double reset_tracker = 0.0;
|
||||
bool device_fell = false;
|
||||
bool gps_mode = false;
|
||||
double first_valid_log_time = NAN;
|
||||
double ttff = NAN;
|
||||
double last_gps_msg = 0;
|
||||
LocalizerGnssSource gnss_source;
|
||||
bool observation_timings_invalid = false;
|
||||
std::map<std::string, double> observation_values_invalid;
|
||||
bool standstill = true;
|
||||
int32_t orientation_reset_count = 0;
|
||||
float gps_std_factor;
|
||||
float gps_variance_factor;
|
||||
float gps_vertical_variance_factor;
|
||||
double gps_time_offset;
|
||||
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
|
||||
|
||||
void configure_gnss_source(const LocalizerGnssSource &source);
|
||||
};
|
||||
@@ -5,7 +5,7 @@ from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
@@ -1,122 +0,0 @@
|
||||
#include "selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
|
||||
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
|
||||
}
|
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
|
||||
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res;
|
||||
for (const Eigen::VectorXd &vec : vec_vec) {
|
||||
res.push_back(get_mapvec(vec));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
|
||||
std::vector<Eigen::Map<MatrixXdr>> res;
|
||||
for (const MatrixXdr &mat : mat_vec) {
|
||||
res.push_back(get_mapmat(mat));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
LiveKalman::LiveKalman() {
|
||||
this->dim_state = live_initial_x.rows();
|
||||
this->dim_state_err = live_initial_P_diag.rows();
|
||||
|
||||
this->initial_x = live_initial_x;
|
||||
this->initial_P = live_initial_P_diag.asDiagonal();
|
||||
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
|
||||
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
|
||||
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
|
||||
this->Q = live_Q_diag.asDiagonal();
|
||||
for (auto& pair : live_obs_noise_diag) {
|
||||
this->obs_noise[pair.first] = pair.second.asDiagonal();
|
||||
}
|
||||
|
||||
// init filter
|
||||
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
|
||||
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
|
||||
std::vector<int>{3}, std::vector<std::string>(), 0.8);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
|
||||
MatrixXdr covs = covs_diag.asDiagonal();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
|
||||
MatrixXdr covs = this->filter->covs();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
VectorXd LiveKalman::get_x() {
|
||||
return this->filter->state();
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::get_P() {
|
||||
return this->filter->covs();
|
||||
}
|
||||
|
||||
double LiveKalman::get_filter_time() {
|
||||
return this->filter->get_filter_time();
|
||||
}
|
||||
|
||||
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
|
||||
std::vector<MatrixXdr> R;
|
||||
for (int i = 0; i < n; i++) {
|
||||
R.push_back(this->obs_noise[kind]);
|
||||
}
|
||||
return R;
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
|
||||
std::optional<Estimate> r;
|
||||
if (R.size() == 0) {
|
||||
R = this->get_R(kind, meas.size());
|
||||
}
|
||||
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
|
||||
return r;
|
||||
}
|
||||
|
||||
void LiveKalman::predict(double t) {
|
||||
this->filter->predict(t);
|
||||
}
|
||||
|
||||
const Eigen::VectorXd &LiveKalman::get_initial_x() {
|
||||
return this->initial_x;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_initial_P() {
|
||||
return this->initial_P;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
|
||||
return this->fake_gps_pos_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
|
||||
return this->fake_gps_vel_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
|
||||
return this->reset_orientation_P;
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::H(const VectorXd &in) {
|
||||
assert(in.size() == 6);
|
||||
Matrix<double, 3, 6, Eigen::RowMajor> res;
|
||||
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
|
||||
return res;
|
||||
}
|
||||
@@ -1,66 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <eigen3/Eigen/Core>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "generated/live_kf_constants.h"
|
||||
#include "rednose/helpers/ekf_sym.h"
|
||||
|
||||
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
using namespace EKFS;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
|
||||
|
||||
class LiveKalman {
|
||||
public:
|
||||
LiveKalman();
|
||||
|
||||
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, double filter_time);
|
||||
|
||||
Eigen::VectorXd get_x();
|
||||
MatrixXdr get_P();
|
||||
double get_filter_time();
|
||||
std::vector<MatrixXdr> get_R(int kind, int n);
|
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
|
||||
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
|
||||
void predict(double t);
|
||||
|
||||
const Eigen::VectorXd &get_initial_x();
|
||||
const MatrixXdr &get_initial_P();
|
||||
const MatrixXdr &get_fake_gps_pos_cov();
|
||||
const MatrixXdr &get_fake_gps_vel_cov();
|
||||
const MatrixXdr &get_reset_orientation_P();
|
||||
|
||||
MatrixXdr H(const Eigen::VectorXd &in);
|
||||
|
||||
private:
|
||||
std::string name = "live";
|
||||
|
||||
std::shared_ptr<EKFSym> filter;
|
||||
|
||||
int dim_state;
|
||||
int dim_state_err;
|
||||
|
||||
Eigen::VectorXd initial_x;
|
||||
MatrixXdr initial_P;
|
||||
MatrixXdr fake_gps_pos_cov;
|
||||
MatrixXdr fake_gps_vel_cov;
|
||||
MatrixXdr reset_orientation_P;
|
||||
MatrixXdr Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise;
|
||||
};
|
||||
@@ -1,242 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
import sympy as sp
|
||||
import inspect
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
|
||||
def numpy2eigenstring(arr):
|
||||
assert(len(arr.shape) == 1)
|
||||
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
|
||||
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
|
||||
|
||||
|
||||
class States:
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
|
||||
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
|
||||
|
||||
# Error-state has different slices because it is an ESKF
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ACCELERATION_ERR = slice(15, 18)
|
||||
ACC_BIAS_ERR = slice(18, 21)
|
||||
|
||||
|
||||
class LiveKalman:
|
||||
name = 'live'
|
||||
|
||||
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
|
||||
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10**2, 10**2, 10**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
1**2, 1**2, 1**2,
|
||||
100**2, 100**2, 100**2,
|
||||
0.01**2, 0.01**2, 0.01**2])
|
||||
|
||||
# state covariance when resetting midway in a segment
|
||||
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
|
||||
|
||||
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
|
||||
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
|
||||
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
|
||||
|
||||
# process noise
|
||||
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
|
||||
0.001**2, 0.001**2, 0.001**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
0.005**2, 0.005**2, 0.005**2])
|
||||
|
||||
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
|
||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
|
||||
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
|
||||
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x, y, z = state[States.ECEF_POS, :]
|
||||
q = state[States.ECEF_ORIENTATION, :]
|
||||
v = state[States.ECEF_VELOCITY, :]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY, :]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
|
||||
acceleration = state[States.ACCELERATION, :]
|
||||
acc_bias = state[States.ACC_BIAS, :]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS, :] = v
|
||||
state_dot[States.ECEF_ORIENTATION, :] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR, :]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR, :] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt * state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
|
||||
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
|
||||
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
|
||||
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
|
||||
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
|
||||
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
|
||||
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
h_gyro_sym = sp.Matrix([
|
||||
vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
|
||||
h_acc_sym = (gravity + acceleration + acc_bias)
|
||||
h_acc_stationary_sym = acceleration
|
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_vel_sym = sp.Matrix([vx, vy, vz])
|
||||
h_orientation_sym = q
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
|
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_vel_sym, ObservationKind.ECEF_VEL, None],
|
||||
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
|
||||
|
||||
# this returns a sympy routine for the jacobian of the observation function of the local vel
|
||||
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
|
||||
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
|
||||
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
|
||||
|
||||
# write constants to extra header file for use in cpp
|
||||
live_kf_header = "#pragma once\n\n"
|
||||
live_kf_header += "#include <unordered_map>\n"
|
||||
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
|
||||
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
|
||||
assert(slc.step is None) # unsupported
|
||||
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
|
||||
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
|
||||
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
|
||||
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
|
||||
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
|
||||
for kind, noise in LiveKalman.obs_noise_diag.items():
|
||||
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
|
||||
live_kf_header += "};\n\n"
|
||||
|
||||
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
LiveKalman.generate_code(generated_dir)
|
||||
@@ -15,6 +15,8 @@ from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
|
||||
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
|
||||
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
|
||||
ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
|
||||
@@ -245,7 +247,9 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b
|
||||
if debug and len(initial_filter_std) != 0:
|
||||
p_initial = np.diag(initial_filter_std)
|
||||
|
||||
steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
|
||||
#steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
|
||||
#steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetDeg
|
||||
steer_ratio, stiffness_factor = lp.steerRatio, lp.stiffnessFactor
|
||||
retrieve_success = True
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Failed to retrieve initial values: {e}")
|
||||
@@ -269,7 +273,8 @@ def main():
|
||||
REPLAY = bool(int(os.getenv("REPLAY", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveParameters'])
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'liveLocationKalman'], poll='livePose')
|
||||
gps_location_service = get_gps_location_service(Params())
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', gps_location_service], poll='livePose', ignore_alive=[gps_location_service], ignore_valid=[gps_location_service])
|
||||
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
@@ -289,12 +294,12 @@ def main():
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
learner.handle_log(t, which, sm[which])
|
||||
|
||||
if sm.updated['liveLocationKalman']:
|
||||
location = sm['liveLocationKalman']
|
||||
if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
|
||||
bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
lat = location.positionGeodetic.value[0]
|
||||
lon = location.positionGeodetic.value[1]
|
||||
if sm.updated[gps_location_service]:
|
||||
gps = sm[gps_location_service]
|
||||
if gps.hasFix:
|
||||
bearing = gps.bearingDeg
|
||||
lat = gps.latitude
|
||||
lon = gps.longitude
|
||||
params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
|
||||
|
||||
|
||||
|
||||
@@ -5,7 +5,7 @@ from collections import deque, defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car, log
|
||||
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
import os
|
||||
import glob
|
||||
|
||||
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
|
||||
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations')
|
||||
lenv = env.Clone()
|
||||
lenvCython = envCython.Clone()
|
||||
|
||||
libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'kj', 'pthread']
|
||||
libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread']
|
||||
frameworks = []
|
||||
|
||||
common_src = [
|
||||
@@ -32,7 +32,7 @@ lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LI
|
||||
tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
|
||||
|
||||
# Get model metadata
|
||||
for model_name in ['driving_vision', 'driving_policy']:
|
||||
for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
|
||||
fn = File(f"models/{model_name}").abspath
|
||||
script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)]
|
||||
cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
|
||||
@@ -50,9 +50,9 @@ def tg_compile(flags, model_name):
|
||||
# Compile small models
|
||||
for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
|
||||
flags = {
|
||||
'larch64': 'DEV=QCOM',
|
||||
'Darwin': 'DEV=CPU IMAGE=0',
|
||||
}.get(arch, 'DEV=LLVM IMAGE=0')
|
||||
'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 IMAGE=2 JIT_BATCH_SIZE=0',
|
||||
'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")}', # tinygrad calls brew which needs a $HOME in the env
|
||||
}.get(arch, 'DEV=CPU CPU_LLVM=1')
|
||||
tg_compile(flags, model_name)
|
||||
|
||||
# Compile BIG model if USB GPU is available
|
||||
|
||||
@@ -1,14 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.dtype import dtypes
|
||||
if TICI:
|
||||
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
os.environ['QCOM'] = '1'
|
||||
else:
|
||||
os.environ['LLVM'] = '1'
|
||||
import math
|
||||
import time
|
||||
import pickle
|
||||
import ctypes
|
||||
@@ -21,48 +16,16 @@ from cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
|
||||
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
|
||||
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
|
||||
from openpilot.system import sentry
|
||||
|
||||
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
|
||||
CALIB_LEN = 3
|
||||
FEATURE_LEN = 512
|
||||
OUTPUT_SIZE = 84 + FEATURE_LEN
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
|
||||
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
|
||||
|
||||
|
||||
class DriverStateResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("face_orientation", ctypes.c_float*3),
|
||||
("face_position", ctypes.c_float*3),
|
||||
("face_orientation_std", ctypes.c_float*3),
|
||||
("face_position_std", ctypes.c_float*3),
|
||||
("face_prob", ctypes.c_float),
|
||||
("_unused_a", ctypes.c_float*8),
|
||||
("left_eye_prob", ctypes.c_float),
|
||||
("_unused_b", ctypes.c_float*8),
|
||||
("right_eye_prob", ctypes.c_float),
|
||||
("left_blink_prob", ctypes.c_float),
|
||||
("right_blink_prob", ctypes.c_float),
|
||||
("sunglasses_prob", ctypes.c_float),
|
||||
("occluded_prob", ctypes.c_float),
|
||||
("ready_prob", ctypes.c_float*4),
|
||||
("not_ready_prob", ctypes.c_float*2)]
|
||||
|
||||
|
||||
class DMonitoringModelResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("driver_state_lhd", DriverStateResult),
|
||||
("driver_state_rhd", DriverStateResult),
|
||||
("poor_vision_prob", ctypes.c_float),
|
||||
("wheel_on_right_prob", ctypes.c_float),
|
||||
("features", ctypes.c_float*FEATURE_LEN)]
|
||||
METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
|
||||
|
||||
|
||||
class ModelState:
|
||||
@@ -70,11 +33,14 @@ class ModelState:
|
||||
output: np.ndarray
|
||||
|
||||
def __init__(self, cl_ctx):
|
||||
assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
|
||||
with open(METADATA_PATH, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
self.input_shapes = model_metadata['input_shapes']
|
||||
self.output_slices = model_metadata['output_slices']
|
||||
|
||||
self.frame = MonitoringModelFrame(cl_ctx)
|
||||
self.numpy_inputs = {
|
||||
'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
|
||||
'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
|
||||
}
|
||||
|
||||
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
|
||||
@@ -90,45 +56,53 @@ class ModelState:
|
||||
if TICI:
|
||||
# The imgs tensors are backed by opencl memory, only need init once
|
||||
if 'input_img' not in self.tensor_inputs:
|
||||
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, (1, MODEL_WIDTH*MODEL_HEIGHT), dtype=dtypes.uint8)
|
||||
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8)
|
||||
else:
|
||||
self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT)), dtype=dtypes.uint8).realize()
|
||||
self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize()
|
||||
|
||||
|
||||
output = self.model_run(**self.tensor_inputs).numpy().flatten()
|
||||
output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
|
||||
|
||||
t2 = time.perf_counter()
|
||||
return output, t2 - t1
|
||||
|
||||
def slice_outputs(model_outputs, output_slices):
|
||||
return {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
|
||||
|
||||
def fill_driver_state(msg, ds_result: DriverStateResult):
|
||||
msg.faceOrientation = list(ds_result.face_orientation)
|
||||
msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
|
||||
msg.facePosition = list(ds_result.face_position[:2])
|
||||
msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
|
||||
msg.faceProb = float(sigmoid(ds_result.face_prob))
|
||||
msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
|
||||
msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
|
||||
msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
|
||||
msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
|
||||
msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
|
||||
msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
|
||||
msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
|
||||
msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
|
||||
def parse_model_output(model_output):
|
||||
parsed = {}
|
||||
parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
|
||||
for ds_suffix in ['lhd', 'rhd']:
|
||||
face_descs = model_output[f'face_descs_{ds_suffix}']
|
||||
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
return parsed
|
||||
|
||||
def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
|
||||
msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
|
||||
msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
|
||||
msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
|
||||
msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float):
|
||||
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
msg = messaging.new_message('driverStateV2', valid=True)
|
||||
ds = msg.driverStateV2
|
||||
ds.frameId = frame_id
|
||||
ds.modelExecutionTime = execution_time
|
||||
ds.gpuExecutionTime = gpu_execution_time
|
||||
ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
|
||||
ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
|
||||
ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
|
||||
fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
|
||||
fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
|
||||
ds.modelExecutionTime = exec_time
|
||||
ds.gpuExecutionTime = gpu_exec_time
|
||||
ds.rawPredictions = model_output['raw_pred']
|
||||
ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
|
||||
fill_driver_data(ds.leftDriverData, model_output, 'lhd')
|
||||
fill_driver_data(ds.rightDriverData, model_output, 'rhd')
|
||||
return msg
|
||||
|
||||
|
||||
@@ -153,7 +127,7 @@ def main():
|
||||
sm = SubMaster(["liveCalibration"])
|
||||
pm = PubMaster(["driverStateV2"])
|
||||
|
||||
calib = np.zeros(CALIB_LEN, dtype=np.float32)
|
||||
calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
|
||||
model_transform = None
|
||||
|
||||
while True:
|
||||
@@ -172,8 +146,12 @@ def main():
|
||||
t1 = time.perf_counter()
|
||||
model_output, gpu_execution_time = model.run(buf, calib, model_transform)
|
||||
t2 = time.perf_counter()
|
||||
|
||||
pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time))
|
||||
raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
|
||||
model_output = slice_outputs(model_output, model.output_slices)
|
||||
model_output = parse_model_output(model_output)
|
||||
model_output['raw_pred'] = raw_pred
|
||||
msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
|
||||
pm.send("driverStateV2", msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
import os
|
||||
import capnp
|
||||
import numpy as np
|
||||
import math
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
|
||||
|
||||
@@ -102,21 +103,42 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
LINE_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
LINE_T_IDXS[0] = 0.0
|
||||
plan_x = net_output_data['plan'][0, :, Plan.POSITION][:, 0].tolist()
|
||||
Tmax = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
|
||||
for xidx in range(1, ModelConstants.IDX_N):
|
||||
tidx = 0
|
||||
# increment tidx until we find an element that's further away than the current xidx
|
||||
while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx + 1] < ModelConstants.X_IDXS[xidx]:
|
||||
tidx += 1
|
||||
if tidx == ModelConstants.IDX_N - 1:
|
||||
# if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
|
||||
LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
|
||||
break
|
||||
for k in range(xidx, ModelConstants.IDX_N):
|
||||
LINE_T_IDXS[k] = Tmax
|
||||
break
|
||||
# interpolate to find `t` for the current xidx
|
||||
current_x_val = plan_x[tidx]
|
||||
next_x_val = plan_x[tidx + 1]
|
||||
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
|
||||
next_x_val - current_x_val) > 1e-9 else float('nan')
|
||||
LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
|
||||
|
||||
dx = next_x_val - current_x_val
|
||||
if dx <= 1e-9:
|
||||
LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[tidx]
|
||||
else:
|
||||
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / dx
|
||||
if p < 0.0: p = 0.0
|
||||
elif p > 1.0: p = 1.0
|
||||
LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1.0 - p) * ModelConstants.T_IDXS[tidx]
|
||||
|
||||
#p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
|
||||
# next_x_val - current_x_val) > 1e-9 else float('nan')
|
||||
#LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
|
||||
|
||||
LINE_T_IDXS = [float(Tmax if math.isnan(float(v)) else float(v)) for v in LINE_T_IDXS]
|
||||
|
||||
# 비내림(monotonic non-decreasing) 보정 (순수 파이썬, numpy 불사용)
|
||||
running = LINE_T_IDXS[0]
|
||||
for i in range(1, len(LINE_T_IDXS)):
|
||||
if LINE_T_IDXS[i] < running:
|
||||
LINE_T_IDXS[i] = running
|
||||
else:
|
||||
running = LINE_T_IDXS[i]
|
||||
|
||||
# lane lines
|
||||
modelV2.init('laneLines', 4)
|
||||
|
||||
@@ -62,6 +62,5 @@ Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in
|
||||
* (deprecated) distracted probabilities: 2
|
||||
* using phone probability: 1
|
||||
* distracted probability: 1
|
||||
* common outputs 2
|
||||
* poor camera vision probability: 1
|
||||
* common outputs 1
|
||||
* left hand drive probability: 1
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
fa69be01-b430-4504-9d72-7dcb058eb6dd
|
||||
d9fb22d1c4fa3ca3d201dbc8edf1d0f0918e53e6
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -2400,9 +2400,8 @@ public:
|
||||
if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
|
||||
strcpy(driving_mode_str_last, driving_mode_str);
|
||||
|
||||
auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
|
||||
bool is_gps_valid = sm.valid("liveLocationKalman") && locationd.getGpsOK();
|
||||
if (is_gps_valid) {
|
||||
auto gps = (s->ublox_avaliable) ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation();
|
||||
if (gps.getHasFix()) {
|
||||
ui_draw_text(s, dx, dy - 45, "GPS", 30, COLOR_GREEN, BOLD);
|
||||
}
|
||||
|
||||
|
||||
+110
-5
@@ -4,6 +4,9 @@
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include <QDebug>
|
||||
#include <QJsonDocument>
|
||||
#include <QJsonObject>
|
||||
#include <QJsonArray>
|
||||
|
||||
#include "selfdrive/ui/qt/maps/map_helpers.h"
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
@@ -13,7 +16,7 @@
|
||||
const int INTERACTION_TIMEOUT = 100;
|
||||
|
||||
//const float MAX_ZOOM = 20;// 17;
|
||||
const float MIN_ZOOM = 14;
|
||||
const float MIN_ZOOM = 15; // 14;
|
||||
const float MAX_PITCH = 50;
|
||||
const float MIN_PITCH = 0;
|
||||
const float MAP_SCALE = 2;
|
||||
@@ -147,6 +150,40 @@ void MapWindow::initLayers() {
|
||||
20, 0
|
||||
};
|
||||
|
||||
if (!m_map->sourceExists("carrotSpeedSource")) {
|
||||
qDebug() << "Initializing carrotSpeedSource";
|
||||
|
||||
// 빈 FeatureCollection GeoJSON (QVariantMap 버전)
|
||||
QVariantMap fc;
|
||||
fc["type"] = "FeatureCollection";
|
||||
fc["features"] = QVariantList{}; // 빈 리스트
|
||||
|
||||
QVariantMap src;
|
||||
src["type"] = "geojson";
|
||||
src["data"] = fc;
|
||||
m_map->addSource("carrotSpeedSource", src);
|
||||
}
|
||||
|
||||
if (!m_map->layerExists("carrotSpeedLayer")) {
|
||||
qDebug() << "Initializing carrotSpeedLayer";
|
||||
QVariantMap layer;
|
||||
layer["type"] = "symbol";
|
||||
layer["source"] = "carrotSpeedSource";
|
||||
m_map->addLayer("carrotSpeedLayer", layer);
|
||||
|
||||
// properties.speed 를 텍스트로 표시 (토큰 방식)
|
||||
// "{speed}" 라고 쓰면 properties.speed 값을 문자열로 넣어줌
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "text-field", "{speed}");
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "text-size", 16.0);
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "text-offset", QVariantList{ 0.0, -1.5 });
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "text-anchor", "top");
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "icon-allow-overlap", true);
|
||||
m_map->setPaintProperty("carrotSpeedLayer", "text-color", QColor("white"));
|
||||
m_map->setPaintProperty("carrotSpeedLayer", "text-halo-color", QColor("black"));
|
||||
m_map->setPaintProperty("carrotSpeedLayer", "text-halo-width", 1.0);
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "text-allow-overlap", true);
|
||||
|
||||
}
|
||||
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-color", QColor("grey"));
|
||||
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-opacity", fillExtrusionOpacity);
|
||||
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-height", fillExtrusionHight);
|
||||
@@ -228,6 +265,74 @@ void MapWindow::updateState(const UIState &s) {
|
||||
|
||||
initLayers();
|
||||
|
||||
{
|
||||
std::string raw = params_memory.get("CarrotSpeedViz");
|
||||
if (!raw.empty()) {
|
||||
QString qraw = QString::fromStdString(raw);
|
||||
//printf("%s\n", qraw.toStdString().c_str());
|
||||
if (qraw != last_viz_raw) {
|
||||
last_viz_raw = qraw;
|
||||
|
||||
QJsonParseError err;
|
||||
QJsonDocument doc = QJsonDocument::fromJson(qraw.toUtf8(), &err);
|
||||
if (err.error == QJsonParseError::NoError && doc.isObject()) {
|
||||
QJsonObject obj = doc.object();
|
||||
QJsonArray pts = obj["pts"].toArray();
|
||||
|
||||
// GeoJSON FeatureCollection → QVariantMap 트리로 만들기
|
||||
QVariantList features; // Feature 리스트
|
||||
|
||||
for (const QJsonValue& v : pts) {
|
||||
QJsonArray arr = v.toArray();
|
||||
if (arr.size() < 3) continue;
|
||||
|
||||
double plat = arr[0].toDouble();
|
||||
double plon = arr[1].toDouble();
|
||||
double spd = arr[2].toDouble();
|
||||
|
||||
// geometry: Point (GeoJSON: [lon, lat] 순서)
|
||||
QVariantList coords;
|
||||
coords.append(plon);
|
||||
coords.append(plat);
|
||||
|
||||
QVariantMap geom;
|
||||
geom["type"] = "Point";
|
||||
geom["coordinates"] = coords;
|
||||
|
||||
// properties: speed
|
||||
QVariantMap props;
|
||||
props["speed"] = static_cast<int>(std::round(spd));
|
||||
|
||||
// Feature
|
||||
QVariantMap feature;
|
||||
feature["type"] = "Feature";
|
||||
feature["geometry"] = geom;
|
||||
feature["properties"] = props;
|
||||
|
||||
features.append(feature);
|
||||
}
|
||||
|
||||
QVariantMap fc;
|
||||
fc["type"] = "FeatureCollection";
|
||||
fc["features"] = features;
|
||||
|
||||
QJsonDocument fc_doc = QJsonDocument::fromVariant(fc);
|
||||
QByteArray fc_bytes = fc_doc.toJson(QJsonDocument::Compact);
|
||||
|
||||
QVariantMap src;
|
||||
src["type"] = "geojson";
|
||||
src["data"] = fc_bytes;
|
||||
m_map->updateSource("carrotSpeedSource", src);
|
||||
m_map->setLayoutProperty("carrotSpeedLayer", "visibility", "visible");
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
// 필요하면 숨기기
|
||||
// m_map->setLayoutProperty("carrotSpeedLayer", "visibility", "none");
|
||||
}
|
||||
}
|
||||
|
||||
if (!locationd_valid) {
|
||||
setError(tr("Waiting for GPS(APN)"));
|
||||
} else if (routing_problem) {
|
||||
@@ -295,17 +400,17 @@ void MapWindow::updateState(const UIState &s) {
|
||||
updateDestinationMarker();
|
||||
}
|
||||
if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) {
|
||||
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
|
||||
if (locationd_location.getGpsOK()) {
|
||||
//auto carrot_man = sm["carrotMan"].getCarrotMan();
|
||||
/*
|
||||
gps = (ublox_avaliable)? sm[gps_service].getGpsLocationExternal() : sm[gps_service].getGpsLocation();
|
||||
if (gps.getHasFix()) {
|
||||
auto model_path = model_to_collection(locationd_location.getCalibratedOrientationECEF(), locationd_location.getPositionECEF(), sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon());
|
||||
//auto model_path = model_to_collection(sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon(), carrotMan.getXPosAngle());
|
||||
QMapLibre::Feature model_path_feature(QMapLibre::Feature::LineStringType, model_path, {}, {});
|
||||
QVariantMap modelV2Path;
|
||||
modelV2Path["type"] = "geojson";
|
||||
modelV2Path["data"] = QVariant::fromValue<QMapLibre::Feature>(model_path_feature);
|
||||
m_map->updateSource("modelPathSource", modelV2Path);
|
||||
}
|
||||
*/
|
||||
model_rcv_frame = sm.rcv_frame("modelV2");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -69,6 +69,7 @@ private:
|
||||
MapInstructions* map_instructions;
|
||||
MapETA* map_eta;
|
||||
|
||||
|
||||
// Blue with normal nav, green when nav is input into the model
|
||||
QColor getNavPathColor(bool nav_enabled) {
|
||||
return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee");
|
||||
@@ -78,10 +79,12 @@ private:
|
||||
void updateDestinationMarker();
|
||||
uint64_t route_rcv_frame = 0;
|
||||
|
||||
// FrogPilot variables
|
||||
Params params;
|
||||
uint64_t model_rcv_frame = 0;
|
||||
|
||||
Params params_memory{ "/dev/shm/params" };
|
||||
QString last_viz_raw;
|
||||
|
||||
float MAX_ZOOM = 17; // carrot
|
||||
private slots:
|
||||
void updateState(const UIState &s);
|
||||
|
||||
@@ -215,19 +215,24 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
//QObject::connect(init_btn, &QPushButton::clicked, this, &DevicePanel::reboot);
|
||||
QObject::connect(init_btn, &QPushButton::clicked, [&]() {
|
||||
if (ConfirmationDialog::confirm(tr("Git pull & Reboot?"), tr("Yes"), this)) {
|
||||
QString cmd =
|
||||
"bash -c 'cd /data/openpilot && "
|
||||
"git fetch && "
|
||||
"if git status -uno | grep -q \"Your branch is behind\"; then "
|
||||
"git pull && reboot; "
|
||||
QString pullscript = "cd /data/openpilot && "
|
||||
"git fetch origin && "
|
||||
"LOCAL=$(git rev-parse HEAD) && "
|
||||
"BRANCH=$(git branch --show-current) && "
|
||||
"REMOTE=$(git rev-parse origin/$BRANCH) && "
|
||||
"if [ $LOCAL != $REMOTE ]; then "
|
||||
"echo 'Local is behind. Pulling updates...' && "
|
||||
"git pull --ff-only && "
|
||||
"sudo reboot; "
|
||||
"else "
|
||||
"echo \"Already up to date.\"; "
|
||||
"echo 'Already up to date.'; "
|
||||
"fi'";
|
||||
|
||||
if (!QProcess::startDetached(cmd)) {
|
||||
bool success = QProcess::startDetached("/bin/sh", QStringList() << "-c" << pullscript);
|
||||
|
||||
if (!success) {
|
||||
ConfirmationDialog::alert(tr("Failed to start update process."), this);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
ConfirmationDialog::alert(tr("Update process started. Device will reboot if updates are applied."), this);
|
||||
}
|
||||
}
|
||||
@@ -858,6 +863,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
|
||||
speedToggles->addItem(new CValueControl("AutoNaviSpeedBumpSpeed", tr("SpeedBumpSpeed(35Km/h)"), "", 10, 100, 5));
|
||||
speedToggles->addItem(new CValueControl("AutoNaviCountDownMode", tr("NaviCountDown mode(2)"), tr("0: off, 1:tbt+camera, 2:tbt+camera+bump"), 0, 2, 1));
|
||||
speedToggles->addItem(new CValueControl("TurnSpeedControlMode", tr("Turn Speed control mode(1)"), tr("0: off, 1:vision, 2:vision+route, 3: route"), 0, 3, 1));
|
||||
speedToggles->addItem(new CValueControl("CarrotSmartSpeedControl", tr("Smart Speed Control(0)"), tr("0: off, 1:accel, 2:decel, 3: all"), 0, 3, 1));
|
||||
speedToggles->addItem(new CValueControl("MapTurnSpeedFactor", tr("Map TurnSpeed Factor(100)"), "", 50, 300, 5));
|
||||
speedToggles->addItem(new CValueControl("ModelTurnSpeedFactor", tr("Model TurnSpeed Factor(0)"), "", 0, 80, 10));
|
||||
speedToggles->addItem(new CValueControl("AutoTurnControl", tr("ATC: Auto turn control(0)"), tr("0:None, 1: lane change, 2: lane change + speed, 3: speed"), 0, 3, 1));
|
||||
|
||||
+3
-1
@@ -98,13 +98,15 @@ void UIState::updateStatus() {
|
||||
}
|
||||
|
||||
UIState::UIState(QObject *parent) : QObject(parent) {
|
||||
ublox_avaliable = Params().getBool("UbloxAvailable");
|
||||
auto gps_service = (ublox_avaliable) ? "gpsLocationExternal" : "gpsLocation";
|
||||
sm = std::make_unique<SubMaster>(std::vector<const char*>{
|
||||
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
|
||||
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
|
||||
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
|
||||
"longitudinalPlan",
|
||||
"carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
|
||||
"navRoute", "navInstruction", "navInstructionCarrot", "liveLocationKalman", "liveDelay",
|
||||
"navRoute", "navInstruction", "navInstructionCarrot", gps_service, "liveDelay",
|
||||
"peripheralState",
|
||||
});
|
||||
prime_state = new PrimeState(this);
|
||||
|
||||
@@ -98,6 +98,8 @@ public:
|
||||
float show_brightness_ratio = 1.0;
|
||||
int show_brightness_timer = 20;
|
||||
|
||||
bool ublox_avaliable = true;
|
||||
|
||||
signals:
|
||||
void uiUpdate(const UIState &s);
|
||||
void offroadTransition(bool offroad);
|
||||
|
||||
Reference in New Issue
Block a user