mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-31 21:23:49 +08:00
Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 8d246f3c0d | |||
| b2885f9f89 | |||
| 01599193df |
@@ -221,11 +221,6 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
|
||||
trackingLead @36 :Bool;
|
||||
stopSignConfirmed @37 :Bool;
|
||||
pulseGlideCoasting @38 :Bool; # developer-only P&G phase for on-road status UI
|
||||
# Curve Speed Controller diagnostics, for tuning and rollout validation
|
||||
cscOverridden @39 :Bool; # driver cancelled this curve with RES+
|
||||
cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin
|
||||
cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m
|
||||
approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off
|
||||
}
|
||||
|
||||
struct StarPilotRadarState @0xb86e6369214c01c8 {
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 13 KiB After Width: | Height: | Size: 11 KiB |
@@ -169,27 +169,12 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 # 1/m
|
||||
CURVATURE_HOLD_CONFIRM_MIN = 0.003 # 1/m (~7 deg) of wound curvature before capture
|
||||
CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation
|
||||
|
||||
# Suppress low-speed action spikes while the model's spatial path remains straight.
|
||||
TWITCH_GUARD_MAX_SPEED = 4.0
|
||||
TWITCH_GUARD_FADE_SPEED = 3.0
|
||||
TWITCH_GUARD_DURATION = 1.5
|
||||
TWITCH_GUARD_PLAN_RATIO = 4.0
|
||||
TWITCH_GUARD_FLOOR = 0.002
|
||||
TWITCH_GUARD_STRAIGHT_LO = 0.005
|
||||
TWITCH_GUARD_STRAIGHT_HI = 0.014
|
||||
TWITCH_GUARD_MIN_REACH = 12.0
|
||||
|
||||
|
||||
def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
|
||||
# Fit curvature through the plan point at the requested lookahead.
|
||||
# curvature of the circle through the origin, tangent to the car's heading, passing
|
||||
# through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
|
||||
px, py = 0.0, 0.0
|
||||
for x, y in zip(xs, ys, strict=False):
|
||||
try:
|
||||
x, y = float(x), float(y)
|
||||
except (TypeError, ValueError, OverflowError):
|
||||
return 0.0
|
||||
if not (math.isfinite(x) and math.isfinite(y)):
|
||||
return 0.0
|
||||
for x, y in zip(xs, ys):
|
||||
px, py = x, y
|
||||
if math.hypot(x, y) >= lookahead:
|
||||
break
|
||||
@@ -200,7 +185,11 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
|
||||
|
||||
|
||||
def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
|
||||
# Use the smaller magnitude of near and far probes to avoid early turn bias.
|
||||
# Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
|
||||
# starts immediately, which over-winds wide turns whose arc begins several meters out
|
||||
# (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
|
||||
# car approaches the arc, so the readout self-scales to the turn geometry. Sign
|
||||
# disagreement means no coherent turn ahead: contribute nothing.
|
||||
xs, ys = model_v2.position.x, model_v2.position.y
|
||||
near = _plan_circle_curvature(xs, ys, d_near)
|
||||
far = _plan_circle_curvature(xs, ys, d_far)
|
||||
@@ -233,52 +222,8 @@ def get_plan_turn_onset_dist(model_v2) -> float:
|
||||
|
||||
|
||||
def get_plan_reach(model_v2) -> float:
|
||||
try:
|
||||
xs = model_v2.position.x
|
||||
return float(xs[-1]) if len(xs) else 0.0
|
||||
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
|
||||
return 0.0
|
||||
|
||||
|
||||
def _plan_positions_are_finite(model_v2) -> bool:
|
||||
try:
|
||||
xs, ys = model_v2.position.x, model_v2.position.y
|
||||
return len(xs) == len(ys) and all(
|
||||
math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True)
|
||||
)
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return False
|
||||
|
||||
|
||||
def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
|
||||
if not (math.isfinite(curvature) and math.isfinite(v_ego)):
|
||||
return curvature
|
||||
if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
|
||||
return curvature
|
||||
if not _plan_positions_are_finite(model_v2):
|
||||
return curvature
|
||||
reach = get_plan_reach(model_v2)
|
||||
if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH:
|
||||
return curvature
|
||||
plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y,
|
||||
CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
|
||||
if not math.isfinite(plan):
|
||||
return curvature
|
||||
straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
|
||||
limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
|
||||
if abs(curvature) <= limit:
|
||||
return curvature
|
||||
fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED)
|
||||
fade = min(max(fade, 0.0), 1.0)
|
||||
return curvature + (math.copysign(limit, curvature) - curvature) * fade
|
||||
|
||||
|
||||
def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
|
||||
if not (math.isfinite(remaining) and math.isfinite(v_ego)):
|
||||
return 0.0
|
||||
if standstill or abs(v_ego) <= 0.3:
|
||||
return TWITCH_GUARD_DURATION
|
||||
return max(remaining - DT_CTRL, 0.0)
|
||||
xs = model_v2.position.x
|
||||
return xs[-1] if len(xs) else 0.0
|
||||
|
||||
|
||||
def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
|
||||
@@ -401,7 +346,6 @@ class Controls:
|
||||
self.turn_hold_handoff_t = 0.0
|
||||
self.turn_hold_done = False
|
||||
self.turn_blinker_swept = 0.0
|
||||
self.twitch_guard_remaining = 0.0
|
||||
self.kona_non_scc_lateral_active = False
|
||||
|
||||
self.pose_calibrator = PoseCalibrator()
|
||||
@@ -457,7 +401,6 @@ class Controls:
|
||||
|
||||
def state_control(self):
|
||||
CS = self.sm['carState']
|
||||
self.twitch_guard_remaining = update_twitch_guard(self.twitch_guard_remaining, CS.vEgo, CS.standstill)
|
||||
|
||||
# Update VehicleModel
|
||||
lp = self.sm['liveParameters']
|
||||
@@ -517,11 +460,7 @@ class Controls:
|
||||
# EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
|
||||
# Disable longitudinal so stock ACC works instead
|
||||
self.update_ecu_disable_failed()
|
||||
CC.longActive = (
|
||||
CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and
|
||||
not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and
|
||||
not self.ecu_disable_failed
|
||||
)
|
||||
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
|
||||
|
||||
actuators = CC.actuators
|
||||
actuators.longControlState = self.LoC.long_control_state
|
||||
@@ -560,9 +499,6 @@ class Controls:
|
||||
# here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
|
||||
# angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
|
||||
blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
|
||||
if (CC.latActive and self.twitch_guard_remaining > 0.0 and
|
||||
blinker_dir == 0.0 and self.turn_hold_curvature == 0.0):
|
||||
new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
|
||||
# heading swept in the blinker's direction over the whole blinker cycle (any speed):
|
||||
# discriminates a turn not yet made from one being exited (see the re-arm below)
|
||||
if blinker_dir == 0.0:
|
||||
|
||||
@@ -85,8 +85,7 @@ class LaneCenteringController:
|
||||
def _covers(x, distance: float) -> bool:
|
||||
return bool(x[0] <= distance <= x[-1])
|
||||
|
||||
@staticmethod
|
||||
def _raw_correction(model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
|
||||
def _raw_correction(self, model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
|
||||
try:
|
||||
lane_lines = model_v2.laneLines
|
||||
probs = np.asarray(model_v2.laneLineProbs, dtype=float)
|
||||
@@ -106,13 +105,11 @@ class LaneCenteringController:
|
||||
right_y = np.asarray(lane_lines[2].y, dtype=float)
|
||||
pos_x = np.asarray(model_v2.position.x, dtype=float)
|
||||
pos_y = np.asarray(model_v2.position.y, dtype=float)
|
||||
if not (LaneCenteringController._valid_path(left_x, left_y) and
|
||||
LaneCenteringController._valid_path(right_x, right_y) and
|
||||
LaneCenteringController._valid_path(pos_x, pos_y)):
|
||||
if not (self._valid_path(left_x, left_y) and self._valid_path(right_x, right_y) and self._valid_path(pos_x, pos_y)):
|
||||
return False, 0.0
|
||||
|
||||
lookahead = float(np.clip(v_ego, 8.0, 35.0))
|
||||
if not all(LaneCenteringController._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
|
||||
if not all(self._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
|
||||
return False, 0.0
|
||||
|
||||
left = float(np.interp(lookahead, left_x, left_y))
|
||||
@@ -133,7 +130,7 @@ class LaneCenteringController:
|
||||
|
||||
try:
|
||||
pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
|
||||
if LaneCenteringController._valid_path(pos_x, pos_y_std):
|
||||
if self._valid_path(pos_x, pos_y_std):
|
||||
path_std = float(np.interp(lookahead, pos_x, pos_y_std))
|
||||
if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
|
||||
break_in = np.clip(
|
||||
@@ -148,43 +145,3 @@ class LaneCenteringController:
|
||||
return True, float(2.0 * error / lookahead ** 2)
|
||||
except (AttributeError, IndexError, TypeError, ValueError):
|
||||
return False, 0.0
|
||||
|
||||
|
||||
def get_raw_lane_centering_correction(model_v2, v_ego: float, offset: float,
|
||||
e2e_authority: float) -> tuple[bool, float]:
|
||||
"""Return the instantaneous lane-centering correction without controller filtering."""
|
||||
return LaneCenteringController._raw_correction(model_v2, v_ego, offset, e2e_authority)
|
||||
|
||||
|
||||
def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e2e_authority: float,
|
||||
enabled: bool, lat_active: bool, pause_on_signal: bool = False,
|
||||
turn_signal_active: bool = False,
|
||||
applied_correction: float | None = None) -> int:
|
||||
"""Return 1 for a right correction, -1 for left, and 0 when no correction is active."""
|
||||
if not enabled or not lat_active or (pause_on_signal and turn_signal_active):
|
||||
return 0
|
||||
|
||||
try:
|
||||
v_ego = float(v_ego)
|
||||
offset = float(offset)
|
||||
e2e_authority = float(e2e_authority)
|
||||
if not np.isfinite([v_ego, offset, e2e_authority]).all() or v_ego < _MIN_V_EGO:
|
||||
return 0
|
||||
if model_v2.meta.laneChangeState != log.LaneChangeState.off:
|
||||
return 0
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
return 0
|
||||
|
||||
valid, correction = get_raw_lane_centering_correction(
|
||||
model_v2,
|
||||
v_ego,
|
||||
float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)),
|
||||
float(np.clip(e2e_authority, 0.0, 1.0)),
|
||||
)
|
||||
if not valid or not np.isfinite(correction):
|
||||
return 0
|
||||
if correction == 0.0:
|
||||
if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0:
|
||||
return 0
|
||||
correction = applied_correction
|
||||
return 1 if correction > 0.0 else -1
|
||||
|
||||
@@ -9,7 +9,6 @@ from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
|
||||
from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
|
||||
@@ -382,11 +381,6 @@ def get_vehicle_min_accel(CP, v_ego):
|
||||
|
||||
# Restored planner constants retained by CEM, stop, and departure paths.
|
||||
A_CRUISE_MIN = -1.0
|
||||
# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
|
||||
# so it stays silent and deceleration sags. Multiplicative so the trim scales with what is
|
||||
# left. Note the car parks where the obstacle sits, so this is also a placement bias — 0.85
|
||||
# stopped ~4.6 m short, 0.93 ~1.6 m.
|
||||
FORCE_STOP_OBSTACLE_TRIM = 0.93
|
||||
STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25
|
||||
STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08
|
||||
STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1
|
||||
@@ -2223,17 +2217,8 @@ class LongitudinalPlanner:
|
||||
force_stop_x = None
|
||||
force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint)
|
||||
if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m:
|
||||
stop_length = float(sm['starpilotPlan'].forcingStopLength)
|
||||
else:
|
||||
# pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag;
|
||||
# getattr so a stale cereal build degrades to the old behaviour instead of raising
|
||||
stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0))
|
||||
if stop_length > force_stop_handoff_m:
|
||||
# ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has
|
||||
# to shift the obstacle too or the slider barely moves anything now that stop_x leads.
|
||||
offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0)))
|
||||
force_stop_x = (
|
||||
stop_length * FORCE_STOP_OBSTACLE_TRIM + offset_ft * FT_TO_M + STOP_DISTANCE +
|
||||
float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
|
||||
get_force_stop_distance_bias(self.CP.carFingerprint)
|
||||
)
|
||||
|
||||
|
||||
@@ -53,8 +53,6 @@ ADJACENT_STOP_REST_FRAMES = 15
|
||||
ADJACENT_STOP_MIN_Y = 1.8 # m — inside this is our own lane
|
||||
ADJACENT_STOP_MAX_Y = 7.5 # m — beyond this is roadside, not an adjacent lane
|
||||
ADJACENT_STOP_MAX_D = 110.0 # m
|
||||
ADJACENT_STOP_QUEUE_GAP_M = 5.0 # m — anything stopped beyond the furthest qualifier means
|
||||
# the bar is past it too, so the hint would stop us short
|
||||
|
||||
|
||||
class KalmanParams:
|
||||
@@ -181,10 +179,6 @@ class Track:
|
||||
if self.leadTrackID == self.identifier:
|
||||
return False
|
||||
|
||||
return self.in_adjacent_lane(model_data)
|
||||
|
||||
def in_adjacent_lane(self, model_data: capnp._DynamicStructReader):
|
||||
"""Lane geometry only, no deceleration history — also used to spot a queue ahead."""
|
||||
if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
|
||||
return False
|
||||
|
||||
@@ -404,10 +398,9 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
|
||||
def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
|
||||
"""Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
|
||||
|
||||
Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches
|
||||
past it. Cars already stopped when we acquire them never show the moving -> stopped
|
||||
transition, so the qualifying set is biased toward the back of a line; without this the
|
||||
hint marks a mid-queue bumper and stops us short of the bar.
|
||||
Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the
|
||||
rest are closer to us, so the nearest one underestimates the distance. The consumer only
|
||||
shortens with this, so underestimating is the harmful direction.
|
||||
"""
|
||||
if len(model_data.laneLines) < 4:
|
||||
return {'status': False}
|
||||
@@ -417,11 +410,6 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr
|
||||
return {'status': False}
|
||||
|
||||
furthest = max(candidates, key=lambda c: c.dRel)
|
||||
for c in tracks.values():
|
||||
if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and
|
||||
abs(c.vLead) < ADJACENT_STOP_REST_V and
|
||||
c.in_adjacent_lane(model_data)):
|
||||
return {'status': False}
|
||||
return {
|
||||
'status': True,
|
||||
'dRel': float(furthest.dRel),
|
||||
|
||||
@@ -3,7 +3,7 @@ from types import SimpleNamespace
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController, get_lane_centering_visual_direction
|
||||
from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
|
||||
|
||||
|
||||
_V_EGO = 20.0
|
||||
@@ -174,20 +174,3 @@ def test_correction_is_smoothed_and_capped():
|
||||
_, steady = _converge(model, authority=0.0)
|
||||
assert 0.0 < first < steady
|
||||
assert np.isclose(steady, 0.004 * 0.30, atol=1e-6)
|
||||
|
||||
|
||||
def test_visual_direction_matches_curvature_sign():
|
||||
# Positive curvature is right in this tree's convention.
|
||||
assert get_lane_centering_visual_direction(_model(left=-1.5, right=2.1), _V_EGO, 0.0, 0.0, True, True) == 1
|
||||
assert get_lane_centering_visual_direction(_model(left=-2.1, right=1.5), _V_EGO, 0.0, 0.0, True, True) == -1
|
||||
|
||||
|
||||
def test_visual_direction_requires_both_primary_lane_lines():
|
||||
model = _model(left=-1.5, right=2.1)
|
||||
model.laneLineProbs[2] = 0.2
|
||||
assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True) == 0
|
||||
|
||||
|
||||
def test_visual_direction_uses_filtered_correction_in_deadband():
|
||||
model = _model()
|
||||
assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=0.001) == 1
|
||||
|
||||
@@ -526,7 +526,6 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
|
||||
forcingStop=False,
|
||||
redLight=False,
|
||||
forcingStopLength=2,
|
||||
approachStopLength=0.0,
|
||||
),
|
||||
}
|
||||
|
||||
@@ -2991,39 +2990,6 @@ def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch):
|
||||
assert not action.shouldStop
|
||||
|
||||
|
||||
def test_modeld_action_uses_current_action_head_scaling_for_v16(monkeypatch):
|
||||
monkeypatch.setenv("DEBUG", "0")
|
||||
fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")
|
||||
fake_commonmodel.DrivingModelFrame = object
|
||||
fake_commonmodel.CLContext = object
|
||||
monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel)
|
||||
|
||||
from openpilot.selfdrive.modeld import modeld
|
||||
|
||||
prev_action = log.ModelDataV2.Action.new_message()
|
||||
prev_action.desiredCurvature = 0.05
|
||||
prev_action.desiredAcceleration = -0.2
|
||||
toggles = SimpleNamespace(vEgoStopping=0.42)
|
||||
|
||||
action = modeld.get_action_from_model(
|
||||
{"action": np.array([[12.0, -0.8]], dtype=np.float32)},
|
||||
prev_action,
|
||||
lat_action_t=0.2,
|
||||
long_action_t=0.73,
|
||||
v_ego=5.0,
|
||||
mlsim=True,
|
||||
is_v9=False,
|
||||
is_v14=False,
|
||||
is_v15=False,
|
||||
starpilot_toggles=toggles,
|
||||
is_v16=True,
|
||||
)
|
||||
|
||||
assert action.desiredCurvature == pytest.approx(modeld.smooth_value(0.48, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS))
|
||||
assert action.desiredAcceleration < -0.2
|
||||
assert not action.shouldStop
|
||||
|
||||
|
||||
def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero():
|
||||
class FakePM:
|
||||
def __init__(self):
|
||||
|
||||
@@ -7,9 +7,7 @@ from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
|
||||
from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
|
||||
from openpilot.starpilot.controls.lib.starpilot_vcruise import (
|
||||
FORCE_STOP_CAP_SLACK_M,
|
||||
FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
|
||||
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME,
|
||||
StarPilotVCruise,
|
||||
get_active_slc_control_target,
|
||||
get_lead_veto_distance,
|
||||
@@ -59,8 +57,6 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
|
||||
vcruise.forcing_stop = forcing_stop
|
||||
vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0
|
||||
vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length
|
||||
# what the not-committed branch would have left behind on the frame before commit
|
||||
vcruise.force_stop_distance_cap = planner.model_length
|
||||
return planner, vcruise
|
||||
|
||||
|
||||
@@ -525,15 +521,14 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
|
||||
|
||||
def test_force_stop_reanchors_when_model_reopens_path_without_stop_action():
|
||||
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
|
||||
planner.model_length = 90.0
|
||||
vcruise.tracked_model_length = 60.0
|
||||
vcruise.force_stop_distance_cap = 90.0
|
||||
planner.model_length = 40.0
|
||||
vcruise.tracked_model_length = 10.0
|
||||
sm = make_sm(standstill=False)
|
||||
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
|
||||
|
||||
result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
|
||||
|
||||
assert vcruise.tracked_model_length == pytest.approx(90.0)
|
||||
assert vcruise.tracked_model_length == pytest.approx(40.0)
|
||||
assert result > 5.0
|
||||
|
||||
|
||||
@@ -565,49 +560,6 @@ def test_santa_fe_force_stop_holds_through_low_speed_detector_dropout():
|
||||
assert result == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_force_stop_does_not_reanchor_inside_reanchor_floor():
|
||||
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
|
||||
planner.model_length = 90.0
|
||||
vcruise.tracked_model_length = 25.0
|
||||
sm = make_sm(standstill=False)
|
||||
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
|
||||
|
||||
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
|
||||
|
||||
assert vcruise.tracked_model_length < 25.0
|
||||
|
||||
|
||||
def test_force_stop_reanchor_bounded_by_distance_driven():
|
||||
# The line can't recede: a ballooning horizon may not push the stop past where it was at
|
||||
# commit minus the distance driven since.
|
||||
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
|
||||
planner.model_length = 200.0
|
||||
vcruise.tracked_model_length = 60.0
|
||||
vcruise.force_stop_distance_cap = 70.0
|
||||
sm = make_sm(standstill=False)
|
||||
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
|
||||
|
||||
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0)
|
||||
|
||||
assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M
|
||||
assert vcruise.tracked_model_length < 100.0 # nowhere near the 200 m the horizon claimed
|
||||
|
||||
|
||||
def test_force_stop_cap_slack_tapers_near_the_line():
|
||||
# Slack protects against an under-read at commit; held near the line it would just aim the
|
||||
# solver that far past the stop bar.
|
||||
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
|
||||
planner.model_length = 200.0
|
||||
vcruise.tracked_model_length = 60.0
|
||||
vcruise.force_stop_distance_cap = 12.0
|
||||
sm = make_sm(standstill=False)
|
||||
sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
|
||||
|
||||
update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0)
|
||||
|
||||
assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0
|
||||
|
||||
|
||||
def test_force_stop_does_not_reanchor_committed_model_stop():
|
||||
planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
|
||||
planner.model_length = 40.0
|
||||
@@ -889,16 +841,16 @@ def test_standstill_light_hold_expires_and_does_not_rearm_from_stopped_model():
|
||||
assert update_vcruise(vcruise, sm, toggles, now=0.0) == pytest.approx(0.0)
|
||||
assert vcruise.standstill_force_stop_reason == "light"
|
||||
|
||||
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME - 0.1) == pytest.approx(0.0)
|
||||
assert update_vcruise(vcruise, sm, toggles, now=4.9) == pytest.approx(0.0)
|
||||
assert vcruise.forcing_stop
|
||||
|
||||
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.1) == pytest.approx(20.0)
|
||||
assert update_vcruise(vcruise, sm, toggles, now=5.1) == pytest.approx(20.0)
|
||||
assert not vcruise.forcing_stop
|
||||
assert not vcruise.standstill_force_stop_hold
|
||||
|
||||
# The red-light model remains stopped, but Force Stop must stay released so
|
||||
# Experimental Mode can own the red-to-green departure.
|
||||
assert update_vcruise(vcruise, sm, toggles, now=STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME + 0.2) == pytest.approx(20.0)
|
||||
assert update_vcruise(vcruise, sm, toggles, now=5.2) == pytest.approx(20.0)
|
||||
assert not vcruise.forcing_stop
|
||||
|
||||
|
||||
|
||||
@@ -1,40 +1,13 @@
|
||||
import math
|
||||
import types
|
||||
|
||||
from cereal import car
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.controlsd import (
|
||||
TWITCH_GUARD_DURATION,
|
||||
TWITCH_GUARD_FLOOR,
|
||||
TWITCH_GUARD_MAX_SPEED,
|
||||
get_control_lateral_smooth_seconds,
|
||||
limit_curvature_to_plan,
|
||||
turn_lead_allowed,
|
||||
update_twitch_guard,
|
||||
)
|
||||
from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed
|
||||
|
||||
|
||||
LateralControlMode = car.CarControl.Actuators.LateralControlMode
|
||||
|
||||
|
||||
def _plan(xs, ys):
|
||||
return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys))
|
||||
|
||||
|
||||
def _arc_plan(radius, n=200):
|
||||
return _plan([radius * math.sin(i / n) for i in range(n)],
|
||||
[radius * (1.0 - math.cos(i / n)) for i in range(n)])
|
||||
|
||||
|
||||
STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)
|
||||
STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0])
|
||||
TURN_PLAN = _arc_plan(30.0)
|
||||
GENTLE_BEND_PLAN = _arc_plan(143.0)
|
||||
|
||||
|
||||
def test_turn_lead_is_suppressed_only_during_applied_angle_control():
|
||||
assert not turn_lead_allowed("rivian", LateralControlMode.angle)
|
||||
assert turn_lead_allowed("rivian", LateralControlMode.torque)
|
||||
@@ -64,79 +37,3 @@ def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected):
|
||||
])
|
||||
def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected):
|
||||
assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("curvature", [0.0155, -0.0155])
|
||||
def test_twitch_against_a_straight_plan_is_clamped_to_the_floor(curvature):
|
||||
guarded = limit_curvature_to_plan(STRAIGHT_PLAN, curvature, 1.2)
|
||||
assert abs(guarded) == pytest.approx(TWITCH_GUARD_FLOOR)
|
||||
assert math.copysign(1.0, guarded) == math.copysign(1.0, curvature)
|
||||
|
||||
|
||||
def test_command_already_below_the_floor_is_untouched():
|
||||
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0015, 1.2) == pytest.approx(0.0015)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("v_ego", [TWITCH_GUARD_MAX_SPEED, 6.0, 30.0])
|
||||
def test_guard_is_inactive_above_its_speed_band(v_ego):
|
||||
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, v_ego) == pytest.approx(0.0155)
|
||||
|
||||
|
||||
def test_guard_fades_out_across_the_speed_band():
|
||||
full = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 1.2)
|
||||
half = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 3.5)
|
||||
assert full < half < 0.0155
|
||||
|
||||
|
||||
@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0])
|
||||
def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
|
||||
action = (1.0 / 30.0) * ratio
|
||||
assert limit_curvature_to_plan(TURN_PLAN, action, 1.2) == pytest.approx(action)
|
||||
|
||||
|
||||
def test_a_barely_bending_plan_does_not_license_a_large_command():
|
||||
guarded = limit_curvature_to_plan(GENTLE_BEND_PLAN, 0.0155, 1.2)
|
||||
assert TWITCH_GUARD_FLOOR < guarded < 0.008
|
||||
|
||||
|
||||
@pytest.mark.parametrize("plan", [STANDSTILL_STUB_PLAN, _plan([], [])])
|
||||
def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan):
|
||||
assert limit_curvature_to_plan(plan, 0.0155, 0.4) == pytest.approx(0.0155)
|
||||
|
||||
|
||||
def test_zero_command_stays_zero():
|
||||
assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("bad_plan", [
|
||||
_plan([0.0, math.nan, 20.0], [0.0, 0.0, 0.0]),
|
||||
_plan([0.0, math.inf, 20.0], [0.0, 0.0, 0.0]),
|
||||
_plan([0.0, 20.0], [0.0]),
|
||||
])
|
||||
def test_invalid_plan_data_leaves_curvature_untouched(bad_plan):
|
||||
assert limit_curvature_to_plan(bad_plan, 0.0155, 1.2) == pytest.approx(0.0155)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
|
||||
def test_nonfinite_guard_inputs_disarm(value):
|
||||
assert update_twitch_guard(value, 1.0, False) == 0.0
|
||||
assert update_twitch_guard(TWITCH_GUARD_DURATION, value, False) == 0.0
|
||||
|
||||
|
||||
def test_twitch_guard_arms_at_standstill_or_creep_speed():
|
||||
assert update_twitch_guard(0.0, 0.0, True) == TWITCH_GUARD_DURATION
|
||||
assert update_twitch_guard(0.0, 0.3, False) == TWITCH_GUARD_DURATION
|
||||
|
||||
|
||||
def test_twitch_guard_decays_after_pullaway_and_expires():
|
||||
remaining = update_twitch_guard(0.0, 0.0, True)
|
||||
remaining = update_twitch_guard(remaining, 1.0, False)
|
||||
assert remaining == pytest.approx(TWITCH_GUARD_DURATION - DT_CTRL)
|
||||
|
||||
for _ in range(int(TWITCH_GUARD_DURATION / DT_CTRL) + 1):
|
||||
remaining = update_twitch_guard(remaining, 1.0, False)
|
||||
assert remaining == 0.0
|
||||
|
||||
|
||||
def test_twitch_guard_does_not_arm_while_moving():
|
||||
assert update_twitch_guard(0.0, 1.0, False) == 0.0
|
||||
|
||||
@@ -204,14 +204,13 @@ def _packed_policy_shapes(input_shapes, include_prev_feature=False):
|
||||
shapes[key] = tuple(shape)
|
||||
if include_prev_feature:
|
||||
features_shape = input_shapes["features_buffer"]
|
||||
shapes["prev_feat"] = (features_shape[0], math.prod(features_shape[2:]))
|
||||
shapes["prev_feat"] = (features_shape[0], features_shape[2])
|
||||
return shapes, [math.prod(shape) for shape in shapes.values()]
|
||||
|
||||
|
||||
def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
|
||||
queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device)
|
||||
features_shape = policy_input_shapes["features_buffer"]
|
||||
feature_dim = math.prod(features_shape[2:])
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
desire_shape = policy_input_shapes[desire_key]
|
||||
packed_shapes, packed_sizes = _packed_policy_shapes(policy_input_shapes)
|
||||
@@ -225,7 +224,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
|
||||
})
|
||||
queues.update({
|
||||
"feat_q": Tensor(
|
||||
np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], feature_dim), dtype=np.float32),
|
||||
np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], features_shape[2]), dtype=np.float32),
|
||||
device=device,
|
||||
).contiguous().realize(),
|
||||
"desire_q": Tensor(
|
||||
@@ -240,7 +239,6 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
|
||||
def make_supercombo_input_queues(input_shapes, frame_skip, device):
|
||||
queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
|
||||
features_shape = input_shapes["features_buffer"]
|
||||
feature_dim = math.prod(features_shape[2:])
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
desire_shape = input_shapes[desire_key]
|
||||
packed_shapes, packed_sizes = _packed_policy_shapes(input_shapes, include_prev_feature=True)
|
||||
@@ -254,7 +252,7 @@ def make_supercombo_input_queues(input_shapes, frame_skip, device):
|
||||
})
|
||||
queues.update({
|
||||
"feat_q": Tensor(
|
||||
np.zeros((frame_skip * features_shape[1], features_shape[0], feature_dim), dtype=np.float32),
|
||||
np.zeros((frame_skip * features_shape[1], features_shape[0], features_shape[2]), dtype=np.float32),
|
||||
device=device,
|
||||
).contiguous().realize(),
|
||||
"desire_q": Tensor(
|
||||
@@ -337,7 +335,6 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order,
|
||||
vision_output = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast("float32")
|
||||
new_feature = vision_output[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
|
||||
features_buffer = shift_and_sample(feat_q, new_feature, sample_skip_fn)
|
||||
features_buffer = features_buffer.reshape(policy_metadata["input_shapes"]["features_buffer"])
|
||||
|
||||
policy_inputs = {
|
||||
"features_buffer": features_buffer,
|
||||
@@ -391,7 +388,6 @@ def make_run_supercombo(model_runner, metadata, frame_skip, image_history_pipeli
|
||||
features_buffer = shift_and_sample(
|
||||
feat_q, previous_feature.reshape(1, 1, -1), sample_skip_fn,
|
||||
)
|
||||
features_buffer = features_buffer.reshape(input_shapes["features_buffer"])
|
||||
model_inputs = {
|
||||
road_key: img,
|
||||
wide_key: big_img,
|
||||
|
||||
+12
-27
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
from collections.abc import Callable
|
||||
import ctypes
|
||||
from functools import cached_property
|
||||
import os
|
||||
import struct
|
||||
@@ -161,10 +159,8 @@ class ChestnutState:
|
||||
if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
|
||||
try:
|
||||
smu = Device["AMD"].iface.dev_impl.smu
|
||||
metrics_t = smu.smu_mod.SmuMetricsExternal_t
|
||||
smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
|
||||
metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
|
||||
metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
|
||||
metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
|
||||
self.metrics = {
|
||||
"tempC": metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
|
||||
"memoryTempC": metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
|
||||
@@ -281,11 +277,10 @@ def _close_tinygrad_disk_cache_connection() -> None:
|
||||
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
|
||||
is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
|
||||
lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS,
|
||||
is_v16: bool = False) -> log.ModelDataV2.Action:
|
||||
if is_v14 or is_v15 or is_v16:
|
||||
lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS) -> log.ModelDataV2.Action:
|
||||
if is_v14 or is_v15:
|
||||
desired_curv_unscaled, desired_accel = model_output['action'][0]
|
||||
if is_v15 or is_v16:
|
||||
if is_v15:
|
||||
desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
|
||||
else:
|
||||
desired_curvature = float(desired_curv_unscaled) / 100.0
|
||||
@@ -469,7 +464,6 @@ class ModelState:
|
||||
self.is_v9 = self.policy_generation == "v9"
|
||||
self.is_v14 = self.policy_generation == "v14"
|
||||
self.is_v15 = self.policy_generation == "v15"
|
||||
self.is_v16 = self.policy_generation == "v16"
|
||||
self.mlsim = is_tinygrad_model_version(self.policy_generation)
|
||||
if write_model_version:
|
||||
params.put("ModelVersion", self.policy_generation)
|
||||
@@ -573,8 +567,7 @@ class ModelState:
|
||||
self._reset_state()
|
||||
|
||||
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool,
|
||||
after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None:
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
frames: dict[str, Tensor] = {}
|
||||
for key, buf in bufs.items():
|
||||
ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
|
||||
@@ -620,12 +613,11 @@ class ModelState:
|
||||
img=img,
|
||||
big_img=big_img,
|
||||
)
|
||||
if after_enqueue is not None:
|
||||
after_enqueue()
|
||||
outputs = [output.numpy().flatten() for output in output_tensors]
|
||||
|
||||
if self.uses_external_gpu and any(not np.isfinite(output).all() for output in outputs):
|
||||
raise RuntimeError("external GPU model output not finite")
|
||||
cloudlog.error("external GPU model output not finite, dropping frame")
|
||||
return None
|
||||
|
||||
if self.model_type == "supercombo":
|
||||
model_output = outputs[0]
|
||||
@@ -927,17 +919,7 @@ def main(demo=False):
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
try:
|
||||
send_chestnut = (
|
||||
chestnut_state is not None and
|
||||
run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0
|
||||
)
|
||||
model_output = model.run(
|
||||
bufs,
|
||||
transforms,
|
||||
inputs,
|
||||
prepare_only,
|
||||
chestnut_state.send if send_chestnut else None,
|
||||
)
|
||||
model_output = model.run(bufs, transforms, inputs, prepare_only)
|
||||
except Exception:
|
||||
if not external_gpu_active or small_model is None:
|
||||
raise
|
||||
@@ -971,7 +953,7 @@ def main(demo=False):
|
||||
lat_action_t,
|
||||
long_action_t,
|
||||
v_ego, model.mlsim, model.is_v9, model.is_v14, model.is_v15, starpilot_toggles,
|
||||
lat_smooth_seconds, long_smooth_seconds, is_v16=model.is_v16,
|
||||
lat_smooth_seconds, long_smooth_seconds,
|
||||
)
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
@@ -1000,6 +982,9 @@ def main(demo=False):
|
||||
if sm.updated['starpilotPlan']:
|
||||
starpilot_toggles = get_starpilot_toggles(sm)
|
||||
|
||||
if chestnut_state is not None and run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0:
|
||||
chestnut_state.send()
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
import argparse
|
||||
|
||||
@@ -3,7 +3,6 @@ from types import MethodType
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.modeld import modeld
|
||||
from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob, tinygrad_dev_config
|
||||
@@ -38,18 +37,17 @@ def test_external_gpu_uses_a_longer_load_watchdog():
|
||||
assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
|
||||
|
||||
|
||||
def test_external_gpu_signal_wait_yields_between_usb_polls(monkeypatch):
|
||||
def test_external_gpu_signal_wait_matches_upstream_busy_poll():
|
||||
from tinygrad.runtime import ops_amd
|
||||
|
||||
sleeps = []
|
||||
monkeypatch.setattr(ops_amd.time, "sleep", sleeps.append)
|
||||
signal = ops_amd.AMDSignal.__new__(ops_amd.AMDSignal)
|
||||
signal.should_return = False
|
||||
signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=lambda _: None))
|
||||
signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=sleeps.append))
|
||||
|
||||
signal._sleep(0)
|
||||
|
||||
assert sleeps == [ops_amd.AMD_USB_POLL_US / 1e6]
|
||||
assert sleeps == []
|
||||
|
||||
|
||||
def test_native_amd_signal_keeps_existing_short_wait_behavior():
|
||||
@@ -203,7 +201,7 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
|
||||
]
|
||||
|
||||
|
||||
def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
|
||||
def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypatch):
|
||||
class FakeTensor:
|
||||
@staticmethod
|
||||
def from_blob(*_args, **_kwargs):
|
||||
@@ -237,7 +235,13 @@ def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
|
||||
state.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY
|
||||
state.warp_enqueue = lambda **_kwargs: object()
|
||||
state.run_policy = lambda **_kwargs: (FakeOutput(),)
|
||||
state._reset_state = MethodType(
|
||||
lambda self: (_ for _ in ()).throw(AssertionError("upstream does not reset or escalate transient non-finite output")),
|
||||
state,
|
||||
)
|
||||
|
||||
monkeypatch.setattr(modeld, "Tensor", FakeTensor)
|
||||
monkeypatch.setattr(modeld.cloudlog, "error", lambda *_args, **_kwargs: None)
|
||||
buffers = {
|
||||
"img": SimpleNamespace(data=bytearray(4)),
|
||||
"big_img": SimpleNamespace(data=bytearray(4)),
|
||||
@@ -248,10 +252,8 @@ def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
|
||||
}
|
||||
inputs = {"desire_pulse": np.zeros(8, dtype=np.float32)}
|
||||
|
||||
callbacks = []
|
||||
with pytest.raises(RuntimeError, match="external GPU model output not finite"):
|
||||
state.run(buffers, transforms, inputs, False, lambda: callbacks.append("sent"))
|
||||
assert callbacks == ["sent"]
|
||||
for _ in range(10):
|
||||
assert state.run(buffers, transforms, inputs, False) is None
|
||||
|
||||
|
||||
def test_out_of_band_artifact_round_trip():
|
||||
|
||||
@@ -1,11 +1,6 @@
|
||||
import json
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
import urllib.error
|
||||
import urllib.request
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
from enum import IntEnum
|
||||
|
||||
import pyray as rl
|
||||
@@ -23,40 +18,6 @@ from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.widgets.scroller import NavScroller
|
||||
|
||||
UPDATER_TIMEOUT = 10.0
|
||||
FAST_UPDATE_HOLD_SECONDS = 0.8
|
||||
FAST_UPDATE_POLL_SECONDS = 0.5
|
||||
FAST_UPDATE_REQUEST_TIMEOUT = 5.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class FastUpdateDisplayState:
|
||||
stage: str = "idle"
|
||||
message: str = ""
|
||||
error: str = ""
|
||||
|
||||
|
||||
def _galaxy_api_url(path: str) -> str:
|
||||
port = os.getenv("SP_GALAXY_PORT", "8082")
|
||||
return f"http://127.0.0.1:{port}/api/update/fast{path}"
|
||||
|
||||
|
||||
def _galaxy_fast_update_request(path: str = "", method: str = "GET") -> dict:
|
||||
request = urllib.request.Request(_galaxy_api_url(path), method=method)
|
||||
try:
|
||||
with urllib.request.urlopen(request, timeout=FAST_UPDATE_REQUEST_TIMEOUT) as response:
|
||||
payload = json.loads(response.read().decode("utf-8"))
|
||||
except urllib.error.HTTPError as error:
|
||||
try:
|
||||
payload = json.loads(error.read().decode("utf-8"))
|
||||
except (json.JSONDecodeError, UnicodeDecodeError):
|
||||
payload = {}
|
||||
raise RuntimeError(payload.get("error") or str(error)) from error
|
||||
except (OSError, urllib.error.URLError, json.JSONDecodeError, UnicodeDecodeError) as error:
|
||||
raise RuntimeError(f"Galaxy fast update unavailable: {error}") from error
|
||||
|
||||
if not isinstance(payload, dict):
|
||||
raise RuntimeError("Galaxy returned an invalid fast update response")
|
||||
return payload
|
||||
|
||||
|
||||
def _split_description(desc: str) -> tuple[str, str, str, str] | None:
|
||||
@@ -140,10 +101,6 @@ class CheckUpdateButton(BigButton):
|
||||
self._waiting_for_updater_t: float | None = None
|
||||
self._hide_value_t: float | None = None
|
||||
self._state: UpdaterState = UpdaterState.IDLE
|
||||
self._press_start_t: float | None = None
|
||||
self._press_action = ""
|
||||
self._fast_update_state = FastUpdateDisplayState()
|
||||
self._fast_update_state_lock = threading.Lock()
|
||||
|
||||
ui_state.add_offroad_transition_callback(self.offroad_transition)
|
||||
|
||||
@@ -151,24 +108,9 @@ class CheckUpdateButton(BigButton):
|
||||
if ui_state.is_offroad():
|
||||
self.set_enabled(True)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_press(mouse_pos)
|
||||
self._press_start_t = rl.get_time()
|
||||
self._press_action = self.get_value()
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
held_for = 0.0 if self._press_start_t is None else rl.get_time() - self._press_start_t
|
||||
self._press_start_t = None
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
if held_for >= FAST_UPDATE_HOLD_SECONDS:
|
||||
self._show_fast_update_confirmation()
|
||||
return
|
||||
|
||||
if self._get_fast_update_state().stage == "error":
|
||||
self._set_fast_update_state(stage="idle")
|
||||
return
|
||||
|
||||
if not system_time_valid():
|
||||
dlg = BigDialog("", tr("Please connect to Wi-Fi to update."))
|
||||
gui_app.push_widget(dlg)
|
||||
@@ -179,75 +121,13 @@ class CheckUpdateButton(BigButton):
|
||||
self.set_icon(self._txt_update_icon)
|
||||
|
||||
def run():
|
||||
if self._press_action == "download update":
|
||||
if self.get_value() == "download update":
|
||||
_request_update_download()
|
||||
else:
|
||||
_request_update_check()
|
||||
|
||||
threading.Thread(target=run, daemon=True).start()
|
||||
|
||||
def _show_fast_update_confirmation(self):
|
||||
if not system_time_valid():
|
||||
gui_app.push_widget(BigDialog("", tr("Please connect to Wi-Fi to update.")))
|
||||
return
|
||||
if ui_state.started:
|
||||
return
|
||||
|
||||
gui_app.push_widget(BigConfirmationDialog(
|
||||
"slide to\nfast update",
|
||||
self._txt_update_icon,
|
||||
self._start_fast_update,
|
||||
red=True,
|
||||
))
|
||||
|
||||
def _set_fast_update_state(self, *, stage: str, message: str = "", error: str = ""):
|
||||
with self._fast_update_state_lock:
|
||||
self._fast_update_state = FastUpdateDisplayState(stage, message, error)
|
||||
|
||||
def _get_fast_update_state(self) -> FastUpdateDisplayState:
|
||||
with self._fast_update_state_lock:
|
||||
state = self._fast_update_state
|
||||
return FastUpdateDisplayState(state.stage, state.message, state.error)
|
||||
|
||||
def _start_fast_update(self):
|
||||
if ui_state.started:
|
||||
return
|
||||
|
||||
state = self._get_fast_update_state()
|
||||
if state.stage not in ("idle", "error"):
|
||||
return
|
||||
|
||||
self._set_fast_update_state(stage="starting", message="starting fast update...")
|
||||
threading.Thread(target=self._run_fast_update, daemon=True).start()
|
||||
|
||||
def _run_fast_update(self):
|
||||
try:
|
||||
_galaxy_fast_update_request(method="POST")
|
||||
|
||||
while True:
|
||||
status = _galaxy_fast_update_request("/status")
|
||||
stage = str(status.get("stage") or "updating")
|
||||
error = str(status.get("lastError") or "").strip()
|
||||
message = str(
|
||||
status.get("progressDetail") or
|
||||
status.get("progressLabel") or
|
||||
status.get("message") or
|
||||
"fast update in progress..."
|
||||
).strip()
|
||||
|
||||
if error or stage == "error":
|
||||
self._set_fast_update_state(stage="error", message="fast update failed", error=error or message)
|
||||
return
|
||||
|
||||
self._set_fast_update_state(stage=stage, message=message)
|
||||
if not bool(status.get("running")):
|
||||
return
|
||||
time.sleep(FAST_UPDATE_POLL_SECONDS)
|
||||
except Exception as error:
|
||||
current = self._get_fast_update_state()
|
||||
if current.stage != "rebooting":
|
||||
self._set_fast_update_state(stage="error", message="fast update failed", error=str(error))
|
||||
|
||||
def set_value(self, value: str):
|
||||
super().set_value(value)
|
||||
self.set_text("" if value else "check for update")
|
||||
@@ -256,25 +136,9 @@ class CheckUpdateButton(BigButton):
|
||||
super()._update_state()
|
||||
|
||||
if ui_state.started:
|
||||
self._press_start_t = None
|
||||
self.set_enabled(False)
|
||||
return
|
||||
|
||||
fast_update_state = self._get_fast_update_state()
|
||||
if fast_update_state.stage != "idle":
|
||||
self.set_rotate_icon(fast_update_state.stage not in ("error", "rebooting"))
|
||||
if fast_update_state.stage == "error":
|
||||
self.set_enabled(True)
|
||||
self.set_value(fast_update_state.error or fast_update_state.message)
|
||||
elif fast_update_state.stage == "rebooting":
|
||||
self.set_enabled(False)
|
||||
self.set_value("update complete\nrebooting...")
|
||||
else:
|
||||
self.set_enabled(False)
|
||||
self.set_value(fast_update_state.message or "fast update in progress...")
|
||||
self.set_text("fast update")
|
||||
return
|
||||
|
||||
updater_state = ui_state.params.get("UpdaterState") or ""
|
||||
|
||||
if self._state == UpdaterState.WAITING_FOR_UPDATER:
|
||||
|
||||
@@ -1,98 +0,0 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.selfdrive.ui.mici.layouts.settings import software
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton
|
||||
|
||||
|
||||
def _make_button() -> software.CheckUpdateButton:
|
||||
button = object.__new__(software.CheckUpdateButton)
|
||||
button._press_start_t = None
|
||||
button._press_action = ""
|
||||
button._fast_update_state = software.FastUpdateDisplayState()
|
||||
button._fast_update_state_lock = software.threading.Lock()
|
||||
return button
|
||||
|
||||
|
||||
def test_fast_update_uses_local_galaxy_api(monkeypatch):
|
||||
monkeypatch.delenv("SP_GALAXY_PORT", raising=False)
|
||||
|
||||
assert software._galaxy_api_url("") == "http://127.0.0.1:8082/api/update/fast"
|
||||
assert software._galaxy_api_url("/status") == "http://127.0.0.1:8082/api/update/fast/status"
|
||||
|
||||
|
||||
def test_long_press_opens_fast_update_confirmation(monkeypatch):
|
||||
button = _make_button()
|
||||
button._press_start_t = 1.0
|
||||
confirmation_calls = []
|
||||
|
||||
monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS)
|
||||
monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
|
||||
monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmation_calls.append(True))
|
||||
|
||||
button._handle_mouse_release(SimpleNamespace())
|
||||
|
||||
assert confirmation_calls == [True]
|
||||
|
||||
|
||||
def test_short_press_keeps_normal_updater_path(monkeypatch):
|
||||
button = _make_button()
|
||||
button._press_start_t = 1.0
|
||||
button._press_action = "download update"
|
||||
downloads = []
|
||||
confirmations = []
|
||||
|
||||
monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS - 0.1)
|
||||
monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
|
||||
monkeypatch.setattr(software, "system_time_valid", lambda: True)
|
||||
monkeypatch.setattr(software, "_request_update_download", lambda: downloads.append(True))
|
||||
monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmations.append(True))
|
||||
|
||||
class ImmediateThread:
|
||||
def __init__(self, target, daemon):
|
||||
self.target = target
|
||||
|
||||
def start(self):
|
||||
self.target()
|
||||
|
||||
monkeypatch.setattr(software.threading, "Thread", ImmediateThread)
|
||||
button.set_enabled = lambda *_args: None
|
||||
button.set_icon = lambda *_args: None
|
||||
button._state = software.UpdaterState.IDLE
|
||||
button._txt_update_icon = None
|
||||
|
||||
button._handle_mouse_release(SimpleNamespace())
|
||||
|
||||
assert downloads == [True]
|
||||
assert confirmations == []
|
||||
|
||||
|
||||
def test_fast_update_worker_uses_galaxy_endpoint(monkeypatch):
|
||||
button = _make_button()
|
||||
requests = []
|
||||
responses = [
|
||||
{"message": "started"},
|
||||
{
|
||||
"running": True,
|
||||
"stage": "updating",
|
||||
"progressDetail": "Fetching latest shallow commit...",
|
||||
"lastError": "",
|
||||
},
|
||||
{
|
||||
"running": False,
|
||||
"stage": "rebooting",
|
||||
"progressDetail": "Update complete. Please wait for device to reboot.",
|
||||
"lastError": "",
|
||||
},
|
||||
]
|
||||
|
||||
def request(path="", method="GET"):
|
||||
requests.append((path, method))
|
||||
return responses.pop(0)
|
||||
|
||||
monkeypatch.setattr(software, "_galaxy_fast_update_request", request)
|
||||
monkeypatch.setattr(software.time, "sleep", lambda *_args: None)
|
||||
|
||||
button._run_fast_update()
|
||||
|
||||
assert requests == [("", "POST"), ("/status", "GET"), ("/status", "GET")]
|
||||
assert button._get_fast_update_state().stage == "rebooting"
|
||||
@@ -158,7 +158,7 @@ class HudRenderer(Widget):
|
||||
self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44)
|
||||
self._txt_egpu_loading: rl.Texture = gui_app.texture('icons_mici/egpu_loading.png', 60, 44)
|
||||
self._txt_egpu_green: rl.Texture = gui_app.texture('icons_mici/egpu_green.png', 60, 44)
|
||||
self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 75, 44)
|
||||
self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 60, 44)
|
||||
self._txt_egpu_crossed: rl.Texture = gui_app.texture('icons_mici/egpu_crossed.png', 60, 52)
|
||||
self._egpu_icon: rl.Texture | None = None
|
||||
|
||||
|
||||
@@ -6,7 +6,6 @@ from cereal import messaging, car
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
|
||||
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
||||
from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.rainbow_path import RainbowPath
|
||||
@@ -362,31 +361,7 @@ class ModelRenderer(Widget):
|
||||
|
||||
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
|
||||
|
||||
def _lane_centering_direction(self) -> int:
|
||||
toggles = ui_state.starpilot_toggles
|
||||
sm = ui_state.sm
|
||||
if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
|
||||
return 0
|
||||
|
||||
car_state = sm["carState"]
|
||||
applied_correction = None
|
||||
if sm.valid.get("controlsState", False):
|
||||
try:
|
||||
applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
pass
|
||||
return get_lane_centering_visual_direction(
|
||||
sm["modelV2"], car_state.vEgo,
|
||||
toggles.get("lane_center_offset", 0.0),
|
||||
toggles.get("lane_centering_e2e_authority", 1.0),
|
||||
bool(toggles.get("lane_centering", False)),
|
||||
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
|
||||
bool(toggles.get("lane_centering_pause_on_signal", True)),
|
||||
bool(car_state.leftBlinker or car_state.rightBlinker),
|
||||
applied_correction,
|
||||
)
|
||||
|
||||
def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, int]:
|
||||
def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, bool]:
|
||||
stock_scheme = is_stock_color_scheme(self._params)
|
||||
line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status
|
||||
|
||||
@@ -399,15 +374,15 @@ class ModelRenderer(Widget):
|
||||
if lane_color is None:
|
||||
lane_color = STOCK_LANE_LINES_COLOR if stock_scheme else get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR)
|
||||
|
||||
lane_centering_direction = self._lane_centering_direction()
|
||||
return stock_scheme, edge_color, lane_color, lane_centering_direction
|
||||
lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
|
||||
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
|
||||
)
|
||||
return stock_scheme, edge_color, lane_color, lane_centering_active
|
||||
|
||||
def _get_ll_color(self, prob: float, adjacent: bool, left: bool, stock_scheme: bool,
|
||||
edge_color: rl.Color, lane_color: rl.Color, lane_centering_direction: int = 0):
|
||||
edge_color: rl.Color, lane_color: rl.Color, lane_centering_active: bool = False):
|
||||
alpha = np.clip(prob, 0.0, 0.7)
|
||||
lane_centering_line = adjacent and ((lane_centering_direction > 0 and not left) or
|
||||
(lane_centering_direction < 0 and left))
|
||||
if lane_centering_line:
|
||||
if lane_centering_active:
|
||||
color = rl.Color(OCEAN_BLUE_LANE_LINES_COLOR.r, OCEAN_BLUE_LANE_LINES_COLOR.g,
|
||||
OCEAN_BLUE_LANE_LINES_COLOR.b, int(alpha * OCEAN_BLUE_LANE_LINES_COLOR.a))
|
||||
elif adjacent:
|
||||
@@ -433,13 +408,13 @@ class ModelRenderer(Widget):
|
||||
def _draw_lane_lines(self):
|
||||
"""Draw lane lines and road edges"""
|
||||
"""Two closest lines should be green (lane line or road edges)"""
|
||||
stock_scheme, edge_color, lane_color, lane_centering_direction = self._lane_line_palette()
|
||||
stock_scheme, edge_color, lane_color, lane_centering_active = self._lane_line_palette()
|
||||
for i, lane_line in enumerate(self._lane_lines):
|
||||
if lane_line.projected_points.size == 0:
|
||||
continue
|
||||
|
||||
color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1),
|
||||
stock_scheme, edge_color, lane_color, lane_centering_direction)
|
||||
stock_scheme, edge_color, lane_color, lane_centering_active)
|
||||
draw_polygon(self._rect, lane_line.projected_points, color)
|
||||
|
||||
for i, road_edge in enumerate(self._road_edges):
|
||||
|
||||
@@ -5,7 +5,6 @@ from cereal import messaging, car
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
|
||||
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
||||
from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
|
||||
from openpilot.selfdrive.ui.onroad.radar_tracks import project_radar_points
|
||||
@@ -370,35 +369,15 @@ class ModelRenderer(Widget):
|
||||
|
||||
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
|
||||
|
||||
def _lane_centering_direction(self) -> int:
|
||||
toggles = ui_state.starpilot_toggles
|
||||
sm = ui_state.sm
|
||||
if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
|
||||
return 0
|
||||
|
||||
car_state = sm["carState"]
|
||||
applied_correction = None
|
||||
if sm.valid.get("controlsState", False):
|
||||
try:
|
||||
applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
pass
|
||||
return get_lane_centering_visual_direction(
|
||||
sm["modelV2"], car_state.vEgo,
|
||||
toggles.get("lane_center_offset", 0.0),
|
||||
toggles.get("lane_centering_e2e_authority", 1.0),
|
||||
bool(toggles.get("lane_centering", False)),
|
||||
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
|
||||
bool(toggles.get("lane_centering_pause_on_signal", True)),
|
||||
bool(car_state.leftBlinker or car_state.rightBlinker),
|
||||
applied_correction,
|
||||
)
|
||||
|
||||
def _draw_lane_lines(self):
|
||||
"""Draw lane lines and road edges"""
|
||||
lane_centering_direction = self._lane_centering_direction()
|
||||
lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
|
||||
ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
|
||||
)
|
||||
lane_lines_override = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a)
|
||||
if lane_lines_override is not None:
|
||||
if lane_centering_active:
|
||||
lane_lines_color = OCEAN_BLUE_LANE_LINES_COLOR
|
||||
elif lane_lines_override is not None:
|
||||
lane_lines_color = lane_lines_override
|
||||
elif is_stock_color_scheme(self._params):
|
||||
lane_lines_color = STOCK_LANE_LINES_COLOR
|
||||
@@ -410,10 +389,7 @@ class ModelRenderer(Widget):
|
||||
continue
|
||||
|
||||
alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7)
|
||||
lane_centering_line = (lane_centering_direction > 0 and i == 2) or \
|
||||
(lane_centering_direction < 0 and i == 1)
|
||||
line_color = OCEAN_BLUE_LANE_LINES_COLOR if lane_centering_line else lane_lines_color
|
||||
color = with_alpha(line_color, int(alpha * line_color.a))
|
||||
color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a))
|
||||
draw_polygon(self._rect, lane_line.projected_points, color)
|
||||
|
||||
for i, road_edge in enumerate(self._road_edges):
|
||||
|
||||
@@ -11,6 +11,11 @@ from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import _csc_state, _intensity, _glow_color
|
||||
from openpilot.selfdrive.ui.lib.starpilot_status import get_border_color
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
|
||||
CONTROL_BORDER,
|
||||
CONTROL_BORDER_WIDTH,
|
||||
draw_control_card,
|
||||
)
|
||||
|
||||
|
||||
# --- Scale factor (single knob for all pixel-space dimensions) ---
|
||||
@@ -35,6 +40,31 @@ ROAD_THICKNESS = 4.0 * SCALE
|
||||
ROAD_HALF_SIZE = 40.0 * SCALE
|
||||
ROAD_EDGE_INSET = 2.0 * SCALE
|
||||
FILL_ALPHA = 90
|
||||
ROAD_CONTOUR_WIDTH = 2.0 * SCALE
|
||||
|
||||
# The gauge overlays an unconstrained camera image. It shares the visible
|
||||
# control-card frame used by Set Speed and MAP, then owns two contained
|
||||
# viewports: animated road graphics above, glanceable status metrics below.
|
||||
# Nothing may draw outside the card.
|
||||
AETHER_CONTENT_INSET_X = max(5.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 3.0 * SCALE)
|
||||
AETHER_ROAD_TOP_INSET = max(5.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 3.0 * SCALE)
|
||||
AETHER_ROAD_VIEWPORT_HEIGHT = 75.0 * SCALE
|
||||
AETHER_ROAD_TO_CRADLE_GAP = 2.0 * SCALE
|
||||
AETHER_CRADLE_TOP = 83.0 * SCALE
|
||||
AETHER_CRADLE_BOTTOM_INSET = max(2.0 * SCALE, CONTROL_BORDER_WIDTH / 2.0 + 2.0 * SCALE)
|
||||
|
||||
AETHER_LABEL_SIZE = int(16.0 * SCALE)
|
||||
AETHER_LABEL_SLOT_HEIGHT = AETHER_LABEL_SIZE
|
||||
AETHER_VALUE_GAP = 2.0 * SCALE
|
||||
AETHER_VALUE_MAX_SIZE = int(40.0 * SCALE)
|
||||
AETHER_VALUE_MIN_SIZE = int(24.0 * SCALE)
|
||||
AETHER_LEAD_MAX_SIZE = int(32.0 * SCALE)
|
||||
AETHER_LEAD_MIN_SIZE = int(18.0 * SCALE)
|
||||
AETHER_REDUCTION_SIZE = int(16.0 * SCALE)
|
||||
AETHER_REDUCTION_GAP = 4.0 * SCALE
|
||||
AETHER_ACCENT_GAP = 2.0 * SCALE
|
||||
AETHER_UNIT_SIZE = int(18.0 * SCALE)
|
||||
AETHER_UNIT_GAP = 2.0 * SCALE
|
||||
|
||||
STOP_SNAP_THRESHOLD = 0.5
|
||||
STOP_LERP_RATE = 0.25
|
||||
@@ -51,7 +81,10 @@ LEAD_STOPPED_SPEED_THRESHOLD = 1.0
|
||||
COLOR_FORCE_STOP = rl.Color(255, 30, 60, 255)
|
||||
COLOR_LEAD_STOPPED = rl.Color(255, 60, 60, 255)
|
||||
COLOR_LEAD_SLOWER = rl.Color(255, 191, 0, 255)
|
||||
COLOR_SHADOW = rl.Color(0, 0, 0, 100)
|
||||
COLOR_CONTOUR = rl.Color(2, 6, 9, 235)
|
||||
COLOR_AETHER_CARD = rl.Color(9, 14, 18, 226)
|
||||
COLOR_PRIMARY_TEXT = rl.Color(245, 250, 252, 255)
|
||||
COLOR_SECONDARY_TEXT = rl.Color(225, 235, 240, 235)
|
||||
COLOR_STOP_SIGN_OUTLINE = rl.Color(255, 255, 255, 255)
|
||||
COLOR_STOP_LINE_GLOW = rl.Color(255, 30, 60, 255)
|
||||
COLOR_STOP_LINE_CORE = rl.Color(255, 200, 200, 255)
|
||||
@@ -150,12 +183,119 @@ def _get_perspective_offset(t: float, data: 'AetherGaugeData | None') -> float:
|
||||
max_offset_top = math.tanh(path_y_far * PERSPECTIVE_GAIN) * PERSPECTIVE_MAX_OFFSET
|
||||
return max_offset_top * (t ** PERSPECTIVE_EXPONENT)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AetherGaugeLayout:
|
||||
card: rl.Rectangle
|
||||
road: rl.Rectangle
|
||||
cradle: rl.Rectangle
|
||||
|
||||
|
||||
def _snapped_rect(x: float, y: float, width: float, height: float) -> rl.Rectangle:
|
||||
return rl.Rectangle(
|
||||
int(round(x)), int(round(y)), max(1, int(round(width))), max(1, int(round(height))),
|
||||
)
|
||||
|
||||
|
||||
def _aether_layout(rect: rl.Rectangle) -> AetherGaugeLayout:
|
||||
"""Return the one geometry contract used by every AetherGauge primitive."""
|
||||
# Match the Set Speed and MAP frame exactly; internal viewports provide the
|
||||
# clearance needed by the denser AetherGauge road and metric visuals.
|
||||
card = _snapped_rect(rect.x, rect.y, rect.width, rect.height)
|
||||
|
||||
cradle_top = int(round(card.y + AETHER_CRADLE_TOP))
|
||||
cradle_bottom = int(round(card.y + card.height - AETHER_CRADLE_BOTTOM_INSET))
|
||||
road_y = int(round(card.y + AETHER_ROAD_TOP_INSET))
|
||||
road_height = min(
|
||||
int(round(AETHER_ROAD_VIEWPORT_HEIGHT)),
|
||||
max(1, cradle_top - road_y - int(round(AETHER_ROAD_TO_CRADLE_GAP))),
|
||||
)
|
||||
content_x = int(round(card.x + AETHER_CONTENT_INSET_X))
|
||||
content_width = max(1, int(round(card.width - 2.0 * AETHER_CONTENT_INSET_X)))
|
||||
|
||||
return AetherGaugeLayout(
|
||||
card=card,
|
||||
road=_snapped_rect(content_x, road_y, content_width, road_height),
|
||||
cradle=_snapped_rect(content_x, cradle_top, content_width, max(1, cradle_bottom - cradle_top)),
|
||||
)
|
||||
|
||||
|
||||
def _draw_aether_card(layout: AetherGaugeLayout, alpha: float = 1.0) -> None:
|
||||
"""Draw the shared control frame with AetherGauge's deeper contrast fill."""
|
||||
draw_control_card(
|
||||
layout.card,
|
||||
fill=_fade(COLOR_AETHER_CARD, alpha),
|
||||
border=_fade(CONTROL_BORDER, alpha),
|
||||
)
|
||||
|
||||
|
||||
def _text_outline_px(size: int) -> int:
|
||||
return max(1, int(round(size / (28.0 * SCALE))))
|
||||
|
||||
|
||||
def _draw_text_with_shadow(font: rl.Font, text: str, pos: rl.Vector2, size: int, color: rl.Color, alpha: float = 1.0):
|
||||
for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1)):
|
||||
rl.draw_text_ex(font, text, rl.Vector2(pos.x + dx, pos.y + dy), size, 0, _fade(rl.BLACK, alpha))
|
||||
outline_px = _text_outline_px(size)
|
||||
for dx, dy in (
|
||||
(-outline_px, 0), (outline_px, 0), (0, -outline_px), (0, outline_px),
|
||||
(-outline_px, -outline_px), (outline_px, -outline_px),
|
||||
(-outline_px, outline_px), (outline_px, outline_px),
|
||||
):
|
||||
rl.draw_text_ex(font, text, rl.Vector2(pos.x + dx, pos.y + dy), size, 0, _fade(COLOR_CONTOUR, alpha))
|
||||
rl.draw_text_ex(font, text, pos, size, 0, _fade(color, alpha))
|
||||
|
||||
|
||||
def _fit_text_size(font: rl.Font, text: str, max_size: int, min_size: int,
|
||||
max_width: float, max_height: float) -> tuple[int, rl.Vector2]:
|
||||
"""Measure down to a guaranteed-safe text size for the supplied content box."""
|
||||
for size in range(max_size, min_size - 1, -1):
|
||||
text_size = measure_text_cached(font, text, size)
|
||||
outline = _text_outline_px(size)
|
||||
if text_size.x + 2 * outline <= max_width and text_size.y + 2 * outline <= max_height:
|
||||
return size, text_size
|
||||
|
||||
# Keep pathological future values contained even if they exceed the intended
|
||||
# type scale. Normal gauge data never reaches this fallback.
|
||||
for size in range(min_size - 1, 0, -1):
|
||||
text_size = measure_text_cached(font, text, size)
|
||||
outline = _text_outline_px(size)
|
||||
if text_size.x + 2 * outline <= max_width and text_size.y + 2 * outline <= max_height:
|
||||
return size, text_size
|
||||
|
||||
return 1, measure_text_cached(font, text, 1)
|
||||
|
||||
|
||||
def _fit_numeric_line(font_bold: rl.Font, font_medium: rl.Font, value: str, reduction: str,
|
||||
max_width: float, max_height: float) -> 'tuple[int, rl.Vector2, rl.Vector2 | None, bool]':
|
||||
"""Fit the primary value and optional reduction inline, or reflow the reduction."""
|
||||
reduction_size = measure_text_cached(font_medium, reduction, AETHER_REDUCTION_SIZE) if reduction else None
|
||||
|
||||
for value_font_size in range(AETHER_VALUE_MAX_SIZE, 0, -1):
|
||||
value_size = measure_text_cached(font_bold, value, value_font_size)
|
||||
value_outline = _text_outline_px(value_font_size)
|
||||
if value_size.y + 2 * value_outline > max_height:
|
||||
continue
|
||||
|
||||
if reduction_size is None:
|
||||
if value_size.x + 2 * value_outline <= max_width:
|
||||
return value_font_size, value_size, None, True
|
||||
continue
|
||||
|
||||
reduction_outline = _text_outline_px(AETHER_REDUCTION_SIZE)
|
||||
inline_width = value_size.x + AETHER_REDUCTION_GAP + reduction_size.x + 2 * max(value_outline, reduction_outline)
|
||||
if inline_width <= max_width:
|
||||
return value_font_size, value_size, reduction_size, True
|
||||
|
||||
if value_font_size == AETHER_VALUE_MIN_SIZE:
|
||||
break
|
||||
|
||||
# The normal source values fit inline. This fallback keeps a future long
|
||||
# value contained by using the reserved label row for the reduction.
|
||||
value_font_size, value_size = _fit_text_size(
|
||||
font_bold, value, AETHER_VALUE_MIN_SIZE, 1, max_width, max_height,
|
||||
)
|
||||
return value_font_size, value_size, reduction_size, False
|
||||
|
||||
|
||||
# --- Data model ---
|
||||
|
||||
class IndicatorType(Enum):
|
||||
@@ -178,6 +318,14 @@ class AetherGaugeData:
|
||||
is_numeric: bool = False
|
||||
|
||||
|
||||
def _state_label(data: AetherGaugeData) -> str:
|
||||
return {
|
||||
IndicatorType.FORCE_STOP: "STOP",
|
||||
IndicatorType.STOP_LIGHT: "RED LIGHT",
|
||||
IndicatorType.LEAD: "LEAD",
|
||||
}.get(data.indicator_type, "")
|
||||
|
||||
|
||||
# --- Source functions (replaces class-based sources) ---
|
||||
|
||||
def _build_curve_gauge_data(curvature: float, target_speed: float, v_cruise: float) -> AetherGaugeData:
|
||||
@@ -460,16 +608,13 @@ class AetherGauge:
|
||||
if not data:
|
||||
return
|
||||
|
||||
if cx is None or bottom is None:
|
||||
if cx is None:
|
||||
base_cx = rect.x + rect.width / 2
|
||||
cy_speed = rect.y + 180 * SCALE
|
||||
speed_text = str(round(current_speed))
|
||||
speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
|
||||
icx = base_cx - speed_text_size.x / 2 - 70.0 * SCALE
|
||||
icy = cy_speed - 39.5 * SCALE
|
||||
else:
|
||||
icx = cx
|
||||
icy = bottom - ROAD_HALF_SIZE
|
||||
|
||||
if data.indicator_type != self._last_indicator_type:
|
||||
self._dist_filter.x = data.indicator_value
|
||||
@@ -497,60 +642,71 @@ class AetherGauge:
|
||||
data.unit = unit
|
||||
|
||||
if data.indicator_type in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.LEAD, IndicatorType.STOP_LIGHT):
|
||||
self._render_unified_road(rect, icx, icy, data, font_bold, font_medium, alpha)
|
||||
self._render_unified_road(rect, icx, data, font_bold, font_medium, alpha)
|
||||
|
||||
def _render_unified_road(self, rect, icx, icy, data, font_bold, font_medium, alpha=1.0):
|
||||
bottom = icy + ROAD_HALF_SIZE
|
||||
def _render_unified_road(self, rect, icx, data, font_bold, font_medium, alpha=1.0):
|
||||
layout = _aether_layout(rect)
|
||||
_draw_aether_card(layout, alpha)
|
||||
bottom = layout.road.y + layout.road.height
|
||||
road_icy = bottom - ROAD_HALF_SIZE
|
||||
|
||||
if data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
|
||||
distance = 15.0
|
||||
else:
|
||||
distance = data.indicator_value
|
||||
|
||||
points_left = []
|
||||
points_right = []
|
||||
self._current_road_h = self._road_h_filter.update(_get_road_height(data))
|
||||
road_h = self._current_road_h
|
||||
# Custom widgets render after AugmentedRoadView releases its content
|
||||
# scissor. Keep every animated primitive inside the road viewport here.
|
||||
rl.begin_scissor_mode(
|
||||
int(layout.road.x), int(layout.road.y), int(layout.road.width), int(layout.road.height),
|
||||
)
|
||||
try:
|
||||
points_left = []
|
||||
points_right = []
|
||||
self._current_road_h = min(self._road_h_filter.update(_get_road_height(data)), layout.road.height)
|
||||
road_h = self._current_road_h
|
||||
|
||||
for i in range(ROAD_SEGMENTS + 1):
|
||||
t = i / ROAD_SEGMENTS
|
||||
offset = _get_perspective_offset(t, data)
|
||||
cx_t = icx + offset
|
||||
y_t = bottom - t * road_h
|
||||
w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
|
||||
for i in range(ROAD_SEGMENTS + 1):
|
||||
t = i / ROAD_SEGMENTS
|
||||
offset = _get_perspective_offset(t, data)
|
||||
cx_t = icx + offset
|
||||
y_t = bottom - t * road_h
|
||||
w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
|
||||
|
||||
points_left.append(rl.Vector2(cx_t - w_t, y_t))
|
||||
points_right.append(rl.Vector2(cx_t + w_t, y_t))
|
||||
points_left.append(rl.Vector2(cx_t - w_t, y_t))
|
||||
points_right.append(rl.Vector2(cx_t + w_t, y_t))
|
||||
|
||||
fill_color = _fade(_with_alpha(data.color, FILL_ALPHA), alpha)
|
||||
for i in range(ROAD_SEGMENTS):
|
||||
t = i / ROAD_SEGMENTS
|
||||
stroke = ROAD_THICKNESS * (1.0 - 0.6 * t)
|
||||
rl.draw_triangle(points_left[i], points_right[i], points_left[i+1], fill_color)
|
||||
rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color)
|
||||
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(COLOR_SHADOW, alpha))
|
||||
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(COLOR_SHADOW, alpha))
|
||||
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(data.color, alpha))
|
||||
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(data.color, alpha))
|
||||
fill_color = _fade(_with_alpha(data.color, FILL_ALPHA), alpha)
|
||||
for i in range(ROAD_SEGMENTS):
|
||||
t = i / ROAD_SEGMENTS
|
||||
stroke = ROAD_THICKNESS * (1.0 - 0.6 * t)
|
||||
rl.draw_triangle(points_left[i], points_right[i], points_left[i+1], fill_color)
|
||||
rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color)
|
||||
rl.draw_line_ex(points_left[i], points_left[i+1], stroke + ROAD_CONTOUR_WIDTH, _fade(COLOR_CONTOUR, alpha))
|
||||
rl.draw_line_ex(points_right[i], points_right[i+1], stroke + ROAD_CONTOUR_WIDTH, _fade(COLOR_CONTOUR, alpha))
|
||||
rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(data.color, alpha))
|
||||
rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(data.color, alpha))
|
||||
|
||||
it = data.indicator_type
|
||||
it = data.indicator_type
|
||||
|
||||
if it in (IndicatorType.STOP_LIGHT, IndicatorType.FORCE_STOP):
|
||||
self._draw_stop_line(icx, bottom, distance, data, alpha)
|
||||
if it in (IndicatorType.STOP_LIGHT, IndicatorType.FORCE_STOP):
|
||||
self._draw_stop_line(icx, bottom, distance, data, alpha)
|
||||
|
||||
if it == IndicatorType.LEAD:
|
||||
self._draw_lead_car(icx, bottom, data, alpha)
|
||||
if it == IndicatorType.LEAD:
|
||||
self._draw_lead_car(icx, bottom, data, alpha)
|
||||
|
||||
if it in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
|
||||
self._draw_standard_chevrons(icx, bottom, distance, data, alpha)
|
||||
if it in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
|
||||
self._draw_standard_chevrons(icx, bottom, distance, data, alpha)
|
||||
|
||||
if it == IndicatorType.STOP_LIGHT:
|
||||
self._draw_traffic_light(icx, icy, distance, data, alpha)
|
||||
if it == IndicatorType.STOP_LIGHT:
|
||||
self._draw_traffic_light(icx, road_icy, distance, data, alpha)
|
||||
|
||||
if it == IndicatorType.FORCE_STOP:
|
||||
self._draw_approaching_stop_sign(icx, icy, bottom, distance, data, font_bold, alpha)
|
||||
if it == IndicatorType.FORCE_STOP:
|
||||
self._draw_approaching_stop_sign(icx, road_icy, bottom, distance, data, font_bold, alpha)
|
||||
finally:
|
||||
rl.end_scissor_mode()
|
||||
|
||||
self._draw_mini_cradle(icx, bottom, data, font_bold, font_medium, alpha)
|
||||
self._draw_mini_cradle(layout.cradle, data, font_bold, font_medium, alpha)
|
||||
|
||||
def _draw_stop_line(self, icx, bottom, distance, data, alpha=1.0):
|
||||
t, cx_line, cy_line = _road_xy(distance, icx, bottom, data, self._current_road_h)
|
||||
@@ -562,6 +718,10 @@ class AetherGauge:
|
||||
fade = 1.0 - t * 0.5
|
||||
glow_a = int(150 * fade)
|
||||
|
||||
rl.draw_line_ex(
|
||||
p_left, p_right, max(3.0 * SCALE, 7.0 * SCALE * fade) + ROAD_CONTOUR_WIDTH,
|
||||
_fade(COLOR_CONTOUR, alpha),
|
||||
)
|
||||
rl.draw_line_ex(p_left, p_right, max(3.0 * SCALE, 7.0 * SCALE * fade), _fade(_with_alpha(COLOR_STOP_LINE_GLOW, glow_a), alpha))
|
||||
rl.draw_line_ex(p_left, p_right, max(1.5 * SCALE, 3.5 * SCALE * fade), _fade(COLOR_STOP_LINE_CORE, alpha))
|
||||
|
||||
@@ -642,7 +802,10 @@ class AetherGauge:
|
||||
|
||||
chev_a = max(0, min(255, int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi))))
|
||||
c_color = _fade(_with_alpha(data.color, chev_a), alpha)
|
||||
c_contour = _fade(_with_alpha(COLOR_CONTOUR, int(chev_a * 0.9)), alpha)
|
||||
|
||||
rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, c_contour)
|
||||
rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, c_contour)
|
||||
rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
|
||||
@@ -690,7 +853,10 @@ class AetherGauge:
|
||||
chev_a = max(0, min(255, int(data.color.a * alpha_factor)))
|
||||
chev_color = _fade(_with_alpha(data.color, chev_a), alpha)
|
||||
chev_shadow = _fade(rl.Color(0, 0, 0, int(chev_a * 0.5)), alpha)
|
||||
chev_contour = _fade(_with_alpha(COLOR_CONTOUR, int(chev_a * 0.9)), alpha)
|
||||
|
||||
rl.draw_line_ex(rl.Vector2(lx, ly), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, chev_contour)
|
||||
rl.draw_line_ex(rl.Vector2(rx, ry), rl.Vector2(cx_t, cy_t), chevron_thick + ROAD_CONTOUR_WIDTH, chev_contour)
|
||||
rl.draw_line_ex(rl.Vector2(lx, ly + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow)
|
||||
rl.draw_line_ex(rl.Vector2(rx, ry + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow)
|
||||
rl.draw_line_ex(rl.Vector2(lx, ly), rl.Vector2(cx_t, cy_t), chevron_thick, chev_color)
|
||||
@@ -761,18 +927,53 @@ class AetherGauge:
|
||||
stop_txt_size = measure_text_cached(font_bold, "STOP", stop_font_size)
|
||||
rl.draw_text_ex(font_bold, "STOP", rl.Vector2(cx_stop - stop_txt_size.x / 2, y_sign - stop_txt_size.y / 2), stop_font_size, 0, _fade(rl.WHITE, alpha))
|
||||
|
||||
def _draw_mini_cradle(self, cx, bottom, data, font_bold, font_medium, alpha=1.0):
|
||||
def _draw_mini_cradle(self, cradle, data, font_bold, font_medium, alpha=1.0):
|
||||
if not data.text:
|
||||
return
|
||||
|
||||
if data.is_numeric:
|
||||
val_size = measure_text_cached(font_bold, data.text, int(50 * SCALE))
|
||||
val_pos = rl.Vector2(int(cx - val_size.x / 2), int(bottom + 6 * SCALE))
|
||||
_draw_text_with_shadow(font_bold, data.text, val_pos, int(50 * SCALE), data.color, alpha)
|
||||
label = _state_label(data)
|
||||
metric_top = int(round(cradle.y + AETHER_LABEL_SLOT_HEIGHT + AETHER_VALUE_GAP))
|
||||
cradle_right = cradle.x + cradle.width
|
||||
|
||||
accent_y = int(val_pos.y + val_size.y + 2 * SCALE)
|
||||
accent_w = int(val_size.x + 16 * SCALE)
|
||||
accent_x = int(cx - accent_w / 2)
|
||||
if data.is_numeric:
|
||||
unit_size = measure_text_cached(font_medium, data.unit, AETHER_UNIT_SIZE) if data.unit else None
|
||||
footer_height = AETHER_ACCENT_GAP
|
||||
if unit_size is not None:
|
||||
footer_height += AETHER_UNIT_GAP + unit_size.y
|
||||
max_metric_height = max(1.0, cradle.y + cradle.height - metric_top - footer_height)
|
||||
max_metric_width = max(1.0, cradle.width - 2 * _text_outline_px(AETHER_VALUE_MAX_SIZE))
|
||||
value_font_size, value_size, reduction_size, reduction_inline = _fit_numeric_line(
|
||||
font_bold, font_medium, data.text, data.reduction_text, max_metric_width, max_metric_height,
|
||||
)
|
||||
value_outline = _text_outline_px(value_font_size)
|
||||
reduction_outline = _text_outline_px(AETHER_REDUCTION_SIZE) if reduction_size is not None else 0
|
||||
|
||||
group_width = value_size.x
|
||||
if reduction_size is not None and reduction_inline:
|
||||
group_width += AETHER_REDUCTION_GAP + reduction_size.x
|
||||
value_x = int(round(cradle.x + (cradle.width - group_width) / 2))
|
||||
value_pos = rl.Vector2(value_x, metric_top)
|
||||
|
||||
label_max_width = cradle.width - 2 * _text_outline_px(AETHER_LABEL_SIZE)
|
||||
if label and reduction_size is not None and not reduction_inline:
|
||||
label_max_width -= reduction_size.x + AETHER_REDUCTION_GAP + reduction_outline
|
||||
if label:
|
||||
label_font_size, label_size = _fit_text_size(
|
||||
font_medium, label, AETHER_LABEL_SIZE, 1, max(1.0, label_max_width), AETHER_LABEL_SLOT_HEIGHT,
|
||||
)
|
||||
label_x = cradle.x + value_outline if reduction_size is not None and not reduction_inline else cradle.x + (cradle.width - label_size.x) / 2
|
||||
label_y = cradle.y + (AETHER_LABEL_SLOT_HEIGHT - label_size.y) / 2
|
||||
_draw_text_with_shadow(
|
||||
font_medium, label, rl.Vector2(int(round(label_x)), int(round(label_y))),
|
||||
label_font_size, COLOR_PRIMARY_TEXT, alpha,
|
||||
)
|
||||
|
||||
_draw_text_with_shadow(font_bold, data.text, value_pos, value_font_size, COLOR_PRIMARY_TEXT, alpha)
|
||||
|
||||
accent_y = int(round(value_pos.y + value_size.y + AETHER_ACCENT_GAP))
|
||||
accent_w = min(max_metric_width, value_size.x + 16 * SCALE)
|
||||
accent_x = int(round(value_pos.x + value_size.x / 2 - accent_w / 2))
|
||||
accent_x = max(int(round(cradle.x + value_outline)), min(accent_x, int(round(cradle_right - value_outline - accent_w))))
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(accent_x, accent_y),
|
||||
rl.Vector2(accent_x + accent_w, accent_y),
|
||||
@@ -780,16 +981,42 @@ class AetherGauge:
|
||||
_fade(_with_alpha(data.color, 160), alpha),
|
||||
)
|
||||
|
||||
if data.reduction_text:
|
||||
red_size = measure_text_cached(font_medium, data.reduction_text, int(22 * SCALE))
|
||||
red_pos = rl.Vector2(int(cx + val_size.x / 2 + 6 * SCALE), int(val_pos.y + val_size.y / 2 - red_size.y / 2))
|
||||
_draw_text_with_shadow(font_medium, data.reduction_text, red_pos, int(22 * SCALE), COLOR_REDUCTION, alpha)
|
||||
if reduction_size is not None:
|
||||
if reduction_inline:
|
||||
reduction_x = int(round(value_pos.x + value_size.x + AETHER_REDUCTION_GAP))
|
||||
reduction_y = int(round(value_pos.y + (value_size.y - reduction_size.y) / 2))
|
||||
else:
|
||||
reduction_x = int(round(cradle_right - reduction_size.x - reduction_outline))
|
||||
reduction_y = int(round(cradle.y + (AETHER_LABEL_SLOT_HEIGHT - reduction_size.y) / 2))
|
||||
_draw_text_with_shadow(
|
||||
font_medium, data.reduction_text, rl.Vector2(reduction_x, reduction_y),
|
||||
AETHER_REDUCTION_SIZE, COLOR_REDUCTION, alpha,
|
||||
)
|
||||
|
||||
if data.unit:
|
||||
unit_size = measure_text_cached(font_medium, data.unit, int(20 * SCALE))
|
||||
unit_pos = rl.Vector2(int(cx - unit_size.x / 2), int(accent_y + 3 * SCALE))
|
||||
_draw_text_with_shadow(font_medium, data.unit, unit_pos, int(20 * SCALE), rl.Color(255, 255, 255, 180), alpha)
|
||||
if unit_size is not None:
|
||||
unit_pos = rl.Vector2(
|
||||
int(round(cradle.x + (cradle.width - unit_size.x) / 2)),
|
||||
int(round(accent_y + AETHER_UNIT_GAP)),
|
||||
)
|
||||
_draw_text_with_shadow(font_medium, data.unit, unit_pos, AETHER_UNIT_SIZE, COLOR_SECONDARY_TEXT, alpha)
|
||||
else:
|
||||
val_size = measure_text_cached(font_bold, data.text, int(32 * SCALE))
|
||||
val_pos = rl.Vector2(int(cx - val_size.x / 2), int(bottom + 10 * SCALE))
|
||||
_draw_text_with_shadow(font_bold, data.text, val_pos, int(32 * SCALE), data.color, alpha)
|
||||
if label:
|
||||
label_font_size, label_size = _fit_text_size(
|
||||
font_medium, label, AETHER_LABEL_SIZE, 1,
|
||||
cradle.width - 2 * _text_outline_px(AETHER_LABEL_SIZE), AETHER_LABEL_SLOT_HEIGHT,
|
||||
)
|
||||
label_pos = rl.Vector2(
|
||||
int(round(cradle.x + (cradle.width - label_size.x) / 2)),
|
||||
int(round(cradle.y + (AETHER_LABEL_SLOT_HEIGHT - label_size.y) / 2)),
|
||||
)
|
||||
_draw_text_with_shadow(font_medium, label, label_pos, label_font_size, COLOR_PRIMARY_TEXT, alpha)
|
||||
|
||||
lead_font_size, lead_size = _fit_text_size(
|
||||
font_bold, data.text, AETHER_LEAD_MAX_SIZE, AETHER_LEAD_MIN_SIZE,
|
||||
cradle.width - 2 * _text_outline_px(AETHER_LEAD_MAX_SIZE),
|
||||
cradle.y + cradle.height - metric_top,
|
||||
)
|
||||
lead_pos = rl.Vector2(
|
||||
int(round(cradle.x + (cradle.width - lead_size.x) / 2)), metric_top,
|
||||
)
|
||||
_draw_text_with_shadow(font_bold, data.text, lead_pos, lead_font_size, COLOR_PRIMARY_TEXT, alpha)
|
||||
|
||||
@@ -9,12 +9,12 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import (
|
||||
CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, SLC_HEIGHT,
|
||||
CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS,
|
||||
draw_control_card, roundness_for,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
|
||||
enabled_source_titles, fit_source_label, source_abbreviated_value_text,
|
||||
source_content_metrics, source_value_text, visible_source_rows,
|
||||
SourceBubbleModel, SourceBubbleTransition, enabled_source_titles,
|
||||
source_value_text, visible_source_rows,
|
||||
)
|
||||
from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state
|
||||
|
||||
@@ -35,7 +35,7 @@ SOURCE_DEFS = [
|
||||
("Dashboard", "Dash", "dashboard_sl", "Dashboard", "dashboard"),
|
||||
("Map Data", "MAP", "map_sl", "Map Data", "map"),
|
||||
("Vision", "VISION", "vision_sl", "Vision", "camera"),
|
||||
("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "map"),
|
||||
("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "navigation"),
|
||||
("Upcoming", "NEXT", "next_sl", "Next", "next"),
|
||||
]
|
||||
|
||||
@@ -179,7 +179,6 @@ def _get_slc_state():
|
||||
'offset_str': offset_str,
|
||||
'speed_conversion': speed_conversion,
|
||||
'speed_unit': " km/h" if ui_state.is_metric else " mph",
|
||||
'slc_abbreviated_sources': params.get_bool("SLCAbbreviatedSources"),
|
||||
'slc_active_sources_only': params.get_bool("SLCActiveSourcesOnly"),
|
||||
'slc_enabled_sources': enabled_source_titles(
|
||||
primary_priority,
|
||||
@@ -417,27 +416,33 @@ def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False):
|
||||
# Fixed outer footprint; the content scale adapts to the visible row count.
|
||||
_SOURCE_PANEL_WIDTH = 248
|
||||
_SOURCE_PANEL_GAP = 20
|
||||
_SOURCE_PANEL_PAD_X = 9
|
||||
_SOURCE_PANEL_PAD_Y = 2
|
||||
_SOURCE_PANEL_PAD_X = 10
|
||||
_SOURCE_PANEL_PAD_Y = 8
|
||||
_SOURCE_PANEL_BG = rl.Color(0, 0, 0, 175)
|
||||
_SOURCE_PANEL_BORDER = rl.Color(196, 205, 208, 80)
|
||||
_SOURCE_DIVIDER = rl.Color(196, 205, 208, 100)
|
||||
_SOURCE_ACTIVE_BAR = rl.Color(CONTROL_BORDER.r, CONTROL_BORDER.g, CONTROL_BORDER.b, 230)
|
||||
_SOURCE_ICON_MUTED = rl.Color(160, 170, 175, 200)
|
||||
_SOURCE_LABEL_MUTED = rl.Color(166, 166, 166, 255)
|
||||
_SOURCE_ACTIVE_BAR_WIDTH = 6.0
|
||||
_SOURCE_ACTIVE_BAR_HEIGHT = 36.0
|
||||
_SOURCE_ACTIVE_BAR_X = 2.0
|
||||
_SOURCE_ACTIVE_BAR_ROW_INSET = 3.0
|
||||
_SOURCE_ICON_MUTED = rl.Color(184, 194, 198, 230)
|
||||
_SOURCE_LABEL_MUTED = rl.Color(205, 211, 214, 240)
|
||||
_SOURCE_MISSING = rl.Color(155, 166, 171, 220)
|
||||
_SOURCE_ACTIVE_BAR_WIDTH = 5.0
|
||||
_SOURCE_ACTIVE_BAR_X = 6.0
|
||||
_SOURCE_ACTIVE_BAR_CORNER_INSET = 20.0
|
||||
_SOURCE_ACTIVE_BAR_MIN_HEIGHT = 20.0
|
||||
_SOURCE_MIN_LABEL_VALUE_GAP = 6.0
|
||||
|
||||
_SOURCE_COMPACT_LABELS = {
|
||||
"Dashboard": "Dash",
|
||||
"Map Data": "OSM",
|
||||
"Vision": "Vision",
|
||||
"Mapbox": "Mapbox",
|
||||
"Next": "Next",
|
||||
}
|
||||
_SOURCE_READABLE_ICON_SIZE = 32.0
|
||||
_SOURCE_READABLE_ICON_GAP = 8.0
|
||||
_SOURCE_LABEL_FONT = 28
|
||||
_SOURCE_MIN_LABEL_FONT = 18
|
||||
_SOURCE_VALUE_FONT = 38
|
||||
_SOURCE_DENSE_VALUE_FONT = 38
|
||||
_SOURCE_DENSE_FOOTER_VALUE_FONT = 36
|
||||
_SOURCE_DENSE_ICON_SIZE = 32
|
||||
_SOURCE_DENSE_SMALL_ICON_SIZE = 28
|
||||
_SOURCE_DENSE_GAP = 8.0
|
||||
_SOURCE_DENSE_FOOTER_HEIGHT = 60.0
|
||||
_SOURCE_MIN_VALUE_FONT = 30
|
||||
_SOURCE_MIN_MESSAGE_FONT = 20
|
||||
|
||||
|
||||
def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.Color) -> None:
|
||||
@@ -477,7 +482,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
|
||||
rl.draw_rectangle_rounded(body, 0.20, 8, color)
|
||||
lens = rl.Vector2(cx, y + size * 0.54)
|
||||
lens_outer = size * 0.17
|
||||
rl.draw_circle_v(lens, lens_outer, _SOURCE_PANEL_BG)
|
||||
rl.draw_circle_v(lens, lens_outer, _color_with_alpha(_SOURCE_PANEL_BG, color.a))
|
||||
rl.draw_ring(lens, size * 0.105, lens_outer, 0, 360, max(24, int(size * 0.25)), color)
|
||||
rl.draw_rectangle_rounded(
|
||||
rl.Rectangle(x + size * 0.30, y + size * 0.18, size * 0.23, size * 0.15),
|
||||
@@ -503,7 +508,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
|
||||
rl.Vector2(cx, y + size * 0.86),
|
||||
color,
|
||||
)
|
||||
rl.draw_circle_v(pin_center, size * 0.09, _SOURCE_PANEL_BG)
|
||||
rl.draw_circle_v(pin_center, size * 0.09, _color_with_alpha(_SOURCE_PANEL_BG, color.a))
|
||||
else: # Dashboard / fallback
|
||||
dashboard_scale = 1.22
|
||||
pivot = rl.Vector2(cx, cy + size * 0.17)
|
||||
@@ -536,32 +541,355 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.
|
||||
rl.draw_circle_v(pivot, max(2.0, size * 0.06 * dashboard_scale), color)
|
||||
|
||||
|
||||
def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None:
|
||||
"""Draw the 3-line centered empty state when no sources are available."""
|
||||
font = _get_semi_bold()
|
||||
font_size = 30
|
||||
line_gap = 6.0
|
||||
lines = (tr("NO"), tr("SOURCES"), tr("AVAILABLE"))
|
||||
|
||||
line_sizes = [measure_text_cached(font, line, font_size) for line in lines]
|
||||
total_h = sum(sz.y for sz in line_sizes) + line_gap * (len(lines) - 1)
|
||||
curr_y = round(panel_rect.y + (panel_rect.height - total_h) / 2)
|
||||
|
||||
for line, sz in zip(lines, line_sizes):
|
||||
pos_x = round(panel_rect.x + (panel_rect.width - sz.x) / 2)
|
||||
rl.draw_text_ex(font, line, rl.Vector2(pos_x, curr_y), font_size, 0, _WHITE)
|
||||
curr_y += round(sz.y + line_gap)
|
||||
def _color_with_alpha(color: rl.Color, alpha: int) -> rl.Color:
|
||||
return rl.Color(color.r, color.g, color.b, round(color.a * alpha / 255))
|
||||
|
||||
|
||||
def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
|
||||
"""Draw the expanded source list attached to the SLC card."""
|
||||
def _source_model(state: dict) -> SourceBubbleModel:
|
||||
enabled_sources = state.get('slc_enabled_sources', ())
|
||||
rows = visible_source_rows(
|
||||
SOURCE_DEFS,
|
||||
state,
|
||||
state['speed_limit_source'],
|
||||
enabled_sources,
|
||||
state.get('slc_active_sources_only', False),
|
||||
)
|
||||
if rows:
|
||||
return SourceBubbleModel(rows)
|
||||
reason = "No sources" if not enabled_sources else "No data"
|
||||
return SourceBubbleModel((), reason)
|
||||
|
||||
|
||||
def _fit_font_to_height(font, text: str, requested_size: int, max_height: float, minimum_size: int) -> int:
|
||||
size = requested_size
|
||||
while size > minimum_size and measure_text_cached(font, text, size).y > max_height:
|
||||
size -= 2
|
||||
return size
|
||||
|
||||
|
||||
def _fit_readable_label(font_semi, label_text: str, max_width: float, initial_size: int = _SOURCE_LABEL_FONT) -> int:
|
||||
"""Scale readable label font down if constrained by long translations."""
|
||||
size = initial_size
|
||||
while size > _SOURCE_MIN_LABEL_FONT and measure_text_cached(font_semi, label_text, size).x > max_width:
|
||||
size -= 2
|
||||
return size
|
||||
|
||||
|
||||
def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle, reason: str, alpha: int, font_semi) -> None:
|
||||
message = tr("No sources") if reason == "No sources" else tr("No data")
|
||||
font_size = _SOURCE_LABEL_FONT
|
||||
max_width = panel_rect.width - 2 * _SOURCE_PANEL_PAD_X
|
||||
max_height = panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y
|
||||
size = measure_text_cached(font_semi, message, font_size)
|
||||
while font_size > _SOURCE_MIN_MESSAGE_FONT and (size.x > max_width or size.y > max_height):
|
||||
font_size -= 2
|
||||
size = measure_text_cached(font_semi, message, font_size)
|
||||
rl.draw_text_ex(
|
||||
font_semi,
|
||||
message,
|
||||
rl.Vector2(round(panel_rect.x + (panel_rect.width - size.x) / 2),
|
||||
round(panel_rect.y + (panel_rect.height - size.y) / 2)),
|
||||
font_size,
|
||||
0,
|
||||
_color_with_alpha(_SOURCE_LABEL_MUTED, alpha),
|
||||
)
|
||||
|
||||
|
||||
def _source_text_color(item, alpha: int) -> rl.Color:
|
||||
if not item.has_reading:
|
||||
return _color_with_alpha(_SOURCE_MISSING, alpha)
|
||||
return _color_with_alpha(_WHITE if item.is_active else _SOURCE_LABEL_MUTED, alpha)
|
||||
|
||||
|
||||
def _source_icon_color(item, alpha: int) -> rl.Color:
|
||||
if not item.has_reading:
|
||||
return _color_with_alpha(_SOURCE_MISSING, alpha)
|
||||
return _color_with_alpha(_WHITE if item.is_active else _SOURCE_ICON_MUTED, alpha)
|
||||
|
||||
|
||||
def _draw_readable_sources(
|
||||
model: SourceBubbleModel,
|
||||
panel_rect: rl.Rectangle,
|
||||
alpha: int,
|
||||
font_semi,
|
||||
font_bold,
|
||||
*,
|
||||
draw_chrome: bool,
|
||||
) -> None:
|
||||
content_left = panel_rect.x + _SOURCE_PANEL_PAD_X
|
||||
content_right = panel_rect.x + panel_rect.width - _SOURCE_PANEL_PAD_X
|
||||
content_top = panel_rect.y + _SOURCE_PANEL_PAD_Y
|
||||
content_height = panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y
|
||||
row_height = content_height / len(model.items)
|
||||
value_font = _fit_font_to_height(
|
||||
font_bold,
|
||||
"000",
|
||||
_SOURCE_VALUE_FONT,
|
||||
max(1.0, row_height - 4),
|
||||
_SOURCE_MIN_VALUE_FONT,
|
||||
)
|
||||
|
||||
icon_size = _SOURCE_READABLE_ICON_SIZE
|
||||
icon_left = content_left + _SOURCE_ACTIVE_BAR_WIDTH + _SOURCE_MIN_LABEL_VALUE_GAP
|
||||
label_left = icon_left + icon_size + _SOURCE_READABLE_ICON_GAP
|
||||
|
||||
for index, item in enumerate(model.items):
|
||||
row_y = content_top + index * row_height
|
||||
if index and draw_chrome:
|
||||
divider_y = round(row_y)
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(content_left, divider_y),
|
||||
rl.Vector2(content_right, divider_y),
|
||||
1,
|
||||
_color_with_alpha(_SOURCE_DIVIDER, alpha),
|
||||
)
|
||||
|
||||
if item.is_active and draw_chrome:
|
||||
row_inset = min(12.0, row_height * 0.16)
|
||||
bar_top = max(panel_rect.y + _SOURCE_ACTIVE_BAR_CORNER_INSET, row_y + row_inset)
|
||||
bar_bottom = min(
|
||||
panel_rect.y + panel_rect.height - _SOURCE_ACTIVE_BAR_CORNER_INSET,
|
||||
row_y + row_height - row_inset,
|
||||
)
|
||||
active_bar_height = max(_SOURCE_ACTIVE_BAR_MIN_HEIGHT, bar_bottom - bar_top)
|
||||
active_bar_rect = rl.Rectangle(
|
||||
panel_rect.x + _SOURCE_ACTIVE_BAR_X,
|
||||
round(bar_top),
|
||||
_SOURCE_ACTIVE_BAR_WIDTH,
|
||||
round(active_bar_height),
|
||||
)
|
||||
rl.draw_rectangle_rounded(active_bar_rect, 0.5, 4, _color_with_alpha(_SOURCE_ACTIVE_BAR, alpha))
|
||||
|
||||
# Icon
|
||||
icon_y = round(row_y + (row_height - icon_size) / 2)
|
||||
_draw_source_icon(item.icon_key, icon_left, icon_y, icon_size, _source_icon_color(item, alpha))
|
||||
|
||||
# Value
|
||||
value_text = source_value_text(item.value)
|
||||
value_size = measure_text_cached(font_bold, value_text, value_font)
|
||||
value_y = round(row_y + (row_height - value_size.y) / 2)
|
||||
value_pos = rl.Vector2(round(content_right - value_size.x), value_y)
|
||||
|
||||
# Label (fitted to available width between icon and speed value)
|
||||
label_text = tr(item.label)
|
||||
max_label_width = max(10.0, (content_right - value_size.x - _SOURCE_MIN_LABEL_VALUE_GAP) - label_left)
|
||||
label_font = _fit_readable_label(font_semi, label_text, max_label_width, _SOURCE_LABEL_FONT)
|
||||
label_size = measure_text_cached(font_semi, label_text, label_font)
|
||||
label_y = round(row_y + (row_height - label_size.y) / 2)
|
||||
rl.draw_text_ex(
|
||||
font_semi,
|
||||
label_text,
|
||||
rl.Vector2(label_left, label_y),
|
||||
label_font,
|
||||
0,
|
||||
_source_text_color(item, alpha),
|
||||
)
|
||||
|
||||
rl.draw_text_ex(font_bold, value_text, value_pos, value_font, 0, _source_text_color(item, alpha))
|
||||
|
||||
|
||||
def _fit_dense_group(font_bold, value_text: str, cell: rl.Rectangle, icon_size: float, value_font: int) -> tuple[float, int, rl.Vector2]:
|
||||
"""Fit an icon/value group inside a dense cell without crossing its divider."""
|
||||
horizontal_edge = 8.0
|
||||
vertical_edge = 2.0
|
||||
fitted_icon = icon_size
|
||||
fitted_font = value_font
|
||||
fit_text = "0" * max(3, len(value_text))
|
||||
max_value_height = max(1.0, cell.height - 2 * vertical_edge)
|
||||
while fitted_font >= 32:
|
||||
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
|
||||
if (value_size.y <= max_value_height and
|
||||
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
|
||||
return fitted_icon, fitted_font, value_size
|
||||
fitted_font -= 2
|
||||
|
||||
while fitted_icon >= 20:
|
||||
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
|
||||
if (value_size.y <= max_value_height and
|
||||
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
|
||||
return fitted_icon, fitted_font, value_size
|
||||
fitted_icon -= 2
|
||||
|
||||
while fitted_font >= 20:
|
||||
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
|
||||
if (value_size.y <= max_value_height and
|
||||
fitted_icon + _SOURCE_DENSE_GAP + value_size.x <= cell.width - horizontal_edge):
|
||||
return fitted_icon, fitted_font, value_size
|
||||
fitted_font -= 2
|
||||
|
||||
fitted_icon = min(fitted_icon, max(20.0, cell.height - 2 * vertical_edge))
|
||||
value_size = measure_text_cached(font_bold, fit_text, fitted_font)
|
||||
return fitted_icon, fitted_font, value_size
|
||||
|
||||
|
||||
def _draw_dense_source_cell(
|
||||
item,
|
||||
cell: rl.Rectangle,
|
||||
alpha: int,
|
||||
font_bold,
|
||||
*,
|
||||
icon_size: float,
|
||||
value_font: int,
|
||||
draw_chrome: bool,
|
||||
footer_label: str | None = None,
|
||||
font_semi = None,
|
||||
) -> None:
|
||||
is_active = item.is_active
|
||||
if draw_chrome:
|
||||
if is_active:
|
||||
rl.draw_rectangle_rounded(cell, 0.30, 8, _color_with_alpha(rl.Color(255, 255, 255, 28), alpha))
|
||||
rl.draw_rectangle_rounded_lines_ex(cell, 0.30, 8, 1.0, _color_with_alpha(rl.Color(255, 255, 255, 90), alpha))
|
||||
bar_h = max(16.0, cell.height - 20.0)
|
||||
bar_r = rl.Rectangle(cell.x + 4.0, round(cell.y + (cell.height - bar_h) / 2), 4.0, round(bar_h))
|
||||
rl.draw_rectangle_rounded(bar_r, 0.5, 4, _color_with_alpha(_SOURCE_ACTIVE_BAR, alpha))
|
||||
else:
|
||||
rl.draw_rectangle_rounded(cell, 0.30, 8, _color_with_alpha(rl.Color(255, 255, 255, 10), alpha))
|
||||
|
||||
value_text = source_value_text(item.value)
|
||||
icon_size, value_font, fit_size = _fit_dense_group(font_bold, value_text, cell, icon_size, value_font)
|
||||
value_size = measure_text_cached(font_bold, value_text, value_font)
|
||||
|
||||
if footer_label and font_semi:
|
||||
label_text = tr(footer_label)
|
||||
label_font = 24
|
||||
label_size = measure_text_cached(font_semi, label_text, label_font)
|
||||
total_group_w = icon_size + _SOURCE_DENSE_GAP + label_size.x + 16.0 + fit_size.x
|
||||
group_left = cell.x + (cell.width - total_group_w) / 2
|
||||
if is_active:
|
||||
group_left += 2.0
|
||||
icon_y = cell.y + (cell.height - icon_size) / 2
|
||||
label_y = cell.y + (cell.height - label_size.y) / 2
|
||||
text_y = cell.y + (cell.height - value_size.y) / 2
|
||||
|
||||
_draw_source_icon(item.icon_key, round(group_left), round(icon_y), icon_size, _source_icon_color(item, alpha))
|
||||
rl.draw_text_ex(
|
||||
font_semi,
|
||||
label_text,
|
||||
rl.Vector2(round(group_left + icon_size + _SOURCE_DENSE_GAP), round(label_y)),
|
||||
label_font,
|
||||
0,
|
||||
_source_text_color(item, alpha),
|
||||
)
|
||||
rl.draw_text_ex(
|
||||
font_bold,
|
||||
value_text,
|
||||
rl.Vector2(
|
||||
round(group_left + icon_size + _SOURCE_DENSE_GAP + label_size.x + 16.0 + (fit_size.x - value_size.x) / 2),
|
||||
round(text_y),
|
||||
),
|
||||
value_font,
|
||||
0,
|
||||
_source_text_color(item, alpha),
|
||||
)
|
||||
else:
|
||||
total_group_w = icon_size + _SOURCE_DENSE_GAP + fit_size.x
|
||||
group_left = cell.x + (cell.width - total_group_w) / 2
|
||||
if is_active:
|
||||
group_left += 2.0
|
||||
icon_y = cell.y + (cell.height - icon_size) / 2
|
||||
text_y = cell.y + (cell.height - value_size.y) / 2
|
||||
|
||||
_draw_source_icon(item.icon_key, round(group_left), round(icon_y), icon_size, _source_icon_color(item, alpha))
|
||||
rl.draw_text_ex(
|
||||
font_bold,
|
||||
value_text,
|
||||
rl.Vector2(
|
||||
round(group_left + icon_size + _SOURCE_DENSE_GAP + (fit_size.x - value_size.x) / 2),
|
||||
round(text_y),
|
||||
),
|
||||
value_font,
|
||||
0,
|
||||
_source_text_color(item, alpha),
|
||||
)
|
||||
|
||||
|
||||
def _draw_dense_sources(model: SourceBubbleModel, panel_rect: rl.Rectangle, alpha: int, font_semi, font_bold, *, draw_chrome: bool) -> None:
|
||||
pad = 8.0
|
||||
gap = 6.0
|
||||
content_w = panel_rect.width - 2 * pad
|
||||
content_h = panel_rect.height - 2 * pad
|
||||
|
||||
if len(model.items) == 5:
|
||||
footer_h = min(44.0, content_h * 0.28)
|
||||
grid_h = content_h - footer_h - gap
|
||||
cell_w = (content_w - gap) / 2
|
||||
cell_h = (grid_h - gap) / 2
|
||||
for index, item in enumerate(model.items[:4]):
|
||||
row, column = divmod(index, 2)
|
||||
_draw_dense_source_cell(
|
||||
item,
|
||||
rl.Rectangle(
|
||||
panel_rect.x + pad + column * (cell_w + gap),
|
||||
panel_rect.y + pad + row * (cell_h + gap),
|
||||
cell_w,
|
||||
cell_h,
|
||||
),
|
||||
alpha,
|
||||
font_bold,
|
||||
icon_size=_SOURCE_DENSE_SMALL_ICON_SIZE,
|
||||
value_font=_SOURCE_DENSE_FOOTER_VALUE_FONT,
|
||||
draw_chrome=draw_chrome,
|
||||
)
|
||||
_draw_dense_source_cell(
|
||||
model.items[4],
|
||||
rl.Rectangle(panel_rect.x + pad, panel_rect.y + pad + grid_h + gap, content_w, footer_h),
|
||||
alpha,
|
||||
font_bold,
|
||||
icon_size=_SOURCE_DENSE_SMALL_ICON_SIZE,
|
||||
value_font=_SOURCE_DENSE_FOOTER_VALUE_FONT,
|
||||
draw_chrome=draw_chrome,
|
||||
footer_label=model.items[4].label,
|
||||
font_semi=font_semi,
|
||||
)
|
||||
return
|
||||
|
||||
cell_w = (content_w - gap) / 2
|
||||
cell_h = (content_h - gap) / 2
|
||||
for index, item in enumerate(model.items):
|
||||
row, column = divmod(index, 2)
|
||||
_draw_dense_source_cell(
|
||||
item,
|
||||
rl.Rectangle(
|
||||
panel_rect.x + pad + column * (cell_w + gap),
|
||||
panel_rect.y + pad + row * (cell_h + gap),
|
||||
cell_w,
|
||||
cell_h,
|
||||
),
|
||||
alpha,
|
||||
font_bold,
|
||||
icon_size=_SOURCE_DENSE_ICON_SIZE,
|
||||
value_font=_SOURCE_DENSE_VALUE_FONT,
|
||||
draw_chrome=draw_chrome,
|
||||
)
|
||||
|
||||
|
||||
def _draw_source_bubble_content(
|
||||
model: SourceBubbleModel,
|
||||
panel_rect: rl.Rectangle,
|
||||
alpha: int,
|
||||
font_semi,
|
||||
font_bold,
|
||||
*,
|
||||
draw_chrome: bool,
|
||||
) -> None:
|
||||
if alpha <= 0:
|
||||
return
|
||||
if model.mode == "empty":
|
||||
_draw_sources_bubble_empty_state(panel_rect, model.empty_reason or "No data", alpha, font_semi)
|
||||
elif model.mode == "dense":
|
||||
_draw_dense_sources(model, panel_rect, alpha, font_semi, font_bold, draw_chrome=draw_chrome)
|
||||
else:
|
||||
_draw_readable_sources(model, panel_rect, alpha, font_semi, font_bold, draw_chrome=draw_chrome)
|
||||
|
||||
|
||||
def _draw_sources_bubble(
|
||||
state: dict,
|
||||
sign_rect: rl.Rectangle,
|
||||
transition: SourceBubbleTransition | None = None,
|
||||
) -> None:
|
||||
"""Draw the expanded source bubble inside its fixed attached footprint."""
|
||||
font_semi = _get_semi_bold()
|
||||
font_bold = _get_bold()
|
||||
active_source = state['speed_limit_source']
|
||||
enabled_sources = state.get('slc_enabled_sources', ())
|
||||
active_only = state.get('slc_active_sources_only', False)
|
||||
abbreviated = state.get('slc_abbreviated_sources', False)
|
||||
|
||||
panel_rect = rl.Rectangle(
|
||||
sign_rect.x + sign_rect.width + _SOURCE_PANEL_GAP,
|
||||
sign_rect.y,
|
||||
@@ -573,113 +901,47 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle):
|
||||
panel_rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 1, _SOURCE_PANEL_BORDER,
|
||||
)
|
||||
|
||||
rows = [
|
||||
(
|
||||
panel_label,
|
||||
_SOURCE_COMPACT_LABELS[panel_label],
|
||||
icon_key,
|
||||
value,
|
||||
is_active,
|
||||
)
|
||||
for panel_label, icon_key, value, is_active in visible_source_rows(
|
||||
SOURCE_DEFS, state, active_source, enabled_sources, active_only,
|
||||
)
|
||||
]
|
||||
model = _source_model(state)
|
||||
if transition is None:
|
||||
outgoing, incoming, alpha = model, None, 1.0
|
||||
else:
|
||||
outgoing, incoming, alpha = transition.update(model, rl.get_time())
|
||||
|
||||
if not rows:
|
||||
_draw_sources_bubble_empty_state(panel_rect)
|
||||
return
|
||||
|
||||
row_h = (panel_rect.height - 2 * _SOURCE_PANEL_PAD_Y) / len(rows)
|
||||
content_left = panel_rect.x + _SOURCE_PANEL_PAD_X
|
||||
content_right = panel_rect.x + panel_rect.width - _SOURCE_PANEL_PAD_X
|
||||
font_size, icon_size, icon_gap = source_content_metrics(len(rows))
|
||||
label_left = (
|
||||
content_left + _SOURCE_ACTIVE_BAR_WIDTH + _SOURCE_MIN_LABEL_VALUE_GAP
|
||||
if abbreviated else content_left + icon_size + icon_gap
|
||||
is_transitioning = incoming is not None
|
||||
outgoing_chrome = not is_transitioning or alpha < 0.5
|
||||
incoming_chrome = not is_transitioning or alpha >= 0.5
|
||||
_draw_source_bubble_content(
|
||||
outgoing,
|
||||
panel_rect,
|
||||
round(255 * (1.0 - alpha)),
|
||||
font_semi,
|
||||
font_bold,
|
||||
draw_chrome=outgoing_chrome,
|
||||
)
|
||||
_draw_source_bubble_content(
|
||||
incoming or outgoing,
|
||||
panel_rect,
|
||||
round(255 * alpha),
|
||||
font_semi,
|
||||
font_bold,
|
||||
draw_chrome=incoming_chrome,
|
||||
)
|
||||
|
||||
for index, (panel_label, compact_label, icon_key, value, is_active) in enumerate(rows):
|
||||
row_y = panel_rect.y + _SOURCE_PANEL_PAD_Y + index * row_h
|
||||
if index:
|
||||
divider_y = round(row_y)
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(content_left, divider_y),
|
||||
rl.Vector2(content_right, divider_y),
|
||||
1,
|
||||
_SOURCE_DIVIDER,
|
||||
)
|
||||
|
||||
if is_active:
|
||||
active_bar_height = min(
|
||||
_SOURCE_ACTIVE_BAR_HEIGHT,
|
||||
max(10.0, row_h - 2 * _SOURCE_ACTIVE_BAR_ROW_INSET),
|
||||
)
|
||||
active_bar_rect = rl.Rectangle(
|
||||
panel_rect.x + _SOURCE_ACTIVE_BAR_X,
|
||||
round(row_y + (row_h - active_bar_height) / 2),
|
||||
_SOURCE_ACTIVE_BAR_WIDTH,
|
||||
active_bar_height,
|
||||
)
|
||||
rl.draw_rectangle_rounded(active_bar_rect, 0.5, 4, _SOURCE_ACTIVE_BAR)
|
||||
|
||||
value_text = source_value_text(value)
|
||||
text_color = _WHITE if is_active else _SOURCE_LABEL_MUTED
|
||||
|
||||
if abbreviated:
|
||||
text_font = font_bold if is_active else font_semi
|
||||
label_text = fit_source_label(
|
||||
f"{tr(compact_label)}-{source_abbreviated_value_text(value)}",
|
||||
"",
|
||||
content_right - label_left,
|
||||
lambda text: measure_text_cached(text_font, text, font_size).x,
|
||||
)
|
||||
label_size = measure_text_cached(text_font, label_text, font_size)
|
||||
text_y = round(row_y + (row_h - label_size.y) / 2)
|
||||
rl.draw_text_ex(
|
||||
text_font,
|
||||
label_text,
|
||||
rl.Vector2(label_left, text_y),
|
||||
font_size,
|
||||
0,
|
||||
text_color,
|
||||
)
|
||||
continue
|
||||
|
||||
compact_label = tr(compact_label)
|
||||
full_label = tr(panel_label)
|
||||
value_size = measure_text_cached(font_bold, value_text, font_size)
|
||||
max_label_width = max(
|
||||
0.0,
|
||||
content_right - label_left - _SOURCE_MIN_LABEL_VALUE_GAP - value_size.x,
|
||||
)
|
||||
label_text = fit_source_label(
|
||||
full_label,
|
||||
compact_label,
|
||||
max_label_width,
|
||||
lambda text: measure_text_cached(font_semi, text, font_size).x,
|
||||
)
|
||||
label_size = measure_text_cached(font_semi, label_text, font_size)
|
||||
text_height = max(label_size.y, value_size.y)
|
||||
text_y = round(row_y + (row_h - text_height) / 2)
|
||||
icon_y = round(row_y + (row_h - icon_size) / 2)
|
||||
|
||||
icon_color = _WHITE if is_active else _SOURCE_ICON_MUTED
|
||||
_draw_source_icon(icon_key, content_left, icon_y, icon_size, icon_color)
|
||||
|
||||
label_pos = rl.Vector2(label_left, text_y)
|
||||
value_pos = rl.Vector2(round(content_right - value_size.x), text_y)
|
||||
rl.draw_text_ex(font_semi, label_text, label_pos, font_size, 0, text_color)
|
||||
rl.draw_text_ex(font_bold, value_text, value_pos, font_size, 0, text_color)
|
||||
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────
|
||||
|
||||
def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = False) -> Optional[rl.Rectangle]:
|
||||
def render_speed_limit_at(
|
||||
state: dict,
|
||||
rect: rl.Rectangle,
|
||||
expanded: bool = False,
|
||||
source_bubble_transition: SourceBubbleTransition | None = None,
|
||||
) -> Optional[rl.Rectangle]:
|
||||
"""Render the SLC sign and optional source bubble at a layout rect."""
|
||||
flashing_pending = state['speed_limit_changed'] and state['unconfirmed_valid']
|
||||
|
||||
if flashing_pending:
|
||||
if source_bubble_transition is not None:
|
||||
source_bubble_transition.reset()
|
||||
_draw_sign(state, rect, pending=True)
|
||||
return None
|
||||
|
||||
@@ -689,6 +951,6 @@ def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = Fals
|
||||
visual_rect = rl.Rectangle(rect.x, rect.y, EU_SIGN_SIZE, EU_SIGN_SIZE) if use_vienna else rect
|
||||
|
||||
if expanded:
|
||||
_draw_sources_bubble(state, visual_rect)
|
||||
_draw_sources_bubble(state, visual_rect, source_bubble_transition)
|
||||
|
||||
return visual_rect
|
||||
|
||||
@@ -1,12 +1,50 @@
|
||||
"""Pure layout decisions for the on-road speed-limit source bubble."""
|
||||
"""Pure source selection and transition decisions for the SLC bubble."""
|
||||
|
||||
import math
|
||||
from collections.abc import Callable, Iterable, Mapping
|
||||
from collections.abc import Iterable, Mapping
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
|
||||
SOURCE_DISPLAY_ORDER = ("Dashboard", "Map Data", "Vision", "Mapbox", "Upcoming")
|
||||
SOURCE_PRIORITY_NAMES = frozenset(("Dashboard", "Map Data", "Vision"))
|
||||
|
||||
SOURCE_BUBBLE_SETTLE_SECONDS = 0.12
|
||||
SOURCE_BUBBLE_TRANSITION_SECONDS = 0.20
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SourceBubbleItem:
|
||||
"""One source row/cell in canonical display order."""
|
||||
|
||||
source_id: str
|
||||
label: str
|
||||
icon_key: str
|
||||
value: float
|
||||
has_reading: bool
|
||||
is_active: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SourceBubbleModel:
|
||||
"""The source roster to render inside the fixed bubble footprint."""
|
||||
|
||||
items: tuple[SourceBubbleItem, ...]
|
||||
empty_reason: str | None = None
|
||||
|
||||
@property
|
||||
def mode(self) -> str:
|
||||
if not self.items:
|
||||
return "empty"
|
||||
return "dense" if len(self.items) >= 4 else "readable"
|
||||
|
||||
@property
|
||||
def structural_key(self) -> tuple[str, tuple[tuple[str, str, str], ...], str | None]:
|
||||
return (
|
||||
self.mode,
|
||||
tuple((item.source_id, item.label, item.icon_key) for item in self.items),
|
||||
self.empty_reason,
|
||||
)
|
||||
|
||||
|
||||
def enabled_source_titles(
|
||||
primary_priority: str,
|
||||
@@ -16,13 +54,7 @@ def enabled_source_titles(
|
||||
mapbox_enabled: bool,
|
||||
dashboard_available: bool = True,
|
||||
) -> tuple[str, ...]:
|
||||
"""Return source rows that are eligible under the current SLC settings.
|
||||
|
||||
``Highest`` and ``Lowest`` are aggregate priority modes. They consider the
|
||||
two non-vision controller inputs directly; Vision is only a controller input
|
||||
when it is explicitly selected in one of the priority slots. Mapbox is a
|
||||
separate fallback toggle, and Next is derived from the selected map source.
|
||||
"""
|
||||
"""Return source rows eligible under the current SLC settings."""
|
||||
if primary_priority in ("Highest", "Lowest"):
|
||||
enabled = {"Dashboard", "Map Data"}
|
||||
else:
|
||||
@@ -45,13 +77,8 @@ def enabled_source_titles(
|
||||
return tuple(source for source in SOURCE_DISPLAY_ORDER if source in enabled)
|
||||
|
||||
|
||||
def source_content_metrics(row_count: int) -> tuple[int, int, int]:
|
||||
"""Return logical text size, icon size, and icon gap for the visible rows."""
|
||||
if row_count <= 3:
|
||||
return 30, 34, 7
|
||||
if row_count == 4:
|
||||
return 30, 32, 7
|
||||
return 28, 30, 6
|
||||
def _has_reading(value: float) -> bool:
|
||||
return math.isfinite(value) and value > 0 and round(value) > 0
|
||||
|
||||
|
||||
def visible_source_rows(
|
||||
@@ -59,54 +86,138 @@ def visible_source_rows(
|
||||
values: Mapping[str, float],
|
||||
active_source: str,
|
||||
enabled_sources: Iterable[str],
|
||||
active_only: bool = True,
|
||||
) -> list[tuple[str, str, float, bool]]:
|
||||
"""Return enabled source rows that possess a valid positive speed reading."""
|
||||
active_only: bool = False,
|
||||
) -> tuple[SourceBubbleItem, ...]:
|
||||
"""Return enabled sources, optionally filtering those without readings.
|
||||
|
||||
Enabled sources remain visible with an em dash by default. This preserves
|
||||
source discoverability without reserving space for sources disabled in the
|
||||
user's settings.
|
||||
"""
|
||||
enabled = set(enabled_sources)
|
||||
rows = []
|
||||
for title, _abbrev, value_key, panel_label, icon_key in source_defs:
|
||||
for title, _active_label, value_key, panel_label, icon_key in source_defs:
|
||||
if title not in enabled:
|
||||
continue
|
||||
value = values[value_key]
|
||||
has_reading = math.isfinite(value) and value > 0 and round(value) > 0
|
||||
if not has_reading:
|
||||
has_reading = _has_reading(value)
|
||||
if active_only and not has_reading:
|
||||
continue
|
||||
rows.append((
|
||||
panel_label,
|
||||
icon_key,
|
||||
value,
|
||||
active_source == title,
|
||||
rows.append(SourceBubbleItem(
|
||||
source_id=title,
|
||||
label=panel_label,
|
||||
icon_key=icon_key,
|
||||
value=value,
|
||||
has_reading=has_reading,
|
||||
is_active=active_source == title,
|
||||
))
|
||||
return rows
|
||||
|
||||
|
||||
def fit_source_label(
|
||||
full_label: str,
|
||||
compact_label: str,
|
||||
max_width: float,
|
||||
measure_width: Callable[[str], float],
|
||||
) -> str:
|
||||
"""Choose the longest useful label that leaves room for the value column."""
|
||||
for label in (full_label, compact_label):
|
||||
if measure_width(label) <= max_width:
|
||||
return label
|
||||
|
||||
ellipsis = "…"
|
||||
candidate = compact_label or full_label
|
||||
while candidate and measure_width(candidate + ellipsis) > max_width:
|
||||
candidate = candidate[:-1]
|
||||
return f"{candidate}{ellipsis}" if candidate else ellipsis
|
||||
return tuple(rows)
|
||||
|
||||
|
||||
def source_value_text(value: float) -> str:
|
||||
"""Format a source speed, keeping missing and non-finite values explicit."""
|
||||
if not math.isfinite(value) or value <= 0:
|
||||
if not _has_reading(value):
|
||||
return "–"
|
||||
rounded = int(round(value))
|
||||
return "–" if rounded <= 0 else str(rounded)
|
||||
return str(int(round(value)))
|
||||
|
||||
|
||||
def source_abbreviated_value_text(value: float) -> str:
|
||||
"""Format a compact source value using the established missing-value marker."""
|
||||
value_text = source_value_text(value)
|
||||
return "X" if value_text == "–" else value_text
|
||||
def _ease_out_cubic(progress: float) -> float:
|
||||
progress = min(1.0, max(0.0, progress))
|
||||
return 1.0 - (1.0 - progress) ** 3
|
||||
|
||||
|
||||
def _refresh_model_values(base: SourceBubbleModel, live: SourceBubbleModel) -> SourceBubbleModel:
|
||||
"""Keep structural content stable while refreshing values during a blend."""
|
||||
live_by_id = {item.source_id: item for item in live.items}
|
||||
refreshed = tuple(
|
||||
live_by_id.get(item.source_id, replace(item, value=0.0, has_reading=False))
|
||||
for item in base.items
|
||||
)
|
||||
return SourceBubbleModel(refreshed, base.empty_reason)
|
||||
|
||||
|
||||
@dataclass
|
||||
class SourceBubbleTransition:
|
||||
"""Coalesce roster changes and crossfade stable bubble presentations."""
|
||||
|
||||
settle_seconds: float = SOURCE_BUBBLE_SETTLE_SECONDS
|
||||
duration_seconds: float = SOURCE_BUBBLE_TRANSITION_SECONDS
|
||||
_current: SourceBubbleModel | None = None
|
||||
_pending: SourceBubbleModel | None = None
|
||||
_pending_since: float | None = None
|
||||
_from: SourceBubbleModel | None = None
|
||||
_to: SourceBubbleModel | None = None
|
||||
_started: float | None = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._current = None
|
||||
self._pending = None
|
||||
self._pending_since = None
|
||||
self._from = None
|
||||
self._to = None
|
||||
self._started = None
|
||||
|
||||
def _begin(self, target: SourceBubbleModel, now: float) -> None:
|
||||
if self._current is None:
|
||||
self._current = target
|
||||
return
|
||||
self._from = self._current
|
||||
self._to = target
|
||||
self._started = now
|
||||
|
||||
def _queue(self, model: SourceBubbleModel, now: float) -> None:
|
||||
if self._pending is None or self._pending.structural_key != model.structural_key:
|
||||
self._pending_since = now
|
||||
self._pending = model
|
||||
|
||||
def update(
|
||||
self,
|
||||
model: SourceBubbleModel,
|
||||
now: float,
|
||||
) -> tuple[SourceBubbleModel, SourceBubbleModel | None, float]:
|
||||
"""Return outgoing model, incoming model, and eased incoming alpha."""
|
||||
if self._current is None:
|
||||
self._current = model
|
||||
return model, None, 1.0
|
||||
|
||||
if self._from is not None and self._to is not None and self._started is not None:
|
||||
if model.structural_key == self._to.structural_key:
|
||||
self._to = model
|
||||
elif model.structural_key == self._from.structural_key:
|
||||
self._current = model
|
||||
self._pending = None
|
||||
self._pending_since = None
|
||||
self._from = None
|
||||
self._to = None
|
||||
self._started = None
|
||||
return model, None, 1.0
|
||||
else:
|
||||
self._from = _refresh_model_values(self._from, model)
|
||||
self._queue(model, now)
|
||||
|
||||
progress = (now - self._started) / max(self.duration_seconds, 1e-6)
|
||||
if progress < 1.0:
|
||||
return self._from, self._to, _ease_out_cubic(progress)
|
||||
|
||||
self._current = self._to
|
||||
self._from = None
|
||||
self._to = None
|
||||
self._started = None
|
||||
|
||||
if model.structural_key == self._current.structural_key:
|
||||
self._current = model
|
||||
self._pending = None
|
||||
self._pending_since = None
|
||||
return self._current, None, 1.0
|
||||
|
||||
self._current = _refresh_model_values(self._current, model)
|
||||
self._queue(model, now)
|
||||
if self._pending_since is not None and now - self._pending_since >= self.settle_seconds:
|
||||
target = self._pending
|
||||
self._pending = None
|
||||
self._pending_since = None
|
||||
self._begin(target, now)
|
||||
if self._from is not None and self._to is not None:
|
||||
return self._from, self._to, 0.0
|
||||
|
||||
return self._current, None, 1.0
|
||||
|
||||
@@ -2,7 +2,7 @@ import pyray as rl
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import AetherGauge, _fade
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import AetherGauge
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH
|
||||
|
||||
class AetherGaugeWidget(LayoutWidget):
|
||||
@@ -29,12 +29,10 @@ class AetherGaugeWidget(LayoutWidget):
|
||||
return
|
||||
|
||||
cx = rect.x + rect.width / 2
|
||||
# Set the road bottom to rect.y + 145, leaving 115px for the text cradle underneath
|
||||
bottom = rect.y + 145.0
|
||||
# AetherGauge derives its bounded road and metric viewports from rect.
|
||||
self._aethergauge.render(
|
||||
rect, self._font_bold, self._font_medium,
|
||||
current_speed=self.hud_renderer.speed,
|
||||
cx=cx,
|
||||
bottom=bottom,
|
||||
alpha=alpha,
|
||||
)
|
||||
|
||||
@@ -6,6 +6,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import (
|
||||
_get_slc_state, render_speed_limit_at, EU_SIGN_SIZE,
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import SourceBubbleTransition
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH, SLC_HEIGHT
|
||||
|
||||
|
||||
@@ -16,6 +17,7 @@ class SpeedLimitWidget(LayoutWidget):
|
||||
super().__init__("speed_limit", priority=2)
|
||||
self._slc_state: dict | None = None
|
||||
self._sign_rect: Optional[rl.Rectangle] = None
|
||||
self._source_bubble_transition = SourceBubbleTransition()
|
||||
|
||||
@property
|
||||
def _hit_rect(self) -> rl.Rectangle:
|
||||
@@ -33,6 +35,7 @@ class SpeedLimitWidget(LayoutWidget):
|
||||
self._slc_state = _get_slc_state()
|
||||
if self._slc_state is None:
|
||||
self._sign_rect = None
|
||||
self._source_bubble_transition.reset()
|
||||
return False
|
||||
return True
|
||||
|
||||
@@ -51,7 +54,14 @@ class SpeedLimitWidget(LayoutWidget):
|
||||
return
|
||||
params = ui_state.ui_params
|
||||
expanded = params.get_bool("SpeedLimitSources")
|
||||
self._sign_rect = render_speed_limit_at(self._slc_state, rect, expanded)
|
||||
if not expanded:
|
||||
self._source_bubble_transition.reset()
|
||||
self._sign_rect = render_speed_limit_at(
|
||||
self._slc_state,
|
||||
rect,
|
||||
expanded,
|
||||
self._source_bubble_transition,
|
||||
)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos) -> None:
|
||||
state = self._slc_state
|
||||
|
||||
@@ -31,16 +31,20 @@ mock_ui_state = types.SimpleNamespace(
|
||||
},
|
||||
)
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import aethergauge
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import widget_style
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import (
|
||||
AetherGauge,
|
||||
AetherGaugeData,
|
||||
IndicatorType,
|
||||
_aether_layout,
|
||||
_cem_curvature_data,
|
||||
_draw_aether_card,
|
||||
_is_cem_curvature,
|
||||
_is_curve_speed,
|
||||
_is_lead,
|
||||
_is_stop_light,
|
||||
_lead_data,
|
||||
_state_label,
|
||||
)
|
||||
from openpilot.starpilot.common.experimental_state import CEStatus
|
||||
|
||||
@@ -295,7 +299,7 @@ def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
|
||||
|
||||
rendered_data = []
|
||||
gauge = AetherGauge()
|
||||
monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, cy, data, fb, fm, alpha: rendered_data.append(data))
|
||||
monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, data, fb, fm, alpha: rendered_data.append(data))
|
||||
|
||||
# Initial frame at 30m
|
||||
_set_plan(redLight=True, forcingStopLength=30.0)
|
||||
@@ -307,3 +311,137 @@ def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
|
||||
gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
|
||||
# Ratchet clamp must prevent the display number from increasing above 30m!
|
||||
assert int(rendered_data[-1].text) <= 30
|
||||
|
||||
|
||||
def test_aether_card_reuses_the_shared_control_frame(monkeypatch):
|
||||
triangles = []
|
||||
fills = []
|
||||
keylines = []
|
||||
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_triangle", lambda *args: triangles.append(args))
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded", lambda *args: fills.append(args))
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded_lines_ex", lambda *args: keylines.append(args))
|
||||
|
||||
rect = aethergauge.rl.Rectangle(10, 20, 176, 260)
|
||||
layout = _aether_layout(rect)
|
||||
_draw_aether_card(layout, 0.5)
|
||||
|
||||
assert not triangles
|
||||
assert len(fills) == 1
|
||||
assert len(keylines) == 1
|
||||
|
||||
card, roundness, segments, fill = fills[0]
|
||||
keyline_card, keyline_roundness, keyline_segments, border_width, border = keylines[0]
|
||||
assert card.x == rect.x
|
||||
assert card.y == rect.y
|
||||
assert card.width == rect.width
|
||||
assert card.height == rect.height
|
||||
assert keyline_card.x == card.x
|
||||
assert keyline_card.y == card.y
|
||||
assert keyline_card.width == card.width
|
||||
assert keyline_card.height == card.height
|
||||
assert roundness == keyline_roundness == widget_style.CONTROL_ROUNDNESS
|
||||
assert segments == keyline_segments == widget_style.CONTROL_SEGMENTS
|
||||
assert border_width == widget_style.CONTROL_BORDER_WIDTH
|
||||
assert fill.a == int(aethergauge.COLOR_AETHER_CARD.a * 0.5)
|
||||
assert (border.r, border.g, border.b, border.a) == (
|
||||
widget_style.CONTROL_BORDER.r,
|
||||
widget_style.CONTROL_BORDER.g,
|
||||
widget_style.CONTROL_BORDER.b,
|
||||
int(widget_style.CONTROL_BORDER.a * 0.5),
|
||||
)
|
||||
|
||||
|
||||
def test_aether_layout_keeps_road_and_cradle_inside_one_card():
|
||||
layout = _aether_layout(aethergauge.rl.Rectangle(10, 20, 176, 260))
|
||||
card_right = layout.card.x + layout.card.width
|
||||
card_bottom = layout.card.y + layout.card.height
|
||||
inner_border = widget_style.CONTROL_BORDER_WIDTH / 2.0
|
||||
|
||||
for viewport in (layout.road, layout.cradle):
|
||||
assert layout.card.x <= viewport.x
|
||||
assert viewport.x + viewport.width <= card_right
|
||||
assert layout.card.y <= viewport.y
|
||||
assert viewport.y + viewport.height <= card_bottom
|
||||
assert layout.road.x >= layout.card.x + inner_border
|
||||
assert layout.road.y >= layout.card.y + inner_border
|
||||
assert layout.cradle.y + layout.cradle.height <= card_bottom - inner_border
|
||||
assert layout.road.y + layout.road.height < layout.cradle.y
|
||||
|
||||
|
||||
def _fake_text_size(_, text, size):
|
||||
return aethergauge.rl.Vector2(max(1, len(text) * size * 0.5), size)
|
||||
|
||||
|
||||
def _assert_text_calls_fit_card(text_calls, card):
|
||||
card_right = card.x + card.width
|
||||
card_bottom = card.y + card.height
|
||||
for text, pos, size in text_calls:
|
||||
measured = _fake_text_size(None, text, size)
|
||||
assert card.x <= pos.x
|
||||
assert pos.x + measured.x <= card_right
|
||||
assert card.y <= pos.y
|
||||
assert pos.y + measured.y <= card_bottom
|
||||
|
||||
|
||||
@pytest.mark.parametrize("data", [
|
||||
AetherGaugeData(
|
||||
text="120", unit="km/h", color=aethergauge.COLOR_CEM_SPEED,
|
||||
indicator_type=IndicatorType.ROAD_CURVE, reduction_text="-20", is_numeric=True,
|
||||
),
|
||||
AetherGaugeData(
|
||||
text="999", unit="ft", color=aethergauge.COLOR_FORCE_STOP,
|
||||
indicator_type=IndicatorType.STOP_LIGHT, is_numeric=True,
|
||||
),
|
||||
AetherGaugeData(
|
||||
text="STOPPED", color=aethergauge.COLOR_LEAD_STOPPED,
|
||||
indicator_type=IndicatorType.LEAD,
|
||||
),
|
||||
])
|
||||
def test_aether_cradle_fits_text_within_card(monkeypatch, data):
|
||||
text_calls = []
|
||||
monkeypatch.setattr(aethergauge, "measure_text_cached", _fake_text_size)
|
||||
monkeypatch.setattr(
|
||||
aethergauge.rl, "draw_text_ex",
|
||||
lambda _, text, pos, size, __, ___: text_calls.append((text, pos, size)),
|
||||
)
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_line_ex", lambda *args: None)
|
||||
|
||||
layout = _aether_layout(aethergauge.rl.Rectangle(10, 20, 176, 260))
|
||||
AetherGauge()._draw_mini_cradle(layout.cradle, data, object(), object())
|
||||
|
||||
_assert_text_calls_fit_card(text_calls, layout.card)
|
||||
|
||||
|
||||
def test_aether_road_render_scissors_the_animated_viewport(monkeypatch):
|
||||
scissor_calls = []
|
||||
events = []
|
||||
monkeypatch.setattr(aethergauge, "measure_text_cached", _fake_text_size)
|
||||
monkeypatch.setattr(aethergauge.rl, "begin_scissor_mode", lambda *args: scissor_calls.append(args))
|
||||
monkeypatch.setattr(aethergauge.rl, "end_scissor_mode", lambda: events.append("end_scissor"))
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded", lambda *args: None)
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_triangle", lambda *args: None)
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_line_ex", lambda *args: None)
|
||||
monkeypatch.setattr(aethergauge.rl, "draw_text_ex", lambda *args: events.append("text"))
|
||||
monkeypatch.setattr(aethergauge.rl, "get_frame_time", lambda: 0.0)
|
||||
|
||||
rect = aethergauge.rl.Rectangle(10, 20, 176, 260)
|
||||
layout = _aether_layout(rect)
|
||||
data = AetherGaugeData(
|
||||
text="23", unit="mph", color=aethergauge.COLOR_CEM_SPEED,
|
||||
indicator_type=IndicatorType.ROAD_CURVE, indicator_value=0.005, is_numeric=True,
|
||||
)
|
||||
AetherGauge()._render_unified_road(rect, 98.0, data, object(), object())
|
||||
|
||||
assert scissor_calls == [(
|
||||
int(layout.road.x), int(layout.road.y), int(layout.road.width), int(layout.road.height),
|
||||
)]
|
||||
assert events.index("end_scissor") < events.index("text")
|
||||
|
||||
|
||||
def test_state_labels_make_urgent_and_lead_states_explicit():
|
||||
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.FORCE_STOP)) == "STOP"
|
||||
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.STOP_LIGHT)) == "RED LIGHT"
|
||||
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.LEAD)) == "LEAD"
|
||||
assert _state_label(AetherGaugeData(text="", indicator_type=IndicatorType.ROAD_CURVE)) == ""
|
||||
|
||||
@@ -1,36 +1,117 @@
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
|
||||
SourceBubbleItem,
|
||||
SourceBubbleModel,
|
||||
SourceBubbleTransition,
|
||||
enabled_source_titles,
|
||||
fit_source_label,
|
||||
source_abbreviated_value_text,
|
||||
source_content_metrics,
|
||||
source_value_text,
|
||||
visible_source_rows,
|
||||
)
|
||||
|
||||
|
||||
def test_source_content_metrics_scale_with_visible_row_count():
|
||||
assert source_content_metrics(3) == (30, 34, 7)
|
||||
assert source_content_metrics(4) == (30, 32, 7)
|
||||
assert source_content_metrics(5) == (28, 30, 6)
|
||||
|
||||
|
||||
def test_fit_source_label_preserves_a_safe_value_column_gap():
|
||||
def width(text: str) -> int:
|
||||
return len(text) * 10
|
||||
|
||||
assert fit_source_label("Dashboard", "Dash", 80, width) == "Dash"
|
||||
assert fit_source_label("Vision", "Vision", 70, width) == "Vision"
|
||||
assert fit_source_label("Dashboard", "Dash", 20, width) == "D…"
|
||||
|
||||
|
||||
def test_source_value_text_keeps_missing_values_as_a_dash():
|
||||
assert source_value_text(0) == "–"
|
||||
assert source_value_text(0.1) == "–"
|
||||
assert source_value_text(55) == "55"
|
||||
assert source_value_text(float("nan")) == "–"
|
||||
assert source_value_text(float("inf")) == "–"
|
||||
assert source_abbreviated_value_text(0) == "X"
|
||||
assert source_abbreviated_value_text(55) == "55"
|
||||
|
||||
|
||||
def test_source_bubble_ignores_legacy_abbreviation_state(monkeypatch):
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
|
||||
|
||||
text_calls = []
|
||||
icon_calls = []
|
||||
monkeypatch.setattr(slc, "_get_bold", lambda: object())
|
||||
monkeypatch.setattr(slc, "_get_semi_bold", lambda: object())
|
||||
monkeypatch.setattr(slc, "tr", lambda text: text)
|
||||
monkeypatch.setattr(
|
||||
slc,
|
||||
"measure_text_cached",
|
||||
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
|
||||
)
|
||||
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_line_ex", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_text_ex", lambda _font, text, *_args: text_calls.append(text))
|
||||
monkeypatch.setattr(slc, "_draw_source_icon", lambda icon_key, *_args: icon_calls.append(icon_key))
|
||||
|
||||
state = {
|
||||
"speed_limit_source": "Dashboard",
|
||||
"slc_enabled_sources": ("Dashboard", "Map Data"),
|
||||
"slc_active_sources_only": False,
|
||||
"dashboard_sl": 45.0,
|
||||
"map_sl": 30.0,
|
||||
}
|
||||
|
||||
def render(abbreviated: bool):
|
||||
text_calls.clear()
|
||||
icon_calls.clear()
|
||||
slc._draw_sources_bubble(
|
||||
{**state, "slc_abbreviated_sources": abbreviated},
|
||||
slc.rl.Rectangle(0, 0, 176, 196),
|
||||
)
|
||||
return list(icon_calls), list(text_calls)
|
||||
|
||||
abbreviated_rows = render(True)
|
||||
full_rows = render(False)
|
||||
|
||||
assert abbreviated_rows == full_rows
|
||||
assert abbreviated_rows[0] == ["dashboard", "map"]
|
||||
assert abbreviated_rows[1] == ["Dashboard", "45", "Map Data", "30"]
|
||||
|
||||
|
||||
def test_dense_source_bubble_uses_distinct_icons_and_no_source_names(monkeypatch):
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
|
||||
|
||||
text_calls = []
|
||||
icon_calls = []
|
||||
monkeypatch.setattr(slc, "_get_bold", lambda: object())
|
||||
monkeypatch.setattr(slc, "_get_semi_bold", lambda: object())
|
||||
monkeypatch.setattr(slc, "tr", lambda text: text)
|
||||
monkeypatch.setattr(
|
||||
slc,
|
||||
"measure_text_cached",
|
||||
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
|
||||
)
|
||||
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_line_ex", lambda *args: None)
|
||||
monkeypatch.setattr(slc.rl, "draw_text_ex", lambda _font, text, *_args: text_calls.append(text))
|
||||
monkeypatch.setattr(slc, "_draw_source_icon", lambda icon_key, *_args: icon_calls.append(icon_key))
|
||||
|
||||
state = {
|
||||
"speed_limit_source": "Mapbox",
|
||||
"slc_enabled_sources": ("Dashboard", "Map Data", "Vision", "Mapbox", "Upcoming"),
|
||||
"slc_active_sources_only": False,
|
||||
"dashboard_sl": 45.0,
|
||||
"map_sl": 30.0,
|
||||
"vision_sl": 50.0,
|
||||
"mapbox_sl": 35.0,
|
||||
"next_sl": 20.0,
|
||||
}
|
||||
|
||||
slc._draw_sources_bubble(state, slc.rl.Rectangle(0, 0, 176, 196))
|
||||
|
||||
assert text_calls == ["45", "30", "50", "35", "Next", "20"]
|
||||
assert icon_calls == ["dashboard", "map", "camera", "navigation", "next"]
|
||||
|
||||
|
||||
def test_dense_source_group_fits_three_digit_values_inside_cell(monkeypatch):
|
||||
from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc
|
||||
|
||||
monkeypatch.setattr(
|
||||
slc,
|
||||
"measure_text_cached",
|
||||
lambda _font, text, size: slc.rl.Vector2(len(text) * size * 0.5, size),
|
||||
)
|
||||
|
||||
icon_size, value_font, value_size = slc._fit_dense_group(
|
||||
object(), "120", slc.rl.Rectangle(0, 0, 114, 49), 36, 48,
|
||||
)
|
||||
|
||||
assert icon_size + slc._SOURCE_DENSE_GAP + value_size.x <= 106
|
||||
assert max(icon_size, value_size.y) <= 47
|
||||
assert value_font < 48
|
||||
|
||||
|
||||
def test_enabled_source_titles_follow_priority_and_fallback_settings():
|
||||
@@ -59,29 +140,106 @@ def test_visible_source_rows_honor_active_only_and_source_order():
|
||||
]
|
||||
values = {"dashboard": 45.0, "map": 0.0, "vision": 50.0, "mapbox": 30.0, "next": 20.0}
|
||||
|
||||
# Map Data has value 0.0, so it is omitted; Dashboard (45.0) is active
|
||||
# Map Data has no reading but remains visible when active-only is disabled.
|
||||
assert visible_source_rows(
|
||||
source_defs, values, "Dashboard", ("Dashboard", "Map Data"),
|
||||
) == [
|
||||
("Dashboard", "dashboard", 45.0, True),
|
||||
]
|
||||
# When Map Data is the active target but has 0.0 reading, Dashboard is inactive (available standby)
|
||||
) == (
|
||||
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),
|
||||
SourceBubbleItem("Map Data", "Map Data", "map", 0.0, False, False),
|
||||
)
|
||||
# Active-only hides the unavailable Map Data row and preserves canonical order.
|
||||
assert visible_source_rows(
|
||||
source_defs, values, "Map Data", ("Dashboard", "Map Data"),
|
||||
) == [
|
||||
("Dashboard", "dashboard", 45.0, False),
|
||||
]
|
||||
source_defs, values, "Map Data", ("Dashboard", "Map Data"), active_only=True,
|
||||
) == (
|
||||
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, False),
|
||||
)
|
||||
# Multiple available sources with readings appear in canonical order
|
||||
assert visible_source_rows(
|
||||
source_defs, values, "Vision", ("Dashboard", "Map Data", "Vision"),
|
||||
) == [
|
||||
("Dashboard", "dashboard", 45.0, False),
|
||||
("Vision", "camera", 50.0, True),
|
||||
]
|
||||
# When no sources have a valid speed reading (> 0), returns empty list (triggers empty state)
|
||||
) == (
|
||||
SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, False),
|
||||
SourceBubbleItem("Map Data", "Map Data", "map", 0.0, False, False),
|
||||
SourceBubbleItem("Vision", "Vision", "camera", 50.0, True, True),
|
||||
)
|
||||
# Active-only removes all sources without a valid speed reading.
|
||||
assert visible_source_rows(
|
||||
source_defs, {key: 0.0 for key in values}, "Map Data", ("Map Data",),
|
||||
) == []
|
||||
source_defs, dict.fromkeys(values, 0.0), "Map Data", ("Map Data",), active_only=True,
|
||||
) == ()
|
||||
|
||||
|
||||
def test_source_bubble_models_select_readable_and_dense_modes():
|
||||
item = SourceBubbleItem("Vision", "Vision", "camera", 50.0, True, True)
|
||||
assert SourceBubbleModel((item,)).mode == "readable"
|
||||
assert SourceBubbleModel(tuple(item for _ in range(3))).mode == "readable"
|
||||
assert SourceBubbleModel(tuple(item for _ in range(4))).mode == "dense"
|
||||
assert SourceBubbleModel(tuple(item for _ in range(5))).mode == "dense"
|
||||
assert SourceBubbleModel((), "NO SOURCE DATA").mode == "empty"
|
||||
|
||||
|
||||
def test_source_bubble_transition_settles_and_crossfades_structural_changes():
|
||||
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
|
||||
second = SourceBubbleModel(tuple(
|
||||
SourceBubbleItem(source, source, icon, value, True, source == "Vision")
|
||||
for source, icon, value in (
|
||||
("Dashboard", "dashboard", 45.0),
|
||||
("Map Data", "map", 30.0),
|
||||
("Vision", "camera", 50.0),
|
||||
("Mapbox", "navigation", 35.0),
|
||||
)
|
||||
))
|
||||
transition = SourceBubbleTransition()
|
||||
|
||||
assert transition.update(first, 0.0) == (first, None, 1.0)
|
||||
outgoing, incoming, alpha = transition.update(second, 0.05)
|
||||
assert outgoing.structural_key == first.structural_key
|
||||
assert outgoing.items[0].is_active is False
|
||||
assert incoming is None
|
||||
assert alpha == 1.0
|
||||
outgoing, incoming, alpha = transition.update(second, 0.18)
|
||||
assert outgoing.structural_key == first.structural_key
|
||||
assert incoming is second
|
||||
assert alpha == 0.0
|
||||
|
||||
_, incoming, alpha = transition.update(second, 0.28)
|
||||
assert incoming is second
|
||||
assert 0.0 < alpha < 1.0
|
||||
|
||||
assert transition.update(second, 0.5) == (second, None, 1.0)
|
||||
|
||||
|
||||
def test_source_bubble_transition_coalesces_latest_pending_layout():
|
||||
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
|
||||
second = SourceBubbleModel(tuple(
|
||||
SourceBubbleItem(source, source, icon, 45.0, True, False)
|
||||
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"))
|
||||
))
|
||||
third = SourceBubbleModel(tuple(
|
||||
SourceBubbleItem(source, source, icon, 45.0, True, False)
|
||||
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"), ("Vision", "camera"))
|
||||
))
|
||||
transition = SourceBubbleTransition()
|
||||
transition.update(first, 0.0)
|
||||
transition.update(second, 0.05)
|
||||
transition.update(third, 0.10)
|
||||
outgoing, incoming, alpha = transition.update(third, 0.23)
|
||||
assert outgoing.structural_key == first.structural_key
|
||||
assert incoming is third
|
||||
assert alpha == 0.0
|
||||
|
||||
|
||||
def test_source_bubble_transition_reverses_to_the_latest_roster():
|
||||
first = SourceBubbleModel((SourceBubbleItem("Dashboard", "Dashboard", "dashboard", 45.0, True, True),))
|
||||
second = SourceBubbleModel(tuple(
|
||||
SourceBubbleItem(source, source, icon, 45.0, True, False)
|
||||
for source, icon in (("Dashboard", "dashboard"), ("Map Data", "map"))
|
||||
))
|
||||
transition = SourceBubbleTransition()
|
||||
transition.update(first, 0.0)
|
||||
transition.update(second, 0.05)
|
||||
transition.update(second, 0.18)
|
||||
|
||||
assert transition.update(first, 0.20) == (first, None, 1.0)
|
||||
assert transition.update(first, 0.40) == (first, None, 1.0)
|
||||
|
||||
|
||||
def test_source_label_color_override_and_engagement_states():
|
||||
|
||||
@@ -3,6 +3,8 @@ from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
|
||||
|
||||
ACCELERATION_PROFILES = {
|
||||
"STANDARD": 0,
|
||||
"ECO": 1,
|
||||
|
||||
@@ -19,9 +19,7 @@ CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
|
||||
CSC_CURVE_RELEASE_HOLD_TIME = 0.75
|
||||
OVERRIDE_FORCE_STOP_TIMER = 10
|
||||
STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75
|
||||
# Open-loop — green is undetectable at standstill, so this only needs to cover the
|
||||
# handoff to CEM+model ownership. Extra seconds are pure departure lag.
|
||||
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 2.0
|
||||
STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0
|
||||
FORCE_STOP_LIGHT_CLEAR_TIME = 0.5
|
||||
SLC_LEAD_DROP_RELAXATION_MIN_SPEED = 20.0 * CV.MPH_TO_MS
|
||||
SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE = 30.0
|
||||
@@ -59,8 +57,6 @@ LEAD_VETO_M_OVERRIDES = {
|
||||
}
|
||||
FORCE_STOP_APPROACH_DECEL = 0.65 # m/s^2 — speed ceiling before commit. LOWER = more early
|
||||
# braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL
|
||||
# approachStopLength is published RAW: model_length converges from above, so rate-limiting
|
||||
# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add.
|
||||
ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard
|
||||
DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap
|
||||
# tracked length closer when dashboard confirms a sign
|
||||
@@ -78,14 +74,6 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
|
||||
FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
|
||||
FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
|
||||
FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise
|
||||
FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldStop doesn't
|
||||
# assert until ~10 m, so horizon jitter would release the stop
|
||||
FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed
|
||||
# what it was at commit minus distance driven. Slack covers an
|
||||
# under-read at commit; without it that would stop us short.
|
||||
FORCE_STOP_CAP_TAPER_M = 60.0 # m — slack fades to 0 as the cap closes. The solver aims at
|
||||
# tracked, so slack held near the line is braking for a stop bar
|
||||
# that far past the real one.
|
||||
|
||||
# Knob bounds (mirror of UI slider; defense in depth)
|
||||
OFFSET_FT_MIN = -20
|
||||
@@ -189,11 +177,9 @@ class StarPilotVCruise:
|
||||
self.force_stop_from_light = False
|
||||
self.force_stop_light_clear_since = None
|
||||
self.controls_enabled_previously = False
|
||||
self.approach_stop_length = 0.0 # published as starpilotPlan.approachStopLength
|
||||
# Kinematic distance estimator. Same attribute also published as
|
||||
# starpilotPlan.forcingStopLength, so the existing reader keeps working.
|
||||
self.tracked_model_length = 0.0
|
||||
self.force_stop_distance_cap = 0.0 # odometry ceiling, re-seeded until commit
|
||||
|
||||
self.stop_sign_confirmed = False
|
||||
self.stop_seen_on_approach_at = None
|
||||
@@ -617,9 +603,6 @@ class StarPilotVCruise:
|
||||
offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw))
|
||||
offset_m = offset_ft * FT_TO_M
|
||||
|
||||
# cleared on every path; only the far-approach envelope below republishes it
|
||||
self.approach_stop_length = 0.0
|
||||
|
||||
if force_standstill_enabled and not self.override_force_standstill:
|
||||
self.forcing_stop = True
|
||||
self.tracked_model_length = 0.0
|
||||
@@ -647,7 +630,7 @@ class StarPilotVCruise:
|
||||
model_wants_stop = False
|
||||
if (
|
||||
not dash_active and
|
||||
self.tracked_model_length > max(force_stop_handoff_m, FORCE_STOP_REANCHOR_MIN_M) and
|
||||
self.tracked_model_length > force_stop_handoff_m and
|
||||
not model_wants_stop and
|
||||
model_length > self.tracked_model_length + FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP and
|
||||
(
|
||||
@@ -659,12 +642,6 @@ class StarPilotVCruise:
|
||||
self.tracked_model_length = model_length
|
||||
else:
|
||||
self.tracked_model_length = min(self.tracked_model_length, model_length)
|
||||
# Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we
|
||||
# had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m)
|
||||
# that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate.
|
||||
self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0)
|
||||
cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0)
|
||||
self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack)
|
||||
if dash_active:
|
||||
if model_length < DASH_MODEL_AGREE_M:
|
||||
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
|
||||
@@ -698,7 +675,6 @@ class StarPilotVCruise:
|
||||
self.stop_sign_confirmed = False
|
||||
|
||||
self.tracked_model_length = self.starpilot_planner.model_length
|
||||
self.force_stop_distance_cap = self.tracked_model_length
|
||||
|
||||
targets = [v_cruise]
|
||||
if self.csc_target >= CSC_MIN_SPEED:
|
||||
@@ -744,8 +720,6 @@ class StarPilotVCruise:
|
||||
adjacent_stop_d = self._get_adjacent_stop_distance(sm)
|
||||
if adjacent_stop_d is not None:
|
||||
approach_d = min(approach_d, adjacent_stop_d)
|
||||
# pre-offset, so it hands off to forcingStopLength at commit without a step
|
||||
self.approach_stop_length = max(approach_d, 0.0)
|
||||
approach_d += offset_m + force_stop_distance_bias_m
|
||||
if approach_d > force_stop_handoff_m:
|
||||
targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m)))
|
||||
|
||||
@@ -31,10 +31,7 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
|
||||
from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
|
||||
|
||||
RADARLESS_TRACK_HOLD_TIME = 0.45
|
||||
FORCE_STOP_JERK_SCALE = 0.20 # accel-change cost multiplier for the whole stop approach,
|
||||
# envelope included (125 -> 25). Lower = reaches the braking
|
||||
# target sooner; it does not make the target deeper. Response
|
||||
# is super-linear here, so raise it if onset feels like a step.
|
||||
FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40)
|
||||
FORCE_STOP_JERK_SCALE_OVERRIDES = {
|
||||
# The Elantra's current force-stop ramp is smooth, but it waits too long
|
||||
# before building decel and then arrives at the initial brake too abruptly.
|
||||
@@ -311,9 +308,7 @@ class StarPilotPlanner:
|
||||
except (KeyError, IndexError, TypeError, AttributeError):
|
||||
car_params = None
|
||||
|
||||
# Also while the far-approach envelope is running: at onset the ramp reaches only
|
||||
# ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost.
|
||||
if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0:
|
||||
if self.starpilot_vcruise.forcing_stop:
|
||||
jerk_scale = get_force_stop_jerk_scale(car_params)
|
||||
elif self.tracking_lead:
|
||||
# Elantra vision leads can hand off from cruise to lead0 while closing
|
||||
@@ -351,7 +346,6 @@ class StarPilotPlanner:
|
||||
|
||||
starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
|
||||
starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
|
||||
starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length)
|
||||
starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed
|
||||
|
||||
starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
from __future__ import annotations
|
||||
from typing import cast
|
||||
import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time
|
||||
import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit
|
||||
assert sys.platform != 'win32'
|
||||
from dataclasses import dataclass
|
||||
from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface
|
||||
@@ -25,7 +25,6 @@ SQTT = ContextVar("SQTT", abs(VIZ.value)>=2)
|
||||
SQTT_ITRACE_SE_MASK, SQTT_LIMIT_SE, SQTT_SIMD_SEL, SQTT_TOKEN_EXCLUDE = \
|
||||
ContextVar("SQTT_ITRACE_SE_MASK", 0b11), ContextVar("SQTT_LIMIT_SE", 0), ContextVar("SQTT_SIMD_SEL", 0), ContextVar("SQTT_TOKEN_EXCLUDE", 0)
|
||||
PMC = ContextVar("PMC", abs(VIZ.value)>=2)
|
||||
AMD_USB_POLL_US = getenv("AMD_USB_POLL_US", 500) # microseconds to sleep between USB signal polls. 0 disables
|
||||
EVENT_INDEX_PARTIAL_FLUSH = 4 # based on a comment in nvd.h
|
||||
WAIT_REG_MEM_FUNCTION_EQ = 3 # ==
|
||||
WAIT_REG_MEM_FUNCTION_NEQ = 4 # !=
|
||||
@@ -46,9 +45,8 @@ class AMDSignal(HCQSignal):
|
||||
def __init__(self, *args, **kwargs): super().__init__(*args, **{**kwargs, 'timestamp_divider': 100})
|
||||
|
||||
def _sleep(self, time_spent_since_last_sleep_ms:int):
|
||||
# USB signals live in VRAM across the link, so yield between polls. Native AMD only blocks after 200 ms.
|
||||
if self.owner is not None and self.owner.is_usb() and AMD_USB_POLL_US: time.sleep(AMD_USB_POLL_US / 1e6)
|
||||
elif time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
|
||||
# Reasonable to sleep for long workloads (which take more than 200ms) and only timeline signals.
|
||||
if time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
|
||||
|
||||
class AMDComputeQueue(HWQueue):
|
||||
def __init__(self, dev:AMDDevice):
|
||||
|
||||
Reference in New Issue
Block a user