mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GWM Model, Lead+1 detect, RadarVisionMatch and... (#214)

Browse Source
This commit is contained in:
carrot
2025-09-21 14:39:46 +09:00
committed by GitHub
parent 77a8919349
commit 8962bb1acc
22 changed files with 502 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files
RELEASES.md
+7
View File
@@ -1,3 +1,10 @@
Carrot2-v9 (2025-09-21)
========================
* GWM Model
* Lead + 1 detect (전전차 감지기능)
* Improve radar vision matching
* Auto safe-mode on stopped vehicle detection
Carrot2-v9 (2025-09-xx)
========================
* TR16 Model
cereal/log.capnp
+2
View File
@@ -770,6 +770,7 @@ struct RadarState @0x9a185389d6fdd05f {
leadsRight @17 : List(LeadData);
leadsLeft2 @19 : List(LeadData);
leadsRight2 @20 : List(LeadData);
leadsCutIn @21 : List(LeadData);
struct LeadData {
dRel @0 :Float32;
@@ -790,6 +791,7 @@ struct RadarState @0x9a185389d6fdd05f {
aLead @5 :Float32;
jLead @16 :Float32;
score @17 :Float32;
}
# deprecated
opendbc_repo/opendbc/car/gm/radar_interface.py
+1
View File
@@ -89,6 +89,7 @@ class RadarInterface(RadarInterfaceBase):
self.pts[targetId].vLead = self.pts[targetId].vRel + self.v_ego
self.pts[targetId].aRel = float('nan')
self.pts[targetId].yvRel = 0# float('nan')
self.pts[targetId].measured = True
for oldTarget in list(self.pts.keys()):
if oldTarget not in currentTargets:
opendbc_repo/opendbc/car/interfaces.py
+38 -28
View File
@@ -218,8 +218,8 @@ class MyTrack:
self.vLead_avg = FirstOrderFilter(self.vLead, 0.1, self.dt)
self.aLead_avg = FirstOrderFilter(self.aLead, 0.15, self.dt)
self.jLead_avg = FirstOrderFilter(self.jLead, 0.4, self.dt)
self.yRel_avg = FirstOrderFilter(self.yRel, 0.02, self.dt)
self.yvRel_avg = FirstOrderFilter(self.yvRel, 0.02, self.dt)
self.yRel_avg = FirstOrderFilter(self.yRel, 0.1, self.dt)
self.yvRel_avg = FirstOrderFilter(self.yvRel, 0.1, self.dt)
self.cnt = 0
def init_point(self, radar_point):
@@ -237,7 +237,7 @@ class MyTrack:
self.yRel_avg.x = self.yRel
self.yvRel_avg.x = self.yvRel
def update(self, radar_point):
def update(self, radar_point, a_ego):
if not radar_point.measured:
if self.cnt > 0:
self.init_point(radar_point)
@@ -247,29 +247,29 @@ class MyTrack:
self.cnt += 1
else:
self.vLead = radar_point.vLead
"""
if abs(radar_point.dRel - self.dRel) > 3.0 or abs(self.vRel - radar_point.vRel) > 20.0 * self.dt:
self.cnt = 0
self.jLead = 0.0
self.aLead = 0.0
self.vLead_avg.x = self.vLead
self.aLead_avg.x = self.aLead
self.jLead_avg.x = self.jLead
self.v_lead_filtered_last = self.vLead
"""
self.yRel = self.yRel_avg.update(radar_point.yRel)
self.yvRel = self.yvRel_avg.update(radar_point.yvRel)
v_lead_filtered = self.vLead_avg.update(self.vLead)
pseudo_stop = abs(v_lead_filtered) < 0.3 and abs(self.vLead - v_lead_filtered) < 0.05
a_raw = (v_lead_filtered - self.v_lead_filtered_last) / self.dt
self.v_lead_filtered_last = v_lead_filtered
a_lead = self.aLead_avg.update(a_raw if not pseudo_stop else 0.0)
if True: #math.isnan(radar_point.aRel): #
v_lead_filtered = self.vLead_avg.update(self.vLead)
pseudo_stop = abs(v_lead_filtered) < 0.3 and abs(self.vLead - v_lead_filtered) < 0.05
a_raw = (v_lead_filtered - self.v_lead_filtered_last) / self.dt
self.v_lead_filtered_last = v_lead_filtered
j_lead = (a_lead - self.aLead) / self.dt
self.aLead = a_lead
self.jLead = self.jLead_avg.update(j_lead if self.cnt > 2 else 0.0)
self.noisy = abs(a_raw - self.aLead) > 3.0
if self.noisy:
self.cnt = 0
a_lead = self.aLead_avg.update(np.clip(a_raw, -10.0, 5.0) if not pseudo_stop else 0.0)
j_lead = (a_lead - self.aLead) / self.dt
self.aLead = a_lead
self.jLead = self.jLead_avg.update(j_lead if self.cnt > 2 else 0.0)
else:
a_lead = radar_point.aRel + a_ego
j_lead = (a_lead - self.aLead) / self.dt
self.aLead = a_lead
self.jLead = self.jLead_avg.update(j_lead if self.cnt > 2 else 0.0)
# Store latest values
self.dRel = radar_point.dRel
@@ -288,6 +288,8 @@ class RadarInterfaceBase(ABC):
delay = CP.radarDelay
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_CTRL)) + 1)
self.v_ego = 0.0
self.a_ego_hist = deque([0.0], maxlen=int(round(delay / DT_CTRL)) + 1)
self.a_ego = 0.0
self.last_timestamp = None
self.dt = None
@@ -308,9 +310,11 @@ class RadarInterfaceBase(ABC):
self.init_samples.append(rcv_time)
def update_carrot(self, v_ego, rcv_time, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None:
def update_carrot(self, v_ego, a_ego, rcv_time, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None:
self.v_ego_hist.append(v_ego)
self.v_ego = self.v_ego_hist[0]
self.a_ego_hist.append(a_ego)
self.a_ego = self.a_ego_hist[0]
ret = self.update(can_packets)
if ret is not None:
@@ -325,12 +329,18 @@ class RadarInterfaceBase(ABC):
new_tracks[track_id] = MyTrack(track_id, radar_point, self.dt)
else:
new_tracks[track_id] = self.tracks[track_id]
new_tracks[track_id].update(radar_point)
new_tracks[track_id].update(radar_point, self.a_ego)
radar_point.aLead = float(new_tracks[track_id].aLead)
radar_point.jLead = float(new_tracks[track_id].jLead)
radar_point.yRel = float(new_tracks[track_id].yRel)
radar_point.yvRel = float(new_tracks[track_id].yvRel)
if new_tracks[track_id].cnt < 6:
radar_point.aLead = 0
radar_point.jLead = 0
radar_point.yRel = float(new_tracks[track_id].yRel)
radar_point.yvRel = float(new_tracks[track_id].yvRel)
else:
radar_point.aLead = float(new_tracks[track_id].aLead)
radar_point.jLead = float(new_tracks[track_id].jLead)
radar_point.yRel = float(new_tracks[track_id].yRel)
radar_point.yvRel = float(new_tracks[track_id].yvRel)
self.tracks = new_tracks
"""
selfdrive/car/card.py
+1 -1
View File
@@ -198,7 +198,7 @@ class Car:
if can_rcv_valid and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, rcv_time, can_list)
RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, CS.aEgo, rcv_time, can_list)
#self.t2 = time.monotonic()
#self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
selfdrive/car/cruise.py
+5 -2
View File
@@ -495,7 +495,10 @@ class VCruiseCarrot:
if self._soft_hold_active > 0:
self._soft_hold_active = 0
elif self._cruise_ready or not CC.enabled or CS.cruiseState.standstill or self.carrot_cruise_active:
pass
if self._cruise_button_mode in [2, 3]:
road_limit_kph = self.nRoadLimitSpeed * self.autoSpeedUptoRoadSpeedLimit
if road_limit_kph > 1.0:
v_cruise_kph = max(v_cruise_kph, road_limit_kph)
elif self._v_cruise_kph_at_brake > 0 and v_cruise_kph < self._v_cruise_kph_at_brake:
v_cruise_kph = self._v_cruise_kph_at_brake
self._v_cruise_kph_at_brake = 0
@@ -576,7 +579,7 @@ class VCruiseCarrot:
self._cruise_cancel_state = True
self._lat_enabled = False
self._paddle_decel_active = False
self.params.put_bool_nonblocking("ExperimentalMode", not self.params.get_bool("ExperimentalMode"))
#self.params.put_bool_nonblocking("ExperimentalMode", not self.params.get_bool("ExperimentalMode"))
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
if self._paddle_mode > 0 and button_type in [ButtonType.paddleLeft, ButtonType.paddleRight]: # paddle button
selfdrive/carrot/carrot_functions.py
+18 -15
View File
@@ -79,7 +79,7 @@ class CarrotPlanner:
self.stopSignCount = 0
self.stop_distance = 6.0
self.trafficStopDistanceAdjust = 1.5 #params.get_float("TrafficStopDistanceAdjust") / 100.
self.trafficStopDistanceAdjust = 2.0 #params.get_float("TrafficStopDistanceAdjust") / 100.
self.comfortBrake = 2.4
self.comfort_brake = self.comfortBrake
@@ -147,12 +147,6 @@ class CarrotPlanner:
else:
self.myDrivingMode = myDrivingMode
self.mySafeFactor = 1.0
if self.myDrivingMode == DrivingMode.Eco: # eco
self.mySafeFactor = self.myEcoModeFactor
elif self.myDrivingMode == DrivingMode.Safe: #safe
self.mySafeFactor = self.mySafeModeFactor
if self.params_count == 10:
self.myHighModeFactor = 1.2 #float(self.params.get_int("MyHighModeFactor")) / 100.
self.trafficLightDetectMode = self.params.get_int("TrafficLightDetectMode") # 0: None, 1:Stop, 2:Stop&Go
@@ -212,13 +206,14 @@ class CarrotPlanner:
self.desireState = 0.0
self.desireStateCount = 0
def dynamic_t_follow(self, t_follow, lead, desired_follow_distance):
def dynamic_t_follow(self, t_follow, lead, desired_follow_distance, prev_a):
self.jerk_factor_apply = self.jerk_factor
if self.desireState > 0.9 and self.desireStateCount < int(1.5 / DT_MDL): # lane change state, 1.5초동안만.
t_follow *= self.dynamicTFollowLC # 차선변경시 t_follow를 줄임.
self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC # 차선변경시 jerk factor를 줄여 aggresive하게
elif lead.status:
elif lead.status:
t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.2, 0.0])
if self.dynamicTFollow > 0.0:
gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1
t_follow += gap_dist_adjust
@@ -333,7 +328,6 @@ class CarrotPlanner:
def update(self, sm, v_cruise_kph, mode):
self._params_update()
self._update_model_desire(sm)
self.events = Events()
@@ -354,14 +348,23 @@ class CarrotPlanner:
v_ego_cluster = carstate.vEgoCluster
v_ego_cluster_kph = v_ego_cluster * CV.MS_TO_KPH
leadOne = radarstate.leadOne
self.mySafeFactor = 1.0
if leadOne.status and leadOne.vLead < 5:
self.mySafeFactor = self.mySafeModeFactor
elif self.myDrivingMode == DrivingMode.Eco: # eco
self.mySafeFactor = self.myEcoModeFactor
elif self.myDrivingMode == DrivingMode.Safe: #safe
self.mySafeFactor = self.mySafeModeFactor
if self.frame % 20 == 0: # every 1 sec
vLead = 0
aLead = 0
dRel = 200
if radarstate.leadOne.status:
vLead = radarstate.leadOne.vLead * CV.MS_TO_KPH
aLead = radarstate.leadOne.aLead
dRel = radarstate.leadOne.dRel
if leadOne.status:
vLead = leadOne.vLead * CV.MS_TO_KPH
aLead = leadOne.aLead
dRel = leadOne.dRel
self.drivingModeDetector.update_data(v_ego_kph, vLead, carstate.aEgo, aLead, dRel)
@@ -433,7 +436,7 @@ class CarrotPlanner:
self.comfort_brake = self.comfortBrake * 0.9
#self.comfort_brake = COMFORT_BRAKE
self.trafficStopAdjustRatio = np.interp(v_ego_kph, [0, 100], [1.0, 0.7])
stop_dist = self.xStop * np.interp(self.xStop, [0, 100], [1.0, self.trafficStopAdjustRatio]) ##�����Ÿ��� ���� �����Ÿ� ��������
stop_dist = self.xStop * np.interp(self.xStop, [0, 50], [1.0, self.trafficStopAdjustRatio]) ##�����Ÿ��� ���� �����Ÿ� ��������
if stop_dist > 10.0: ### 10M�̻��϶���, self.actual_stop_distance�� ������Ʈ��.
self.actual_stop_distance = stop_dist
stop_model_x = 0
selfdrive/carrot/carrot_man.py
+7 -1
View File
@@ -630,7 +630,13 @@ class CarrotMan:
car_selected = car_selected.decode('utf-8')
git_branch = Params().get("GitBranch").decode('utf-8')
directory = git_branch + " " + car_selected + " " + Params().get("DongleId").decode('utf-8')
try:
ftp.mkd(git_branch)
except Exception as e:
print(f"Directory creation failed: {e}")
ftp.cwd(git_branch)
directory = car_selected + " " + Params().get("DongleId").decode('utf-8')
current_time = datetime.now().strftime("%Y%m%d-%H%M%S")
filename = tmux_why + "-" + current_time + "-" + git_branch + ".txt"
selfdrive/controls/lib/desire_helper.py
+14 -4
View File
@@ -147,6 +147,7 @@ class DesireHelper:
self.turn_desire_state = False
self.desire_disable_count = 0
self.turn_disable_count = 0
self.blindspot_detected_counter = 0
self.auto_lane_change_enable = False
self.next_lane_change = False
@@ -177,13 +178,18 @@ class DesireHelper:
self.available_left_edge = self.road_edge_left_count.counter > available_count and self.distance_to_road_edge_left_far > min_lane_width
self.available_right_edge = self.road_edge_right_count.counter > available_count and self.distance_to_road_edge_right_far > min_lane_width
def check_desire_state(self, modeldata):
def check_desire_state(self, modeldata, carstate):
desire_state = modeldata.meta.desireState
self.turn_desire_state = (desire_state[1] + desire_state[2]) > 0.1
if self.turn_desire_state:
self.desire_disable_count = int(2.0/DT_MDL)
else:
self.desire_disable_count = max(0, self.desire_disable_count - 1)
if abs(carstate.steeringAngleDeg) > 80:
self.turn_disable_count = int(10.0/DT_MDL)
else:
self.turn_disable_count = max(0, self.turn_disable_count - 1)
#print(f"desire_state = {desire_state}, turn_desire_state = {self.turn_desire_state}, disable_count = {self.desire_disable_count}")
def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):
@@ -202,7 +208,7 @@ class DesireHelper:
##### check lane state
self.check_lane_state(modeldata)
self.check_desire_state(modeldata)
self.check_desire_state(modeldata, carstate)
#### check driver's blinker state
driver_blinker_state = carstate.leftBlinker * 1 + carstate.rightBlinker * 2
@@ -312,8 +318,12 @@ class DesireHelper:
elif desire_enabled and ((below_lane_change_speed and not carstate.standstill and self.enable_turn_desires) or self.turn_desire_state):
#print("Desire Turning")
self.lane_change_state = LaneChangeState.off
self.turn_direction = TurnDirection.turnLeft if blinker_state == BLINKER_LEFT else TurnDirection.turnRight
self.lane_change_direction = self.turn_direction #LaneChangeDirection.none
if self.turn_disable_count > 0:
self.turn_direction = TurnDirection.none
self.lane_change_direction = LaneChangeDirection.none
else:
self.turn_direction = TurnDirection.turnLeft if blinker_state == BLINKER_LEFT else TurnDirection.turnRight
self.lane_change_direction = self.turn_direction #LaneChangeDirection.none
desire_enabled = False
elif self.desire_disable_count > 0: # Turn 후 일정시간 동안 차선변경 불가능
#print("Desire after turning")
selfdrive/controls/lib/latcontrol_angle.py
+5 -1
View File
@@ -12,6 +12,10 @@ class LatControlAngle(LatControl):
super().__init__(CP, CI)
self.sat_check_min_speed = 5.
self.angle_steers_des = 0.0
#print(CP.carFingerprint, "using LatControlAngle")
#self.factor = 0.5 if CP.carFingerprint in ["HYUNDAI_IONIQ_5_PE"] else 1.0
#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):
angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -22,7 +26,7 @@ class LatControlAngle(LatControl):
else:
angle_log.active = True
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg
angle_steers_des += params.angleOffsetDeg #* self.factor
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+2 -2
View File
@@ -373,7 +373,7 @@ class LongitudinalMpc:
else:
self.j_lead = 0.0
lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne, np.clip(self.j_lead * carrot.j_lead_factor, -2.0, 2.0))
lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne, np.clip(self.j_lead * carrot.j_lead_factor, -1.0, 1.0))
lead_xv_1, _ = self.process_lead(radarstate.leadTwo, 0.0)
mode = self.mode
@@ -385,7 +385,7 @@ class LongitudinalMpc:
else:
v_cruise, stop_x, mode = carrot.v_cruise, carrot.stop_dist, carrot.mode
desired_distance = desired_follow_distance(v_ego, lead_v_0, comfort_brake, stop_distance, t_follow)
t_follow = carrot.dynamic_t_follow(t_follow, radarstate.leadOne, desired_distance)
t_follow = carrot.dynamic_t_follow(t_follow, radarstate.leadOne, desired_distance, self.prev_a)
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
selfdrive/controls/radard.py
+147 -150
View File
@@ -4,6 +4,7 @@ import numpy as np
from collections import deque
from typing import Any
import heapq
import copy
import capnp
from cereal import messaging, log, car
@@ -37,8 +38,11 @@ class Track:
self.selected_count = 0
self.cut_in_count = 0
self.measured = False
self.score = 0.0
self.in_lane_prob = 0.0
self.in_lane_prob_future = 0.0
def update(self, md, pt, ready, radar_reaction_factor):
def update(self, md, pt, ready, radar_reaction_factor, radar_lat_factor):
#pt_yRel = -leads_v3[0].y[0] if track_id in [0, 1] and pt.yRel == 0 and self.ready and leads_v3[0].prob > 0.5 else pt.yRel
self.dRel = pt.dRel
@@ -54,15 +58,10 @@ class Track:
if not self.measured:
self.cnt = 0
self.yRel_future = self.yRel + self.yvLead * radar_lat_factor
self.dRel_future = self.dRel + self.vLead * radar_lat_factor
if ready:
self.dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
if self.cnt == 0:
self.yRel_filtered = self.yRel
self.yvLead_filtered = self.yvLead
else:
self.yRel_filtered = self.yRel_filtered * 0.9 + self.yRel * 0.1
self.yvLead_filtered = self.yvLead_filtered * 0.9 + self.yvLead * 0.1
self.d_path(md) #self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
a_lead_threshold = 0.5 * radar_reaction_factor
if abs(self.aLead) < a_lead_threshold and abs(self.jLead) < 0.5:
@@ -72,11 +71,25 @@ class Track:
self.cnt += 1
def d_path(self, md):
lane_xs = md.laneLines[1].x
left_ys = md.laneLines[1].y
right_ys = md.laneLines[2].y
def d_path_interp(dRel, yRel):
left_lane_y = np.interp(dRel, lane_xs, left_ys)
right_lane_y = np.interp(dRel, lane_xs, right_ys)
center_y = (left_lane_y + right_lane_y) / 2.0
lane_half_width = abs(right_lane_y - left_lane_y) / 2.0
dist_from_center = yRel + center_y
in_lane_prob = max(0.0, 1.0 - (abs(dist_from_center) / lane_half_width))
return dist_from_center, in_lane_prob
self.dPath, self.in_lane_prob = d_path_interp(self.dRel, self.yRel)
self.dPath_future, self.in_lane_prob_future = d_path_interp(self.dRel_future, self.yRel_future)
def get_RadarState(self, model_prob: float = 0.0, vision_y_rel = 0.0):
yRel = vision_y_rel if vision_y_rel != 0.0 else float(self.yRel)
return {
"dRel": float(self.dRel),
"yRel": float(self.yRel) if vision_y_rel == 0.0 else vision_y_rel,
"yRel": float(self.yRel) if self.yRel != 0.0 else vision_y_rel,
"dPath" : float(self.dPath),
"vRel": float(self.vRel),
"vLead": float(self.vLead),
@@ -91,6 +104,7 @@ class Track:
"modelProb": model_prob,
"radar": True,
"radarTrackId": self.identifier,
"score": self.score # for debug purposes only
}
def potential_low_speed_lead(self, v_ego: float):
@@ -109,122 +123,98 @@ def laplacian_pdf(x: float, mu: float, b: float):
diff = abs(x - mu) / max(b, 1e-4)
return 0.0 if diff > 50.0 else math.exp(-diff)
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track], radar_lat_factor = 0.0):
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
#vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0
max_vision_dist = max(offset_vision_dist * 1.25, 5.0)
min_vision_dist = max(offset_vision_dist * 0.6, 1.0)
min_vision_dist = max(offset_vision_dist * 0.8, 1.0)
max_vision_dist2 = max(offset_vision_dist * 1.45, 5.0)
min_vision_dist2 = 1.5 #max(offset_vision_dist * 0.3, 1.0)
max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # 확률이 낮으면 속도오차를 줄임.
def prob(c):
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_y2 = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0] * 2) # for cut-in
prob_v = laplacian_pdf(c.vLead, lead.v[0], lead.vStd[0])
weight_v = np.interp(c.vLead, [0, 10], [0.3, 1])
#weight_v = np.interp(c.vLead, [0, 10], [0.3, 1])
score = prob_d * prob_y * prob_v # * weight_v
score2 = prob_d * prob_y2 * prob_v # * weight_v
return prob_d * prob_y * prob_v * weight_v
return score, score2 #prob_d * prob_y * prob_v * weight_v
def vel_sane(c):
return (abs(c.vLead - lead.v[0]) < max_offset_vision_vel) or (c.vLead > 3)
def dist_sane(c, second=False):
if second:
return min_vision_dist2 < c.dRel < max_vision_dist2
return min_vision_dist < c.dRel < max_vision_dist
def y_sane(c, second=False):
if second:
return abs(c.yRel + lead.y[0]) < 4.0
return abs(c.yRel + lead.y[0]) < 2.0
best_track = max(tracks.values(), key=prob)
first_track, second_track, extra_track = None, None, None
first_score, second_score, extra_score = -1e6, -1e6, -1e6
for c in tracks.values():
c.score, score2 = prob(c)
if c.score > first_score:
second_score = first_score
second_track = first_track
first_score = c.score
first_track = c
if score2 > extra_score:
extra_score = score2
extra_track = c
#best_track = max(tracks.values(), key=prob)
# 끼어드는차량을 간헐적 멀리있는 차량으로 검출하는 문제가 있음..
y_gate = 2.0 #min(1.7, lead.yStd[0] * 2.0)
v_gate = max(5.0, lead.vStd[0] * 2.0)
yv_candidates = [
c for c in tracks.values()
if (min_vision_dist < c.dRel < max_vision_dist)
and (abs(c.yRel + lead.y[0]) < y_gate)
and (abs(c.vLead - lead.v[0]) < v_gate)
]
if False and yv_candidates:
best_track = min(yv_candidates, key=lambda c: c.dRel)
dist_sane = min_vision_dist < best_track.dRel < max_vision_dist #abs(best_track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.35, 5.0])
vel_sane = (abs(best_track.vLead - lead.v[0]) < 10) or (best_track.vLead > 3)
y_sane = abs(best_track.yRel + lead.y[0]) < y_gate
if dist_sane and y_sane:
if vel_sane and lead.prob < 0.45: # 근처에 달리고 있는차를 오감지 했을수 있음
best_track = None
elif not vel_sane or lead.prob < 0.5: # 속도가 안맞거나 희미하게 감지된 차인경우
if best_track.selected_count < 1: # 이전에 선택된 경우에는 그냥 통과함.
best_track.is_stopped_car_count += 1
if best_track.is_stopped_car_count < int(1.0/DT_MDL): # 2초 -> 1초
best_track = None
else:
best_track.is_stopped_car_count = max(0, best_track.is_stopped_car_count - 1)
def select_track(track, score, track2, score2, extra_track, extra_score):
if score < 0.0001:
return None
best_track = None
if dist_sane(track) and vel_sane(track):
if y_sane(track):
if lead.prob > 0.5:
best_track = track
elif lead.prob > 0.4 and track.selected_count > 0: # 비젼이 희미하지만 직전에 선택된 트랙인경우
best_track = track
elif lead.prob > 0.6:
best_track = track
elif dist_sane(track) and y_sane(track, True): # stopped-car
if score2 > 0.00001 and dist_sane(track2) and y_sane(track2) and vel_sane(track2):
best_track = track2
elif track.selected_count > 0:
best_track = track
else:
track.is_stopped_car_count += 2
if track.is_stopped_car_count > int(1.0/DT_MDL):
best_track = track
#elif dist_sane(track) and vel_sane(track) and lead.prob > 0.5:
# best_track = track
elif offset_vision_dist < 90 and lead.prob > 0.65:
# wide y detect, for cut-in
if extra_score > score and dist_sane(extra_track, True) and vel_sane(extra_track) and y_sane(extra_track, True):
best_track = extra_track
# wide dRel, y detect, for cut-in
elif dist_sane(track, True) and vel_sane(track) and y_sane(track, True):
best_track = track
elif score2 > 0.0001 and dist_sane(track2, True) and vel_sane(track2) and y_sane(track2, True):
best_track = track2
return best_track
best_track = select_track(first_track, first_score, second_track, second_score, extra_track, extra_score)
for c in tracks.values():
if c is best_track:
best_track.selected_count += 1
c.is_stopped_car_count = 0
else:
c.selected_count = 0
return best_track
def match_vision_to_track_old(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track], radar_lat_factor = 0.0):
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
#vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0
max_offset_vision_dist = max(offset_vision_dist * 0.35, 5.0)
max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # 확률이 낮으면 속도오차를 줄임.
def prob(c):
#if abs(offset_vision_dist - c.dRel) > max_offset_vision_dist:
# return -1e6
#if abs(lead.v[0] - c.vLead) > max_offset_vision_vel:
# return -1e6
#if abs(c.yRel + c.yvLead * radar_lat_factor + lead.y[0]) > 3.0: # lead.y[0]는 반대..
# return -1e6
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(c.yRel + c.yvLead * radar_lat_factor, -lead.y[0], lead.yStd[0])
prob_v = laplacian_pdf(c.vLead, lead.v[0], lead.vStd[0])
weight_v = np.interp(c.vLead, [0, 10], [0.3, 1])
return prob_d * prob_y * prob_v * weight_v
#track = max(tracks.values(), key=prob, default=None)
#return track if track and prob(track) > -1e6 else None
best_track = None
best_score = -1e6
for c in tracks.values():
score = prob(c)
if score > best_score:
best_score = score
best_track = c
if best_track is not None and offset_vision_dist - best_track.dRel > max_offset_vision_dist:
best_track = None
#if best_track is not None and lead.v[0] - best_track.vLead > max_offset_vision_vel:
# best_track = None
if best_track is not None and abs(best_track.yRel + best_track.yvLead * radar_lat_factor + lead.y[0]) > 3.0: # lead.y[0]는 반대..
best_track = None
if best_track is not None:
if lead.v[0] - best_track.vLead > max_offset_vision_vel:
best_track.is_stopped_car_count += 1
# 직전에 사용되었던것이라면 재사용, 2초간 유지된다면 정지차로 간주.
if best_track.selected_count < 1 and best_track.is_stopped_car_count < int(2.0/DT_MDL):
best_track = None
if best_track is not None:
best_track.selected_count += 1
for c in tracks.values():
if c is not best_track:
c.selected_count = 0
c.is_stopped_car_count = max(0, c.is_stopped_car_count - 1)
return best_track
@@ -420,7 +410,7 @@ class RadarD:
if track_id not in self.tracks:
self.tracks[track_id] = Track(track_id)
self.tracks[track_id].update(sm['modelV2'], pt, self.ready, self.radar_reaction_factor)
self.tracks[track_id].update(sm['modelV2'], pt, self.ready, self.radar_reaction_factor, self.radar_lat_factor)
for tid in list(self.tracks.keys()):
if tid not in valid_ids:
@@ -451,12 +441,14 @@ class RadarD:
self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, md)
self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, md)
alive_tracks = {tid: trk for tid, trk in self.tracks.items() if trk.measured }
alive_tracks = {tid: trk for tid, trk in self.tracks.items() if trk.cnt > 2 }
self.radar_state.leadOne, self.radar_detected = self.get_lead(sm['carState'], md, alive_tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
self.radar_state.leadTwo, _ = self.get_lead(sm['carState'], md, alive_tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False)
self.lane_line_available = md.laneLineProbs[1] > 0.5 and md.laneLineProbs[2] > 0.5
self.compute_leads(self.v_ego, alive_tracks, md, lane_width=3.2, model_v_ego=model_v_ego)
self.compute_leads(self.v_ego, alive_tracks, md)
if self.leadTwo is not None:
self.radar_state.leadTwo = self.leadTwo
if self.enable_radar_tracks == 3:
self._pick_lead_one_from_state()
@@ -481,14 +473,15 @@ class RadarD:
track_scc = tracks.pop(0, None)
# Determine leads, this is where the essential logic happens
if len(tracks) > 0 and ready and lead_msg.prob > .3:
track = match_vision_to_track(v_ego, lead_msg, tracks, self.radar_lat_factor)
if len(tracks) > 0 and ready and lead_msg.prob > .4:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
if track is None and track_scc is not None and track_scc.measured:
if self.enable_radar_tracks in [-1, 2] or model_v_ego < 5 or track_scc.vLead < 5.0:
track = track_scc
if (track is None or lead_msg.prob < .6) and track_scc is not None and track_scc.cnt > 2:
#if self.enable_radar_tracks in [-1, 2] or model_v_ego < 5 or track_scc.vLead < 5.0:
if self.enable_radar_tracks in [-1, 2] or track_scc.vLead < 5.0:
track = track_scc
lead_dict = {'status': False}
radar = False
@@ -513,7 +506,7 @@ class RadarD:
return lead_dict, radar
def compute_leads(self, v_ego, tracks, md, lane_width=3.2, model_v_ego=0.0):
def compute_leads(self, v_ego, tracks, md):
lead_msg = md.leadsV3[0] if (md is not None and len(md.position.x) == 33) else None
self.leadCutIn = {'status': False}
if lead_msg is None:
@@ -524,56 +517,46 @@ class RadarD:
self.radar_state.leadLeft = {'status': False}
self.radar_state.leadRight = {'status': False}
return
md_x, md_y = md.position.x, md.position.y
lane_xs = md.laneLines[1].x
left_ys = md.laneLines[1].y
right_ys = md.laneLines[2].y
left_list, right_list, center_list = [], [], []
left_list, right_list, center_list, cutin_list = [], [], [], []
for c in tracks.values():
dy = c.yRel_filtered + np.interp(c.dRel, md_x, md_y) # + c.yvLead_filtered * self.radar_lat_factor
dy_with_vel = dy + c.yvLead_filtered * self.radar_lat_factor
y_with_vel_neg = -(c.yRel_filtered + c.yvLead_filtered * self.radar_lat_factor)
left_y = np.interp(c.dRel, lane_xs, left_ys)
right_y = np.interp(c.dRel, lane_xs, right_ys)
y_rel_neg = - c.yRel
# center
if left_y < y_rel_neg < right_y:
if c.cnt > 6:
if c.in_lane_prob > 0.1:
if c.cnt > 3:
ld = c.get_RadarState(lead_msg.prob, float(-lead_msg.y[0]))
ld['modelProb'] = 0.01
center_list.append(ld)
# left/right
elif y_rel_neg < left_y:
elif y_rel_neg < 0: #left_lane_y:
ld = c.get_RadarState(0, 0)
if self.lane_line_available and c.in_lane_prob_future > 0.1 and c.cnt > int(2.0/DT_MDL):
if c.cut_in_count > int(0.1/DT_MDL):
ld['modelProb'] = 0.03
cutin_list.append(ld)
c.cut_in_count += 2
left_list.append(ld)
else:
ld = c.get_RadarState(0, 0)
if self.lane_line_available and c.in_lane_prob_future > 0.1 and c.cnt > int(2.0/DT_MDL):
if c.cut_in_count > int(0.1/DT_MDL):
ld['modelProb'] = 0.03
cutin_list.append(ld)
c.cut_in_count += 2
right_list.append(ld)
# cut-in
#cut_in_width = 3.0 #3.4 # 끼어들기 차폭
#if self.lane_line_available and left_y < y_with_vel_neg < right_y and (3 < c.dRel < 20 and c.vLead > 4 and c.cnt > int(2.0/DT_MDL) and c.yRel_filtered * c.yvLead_filtered < 0):
if self.lane_line_available and 3 < c.dRel < 50 and c.vLead > 4 and c.cnt > int(2.0/DT_MDL):
if (y_rel_neg < left_y and y_with_vel_neg > left_y) or (y_rel_neg > right_y and y_with_vel_neg < right_y):
if not self.leadCutIn['status'] or c.dRel < self.leadCutIn['dRel']:
c.cut_in_count += 1
else:
c.cut_in_count = 0
if c.cut_in_count > int(0.5/DT_MDL):
self.leadCutIn = c.get_RadarState(lead_msg.prob)
else:
c.cut_in_count = 0
else:
c.cut_in_count = 0
c.cut_in_count = max(c.cut_in_count - 1, 0)
self.radar_state.leadsLeft = left_list
self.radar_state.leadsRight = right_list
self.radar_state.leadsCenter = center_list
self.radar_state.leadsCutIn = cutin_list
self.leadCutIn = min(
(ld for ld in cutin_list if 3 < ld['dRel'] < 50 and ld['vLead'] > 4),
key=lambda d: d['dRel'],
default={'status': False}
)
self.radar_state.leadLeft = min(
(ld for ld in left_list if ld['dRel'] > 5 and abs(ld['dPath']) < 3.5),
@@ -585,12 +568,26 @@ class RadarD:
key=lambda d: d['dRel'],
default={'status': False}
)
self.leadTwo = None
if self.lane_line_available:
self.leadCenter = min(
(ld for ld in center_list if ld['vLead'] > 5 and ld['radar'] and abs(ld['yRel']) < 5.0 and ld['dRel'] > 3.5),
(ld for ld in center_list if ld['vLead'] > 5 and ld['radar'] and ld['dRel'] > 3.5),
key=lambda d: d['dRel'],
default={'status': False}
default=None
)
if self.radar_state.leadOne.status and self.radar_state.leadOne.radar:
self.leadTwo = min(
(ld for ld in center_list if ld['vLead'] > 5 and ld['radar'] and self.radar_state.leadOne.dRel < ld['dRel'] < 80),
key=lambda d: d['dRel'],
default=None
)
if self.leadTwo is not None:
self.leadTwo = copy.deepcopy(self.leadTwo)
#gap = self.leadTwo['dRel'] - self.radar_state.leadOne.dRel
#offset = 3.0 + min(gap * 0.2, 10)
#self.leadTwo['dRel'] = self.radar_state.leadOne.dRel + offset
self.leadTwo['dRel'] = max(self.radar_state.leadOne.dRel + 3.0, self.leadTwo['dRel'] - 8.0) # lead+1 차를 뒤로 8M후퇴하여, mpc에서 감자하도록함.. 최소 lead보다 3M앞에 위치하도록
else:
self.leadCenter = None
selfdrive/frogpilot/fleetmanager/fleet_manager.py
+3 -1
View File
@@ -110,6 +110,8 @@ def upload_folder_to_ftp(local_folder, directory, remote_path):
ftp.connect(ftp_server, ftp_port)
ftp.login(ftp_username, ftp_password)
ftp.cwd("routes")
try:
def create_path(path):
try:
@@ -223,7 +225,7 @@ def upload_carrot(route, segment):
abort(404, "Folder not found")
car_selected = Params().get("CarName", "none").decode('utf-8')
dongle_id = Params().get("DongleId", "unknown").decode('utf-8')
directory = f"routes {car_selected} {dongle_id}"
directory = f"{car_selected} {dongle_id}"
success = upload_folder_to_ftp(local_folder, directory, f"{route}--{segment}")
if success:
temproute = route
selfdrive/locationd/paramsd.py
+2 -1
View File
@@ -55,7 +55,8 @@ class ParamsLearner:
if roll_valid:
roll = localizer_roll
# Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
roll_std = 2 * localizer_roll_std
#roll_std = 2 * localizer_roll_std
roll_std = 3 * localizer_roll_std
else:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
selfdrive/modeld/constants.py
+3 -6
View File
@@ -13,12 +13,9 @@ class ModelConstants:
META_T_IDXS = [2., 4., 6., 8., 10.]
# model inputs constants
MODEL_FREQ = 20
HISTORY_FREQ = 5
HISTORY_LEN_SECONDS = 5
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
N_FRAMES = 2
MODEL_RUN_FREQ = 20
MODEL_CONTEXT_FREQ = 5 # "model_trained_fps"
FEATURE_LEN = 512
selfdrive/modeld/fill_model_msg.py
+1 -1
View File
@@ -169,7 +169,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
meta.hardBrakePredicted = hard_brake_predicted.item()
# confidence
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
if vipc_frame_id % (2*ModelConstants.MODEL_RUN_FREQ) == 0:
# any disengage prob
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
selfdrive/modeld/modeld.py
+76 -36
View File
@@ -82,6 +82,64 @@ class FrameMeta:
if vipc is not None:
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
class InputQueues:
def __init__ (self, model_fps, env_fps, n_frames_input):
assert env_fps % model_fps == 0
assert env_fps >= model_fps
self.model_fps = model_fps
self.env_fps = env_fps
self.n_frames_input = n_frames_input
self.dtypes = {}
self.shapes = {}
self.q = {}
def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None:
self.dtypes.update(input_dtypes)
if self.env_fps == self.model_fps:
self.shapes.update(input_shapes)
else:
for k in input_shapes:
shape = list(input_shapes[k])
if 'img' in k:
n_channels = shape[1] // self.n_frames_input
shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels
else:
shape[1] = (self.env_fps // self.model_fps) * shape[1]
self.shapes[k] = tuple(shape)
def reset(self) -> None:
self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()}
def enqueue(self, inputs:dict[str, np.ndarray]) -> None:
for k in inputs.keys():
if inputs[k].dtype != self.dtypes[k]:
raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}')
input_shape = list(self.shapes[k])
input_shape[1] = -1
single_input = inputs[k].reshape(tuple(input_shape))
sz = single_input.shape[1]
self.q[k][:,:-sz] = self.q[k][:,sz:]
self.q[k][:,-sz:] = single_input
def get(self, *names) -> dict[str, np.ndarray]:
if self.env_fps == self.model_fps:
return {k: self.q[k] for k in names}
else:
out = {}
for k in names:
shape = self.shapes[k]
if 'img' in k:
n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1))
out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1)
elif 'pulse' in k:
# any pulse within interval counts
out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2)
else:
idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1]
out[k] = self.q[k][:, idxs]
return out
class ModelState:
frames: dict[str, DrivingModelFrame]
inputs: dict[str, np.ndarray]
@@ -102,22 +160,15 @@ class ModelState:
self.policy_output_slices = policy_metadata['output_slices']
policy_output_size = policy_metadata['output_shapes']['outputs'][1]
self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
self.frames = {name: DrivingModelFrame(context, ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ) for name in self.vision_input_names}
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
# policy inputs
self.numpy_inputs = {
'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32),
'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32),
'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32),
'prev_desired_curv': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32),
'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32),
}
self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes}
self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES)
for k in ['desire_pulse', 'features_buffer']:
self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape})
self.full_input_queues.reset()
# img buffers are managed in openCL transform code
self.vision_inputs: dict[str, Tensor] = {}
@@ -139,16 +190,10 @@ class ModelState:
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
inputs['desire'][0] = 0
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
self.prev_desire[:] = inputs['desire']
inputs['desire_pulse'][0] = 0
new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0)
self.prev_desire[:] = inputs['desire_pulse']
self.full_desire[0,:-1] = self.full_desire[0,1:]
self.full_desire[0,-1] = new_desire
self.numpy_inputs['desire'][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
if TICI and not USBGPU:
@@ -164,21 +209,17 @@ class ModelState:
if prepare_only:
return None
self.vision_output = self.vision_run(**self.vision_inputs).numpy().flatten()
self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy()
vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire})
for k in ['desire_pulse', 'features_buffer']:
self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k]
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
self.policy_output = self.policy_run(**self.policy_inputs).numpy().flatten()
self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy()
policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
# TODO model only uses last value now
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
if SEND_RAW_PRED:
combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()])
@@ -235,7 +276,7 @@ def main(demo=False):
params = Params()
# setup filter to track dropped frames
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
frame_id = 0
last_vipc_frame_id = 0
run_count = 0
@@ -316,7 +357,7 @@ def main(demo=False):
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.)
lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
#lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -347,9 +388,8 @@ def main(demo=False):
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
inputs:dict[str, np.ndarray] = {
'desire': vec_desire,
'desire_pulse': vec_desire,
'traffic_convention': traffic_convention,
'lateral_control_params': lateral_control_params,
}
mt1 = time.perf_counter()
selfdrive/modeld/models/driving_policy.onnx
BIN
View File
Binary file not shown.
selfdrive/modeld/models/driving_vision.onnx
BIN
View File
Binary file not shown.
selfdrive/modeld/parse_model_outputs.py
+16 -9
View File
@@ -85,6 +85,13 @@ class Parser:
outs[name] = pred_mu_final.reshape(final_shape)
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
def is_mhp(self, outs, name, shape):
if self.check_missing(outs, name):
return False
if outs[name].shape[1] == 2 * shape:
return False
return True
def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
@@ -94,19 +101,19 @@ class Parser:
self.parse_binary_crossentropy('lane_lines_prob', outs)
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
self.parse_binary_crossentropy('meta', outs)
self.parse_binary_crossentropy('lead_prob', outs)
lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH)
lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0)
lead_out_shape = (ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) if lead_mhp else \
(ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)
self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape)
return outs
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
if 'lat_planner_solution' in outs:
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
if 'desired_curvature' in outs:
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH)
plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
self.parse_binary_crossentropy('lead_prob', outs)
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
return outs
def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
selfdrive/ui/carrot.cc
+152 -23
View File
@@ -599,6 +599,13 @@ private:
QPointF tf_vertex_left;
QPointF tf_vertex_right;
QPointF lead_two_left;
QPointF lead_two_right;
float lead_two_xl = 0.0;
float lead_two_xr = 0.0;
float lead_two_y = 0.0;
int lead_two_status = 0;
protected:
bool make_data(const UIState* s) {
SubMaster& sm = *(s->sm);
@@ -641,6 +648,34 @@ protected:
_model->mapToScreen(max_distance, y - 1.2, z + 1.22, &path_end_left_vertex);
_model->mapToScreen(max_distance, y + 1.2, z + 1.22, &path_end_right_vertex);
auto lead_two = sm["radarState"].getRadarState().getLeadTwo();
if (lead_two.getRadar() && lead_two.getDRel() > lead_one.getDRel() + 3.0) {
z = line.getZ()[get_path_length_idx(line, lead_two.getDRel())];
y = -lead_two.getYRel();
_model->mapToScreen(lead_two.getDRel(), y - 1.2, z + 1.22, &lead_two_left);
_model->mapToScreen(lead_two.getDRel(), y + 1.2, z + 1.22, &lead_two_right);
if (lead_two_status > 0) {
lead_two_xl = lead_two_xl * 0.8 + lead_two_left.x() * 0.2;
lead_two_xr = lead_two_xr * 0.8 + lead_two_right.x() * 0.2;
lead_two_y = lead_two_y * 0.8 + lead_two_left.y() * 0.2;
}
else {
lead_two_xl = lead_two_left.x();
lead_two_xr = lead_two_right.x();
lead_two_y = lead_two_left.y();
}
if(lp.getLongitudinalPlanSource() == cereal::LongitudinalPlan::LongitudinalPlanSource::LEAD1) {
lead_two_status = 2;
}
else {
lead_two_status = 1;
}
}
else {
lead_two_status = 0;
}
float lex = path_end_left_vertex.x();
float ley = path_end_left_vertex.y();
float rex = path_end_right_vertex.x();
@@ -782,7 +817,18 @@ public:
ui_draw_line2(s, px, py, 7, &pcolor, nullptr, 3.0f);
}
if (isLeadDetected()) {
NVGcolor radar_stroke = isRadarDetected() ? rcolor : COLOR_BLUE;
NVGcolor radar_stroke = COLOR_BLUE;
if (lead_two_status > 0) {
radar_stroke = COLOR_OCHRE;
int path_width2 = lead_two_xr - lead_two_xl;
ui_fill_rect(
s->vg,
{ (int)(lead_two_xl - 10), (int)(lead_two_y - path_width2 * 0.8), (int)(path_width2 + 20), (int)(path_width2 * 0.8) },
(lead_two_status == 2) ? COLOR_RED_ALPHA(50) : COLOR_BLACK_ALPHA(20),
15, 3, &radar_stroke
);
}
radar_stroke = isRadarDetected() ? rcolor : COLOR_BLUE;
ui_fill_rect(s->vg, { (int)(path_x - path_width / 2 - 10), (int)(path_y - path_width * 0.8), (int)(path_width + 20), (int)(path_width * 0.8) }, COLOR_BLACK_ALPHA(20), 15, 3, &radar_stroke);
#if 0
px[0] = path_x - path_width / 2 - 10;
@@ -1893,10 +1939,11 @@ private:
}
};
#if 0
typedef struct {
float x, y, d, v, y_rel, v_lat, radar;
float x, y, d, v, y_rel, v_lat, radar, model_prob, score;
} lead_vertex_data;
#endif
char carrot_man_debug[128] = "";
class DrawCarrot : public QObject {
@@ -1932,9 +1979,9 @@ public:
int nav_path_vertex_count = 0;
bool nav_path_display = false;
std::vector<lead_vertex_data> lead_vertices_side;
//std::vector<lead_vertex_data> lead_vertices_side;
void updateState(UIState *s) {
bool updateState(UIState *s) {
const SubMaster& sm = *(s->sm);
const bool cs_alive = sm.alive("carState");
const bool car_state_alive = sm.alive("carState");
@@ -1953,7 +2000,7 @@ public:
const auto lane_lines = model.getLaneLines();
nav_path_display = params.getInt("ShowRouteInfo");
if (!cs_alive || !car_control_alive || !car_state_alive || !lp_alive) return;
if (!cs_alive || !car_control_alive || !car_state_alive || !lp_alive) return false;
auto selfdrive_state = sm["selfdriveState"].getSelfdriveState();
longActive = selfdrive_state.getEnabled();
latActive = car_control.getLatActive();
@@ -2033,6 +2080,7 @@ public:
s->max_distance = std::clamp((float)lead_d, 0.0f, s->max_distance);
}
#if 0
lead_vertices_side.clear();
for (auto const& rs : { radar_state.getLeadsLeft(), radar_state.getLeadsRight(), radar_state.getLeadsCenter() }) {
for (auto const& l : rs) {
@@ -2048,18 +2096,109 @@ public:
vd.y_rel = l.getDPath();// l.getYRel();
vd.v_lat = l.getVLat();
vd.radar = l.getRadar();
vd.model_prob = l.getModelProb();
vd.score = l.getScore();
lead_vertices_side.push_back(vd);
}
}
}
#endif
return true;
}
void drawRadarInfo(UIState* s) {
char str[128];
int show_radar_info = params.getInt("ShowRadarInfo");
float radar_lat_factor = params.getFloat("RadarLatFactor") / 100;
if (show_radar_info > 0) {
const SubMaster& sm = *(s->sm);
const auto radar_state = sm["radarState"].getRadarState();
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto lane_lines = model.getLaneLines();
int wStr = 40;
for (auto const& rs : { radar_state.getLeadsLeft(), radar_state.getLeadsRight(), radar_state.getLeadsCenter() }) {
for (auto const& l : rs) {
QPointF side, a_side;
float x, y, z, ax, ay;
float v, v_lat, y_rel;
float t = radar_lat_factor; // 예측 시간
float model_prob = 0.0f;
//float score = 0.0f;
float dRel = l.getDRel();
// 현재점 투영
z = lane_lines[2].getZ()[get_path_length_idx(lane_lines[2], l.getDRel())] - 0.61f;
if (dRel > 2.5 && _model->mapToScreen(dRel, -l.getYRel(), z, &side)) {
x = side.x();
y = side.y();
v = l.getVLeadK();
v_lat = l.getVLat();
y_rel = l.getYRel();
bool radar = l.getRadar();
model_prob = l.getModelProb();
//score = l.getScore();
// 속도 크기/표시값 (v_ego 없이 간단 처리)
float v_abs = std::sqrt(v * v + v_lat * v_lat);
float v_sum = (v >= 0.f) ? v_abs : -v_abs;
if (v_abs > 3.0f) {
// 미래점(월드) 계산
float a_dRel = dRel + v * t;
if (a_dRel < 2.0f) a_dRel = 2.0f;
float a_yRel = y_rel + v_lat * t;
// 미래점 투영 (y는 기존과 동일하게 부호 반전)
if (std::fabs(v) > 3.0f && _model->mapToScreen(a_dRel, -a_yRel, z, &a_side)) { // (필요시 z를 a_dRel로 재계산 권장)
ax = a_side.x();
ay = a_side.y();
QPolygonF vertext;
vertext.push_back(side);
vertext.push_back(a_side);
ui_draw_line(s, vertext, nullptr, nullptr, 3.0, (v_sum > 0.f)? COLOR_GREEN: COLOR_RED);
nvgBeginPath(s->vg);
nvgCircle(s->vg, ax, ay, 10);
nvgFillColor(s->vg, (v_sum > 0.f) ? COLOR_GREEN : COLOR_RED);
nvgFill(s->vg);
}
// 속도 텍스트/박스
sprintf(str, "%.0f", (s->scene.is_metric) ? v_sum * MS_TO_KPH : v_sum * MS_TO_MPH);
wStr = 35 * (int)strlen(str);
ui_fill_rect(s->vg,
{ (int)(x - wStr / 2), (int)(y - 35), wStr, 42 },
(!radar) ? COLOR_BLUE : (model_prob == 0.01f) ? COLOR_GREEN : (v_sum > 0.f) ? COLOR_ORANGE : COLOR_RED,
15);
ui_draw_text(s, x, y, str, 40, COLOR_WHITE, BOLD);
if (show_radar_info >= 2) {
sprintf(str, "%.1f", y_rel);
ui_draw_text(s, x, y - 40, str, 30, COLOR_WHITE, BOLD);
sprintf(str, "%.1f", (s->scene.is_metric)? dRel : dRel * KM_TO_MILE);
ui_draw_text(s, x, y + 30, str, 30, COLOR_WHITE, BOLD);
}
}
else if (show_radar_info >= 3) {
strcpy(str, "*");
ui_draw_text(s, x, y, str, 40, COLOR_WHITE, BOLD);
}
}
}
}
}
}
#if 0
void drawRadarInfo_old(UIState* s) {
char str[128];
int show_radar_info = params.getInt("ShowRadarInfo");
if (show_radar_info > 0) {
int wStr = 40;
for (auto const& vrd : lead_vertices_side) {
auto [rx, ry, rd, rv, ry_rel, v_lat, radar] = vrd;
auto [rx, ry, rd, rv, ry_rel, v_lat, radar, model_prob, score] = vrd;
float v_abs = 0.0;
float v_sum = 0.0;
if (v_ego > 1.0) v_sum = v_abs = rv;
@@ -2071,28 +2210,16 @@ public:
if (v_sum < -1.0 || v_sum > 1.0) {
sprintf(str, "%.0f", (s->scene.is_metric)? v_sum * MS_TO_KPH : v_sum * MS_TO_MPH);
wStr = 35 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(rx - wStr / 2), (int)(ry - 35), wStr, 42 }, (!radar) ? COLOR_BLUE : (v_sum > 0.) ? COLOR_GREEN : COLOR_RED, 15);
ui_fill_rect(s->vg, { (int)(rx - wStr / 2), (int)(ry - 35), wStr, 42 }, (!radar) ? COLOR_BLUE : (model_prob==0.01f) ? COLOR_ORANGE : (v_sum > 0.) ? COLOR_GREEN : COLOR_RED, 15);
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
if (show_radar_info >= 2) {
sprintf(str, "%.1f", ry_rel);
ui_draw_text(s, rx, ry - 40, str, 30, COLOR_WHITE, BOLD);
sprintf(str, "%.2f", v_lat);
sprintf(str, "%.3f", score);
//sprintf(str, "%.2f", rd);
ui_draw_text(s, rx, ry + 30, str, 30, COLOR_WHITE, BOLD);
}
}
#if 0
else if (v_lat < -1.0 || v_lat > 1.0) {
sprintf(str, "%.0f", (rv + v_lat) * 3.6);
wStr = 35 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(rx - wStr / 2), (int)(ry - 35), wStr, 42 }, COLOR_ORANGE, 15);
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
if (s->show_radar_info >= 2) {
sprintf(str, "%.1f", ry_rel);
ui_draw_text(s, rx, ry - 40, str, 30, COLOR_WHITE, BOLD);
}
}
#endif
else if (show_radar_info >= 3) {
strcpy(str, "*");
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
@@ -2101,6 +2228,7 @@ public:
}
}
#endif
void drawDebug(UIState* s) {
if (params.getInt("ShowDebugUI") > 1) {
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BOTTOM);
@@ -2711,7 +2839,7 @@ void ui_draw(UIState *s, ModelRenderer* model_renderer, int w, int h) {
//nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
//ui_draw_text(s, 500, 500, "Carrot", 100, COLOR_GREEN, BOLD);
Params params;
drawCarrot.updateState(s);
bool draw_carrot = drawCarrot.updateState(s);
drawCarrot.drawNaviPath(s);
static float pathDrawSeq = 0.0;
int show_lane_info = params.getInt("ShowLaneInfo");
@@ -2728,7 +2856,8 @@ void ui_draw(UIState *s, ModelRenderer* model_renderer, int w, int h) {
drawBlindSpot.draw(s);
drawCarrot.drawRadarInfo(s);
if(draw_carrot)
drawCarrot.drawRadarInfo(s);
drawCarrot.drawHud(s);
system/loggerd/uploader.py
+2 -2
View File
@@ -86,8 +86,8 @@ class Uploader:
self.immediate_folders = ["crash/", "boot/"]
self.immediate_priority = {"qlog": 0, "qlog.zst": 0, "qcamera.ts": 1}
if (self.params.get_int("EnableConnect") == 2):
self.immediate_priority.update({"rlog": 0, "rlog.zst": 0})
#if (self.params.get_int("EnableConnect") == 2):
# self.immediate_priority.update({"rlog": 0, "rlog.zst": 0})
def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]:
r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8")