mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-05 08:15:58 +08:00
CEM / NO CAMERA / UI / REDNECK
This commit is contained in:
firestar5683
@@ -5,7 +5,10 @@ from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, create_gas_in
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.values import ASCM_INT, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags
|
||||
from opendbc.car.gm.values import (
|
||||
ASCM_INT, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
CruiseButtons, GMFlags,
|
||||
)
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from openpilot.common.params import Params, UnknownKeyName
|
||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
@@ -66,6 +69,14 @@ def should_spoof_ecm_cruise_status(CP):
|
||||
)
|
||||
|
||||
|
||||
def should_send_cc_button_spam(CP, CC, CS):
|
||||
return (
|
||||
bool(CP.flags & GMFlags.CC_LONG.value) and
|
||||
CC.longActive and
|
||||
CS.out.vEgo > CP.minEnableSpeed
|
||||
)
|
||||
|
||||
|
||||
def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int:
|
||||
return int(round(np.interp(v_ego, [0.0, 6.0, 15.0, 30.0], [40.0, 85.0, 130.0, 170.0])))
|
||||
|
||||
@@ -478,7 +489,11 @@ class CarController(CarControllerBase):
|
||||
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
|
||||
|
||||
maneuver_sng_launch = self.longitudinal_maneuver_mode and self.is_volt
|
||||
if self.CP.enableGasInterceptorDEPRECATED and self.apply_gas > self.params.INACTIVE_REGEN and use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch):
|
||||
if (
|
||||
self.CP.enableGasInterceptorDEPRECATED and
|
||||
self.apply_gas > self.params.INACTIVE_REGEN and
|
||||
use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch)
|
||||
):
|
||||
interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS
|
||||
if maneuver_sng_launch:
|
||||
interceptor_gas_cmd = max(interceptor_gas_cmd, float(np.interp(actuators.accel, [0.0, 1.0, 2.0], [self.params.SNG_INTERCEPTOR_GAS, 0.11, 0.16])))
|
||||
@@ -501,7 +516,7 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, paddle_spoof_pressed))
|
||||
|
||||
if self.CP.flags & GMFlags.CC_LONG.value:
|
||||
if CC.longActive and CS.out.cruiseState.enabled and CS.out.vEgo > self.CP.minEnableSpeed:
|
||||
if should_send_cc_button_spam(self.CP, CC, CS):
|
||||
# Using extend instead of append since the message is only sent intermittently
|
||||
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles))
|
||||
elif (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
from opendbc.car import DT_CTRL
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.values import CAR, CanBus, CruiseButtons
|
||||
from opendbc.car.gm.values import CAR, CanBus, CruiseButtons, GMFlags
|
||||
|
||||
MALIBU_BUTTON_TABLE = {
|
||||
0: [0x10FF, 0x15EE, 0x1ADD, 0x1FCC],
|
||||
@@ -314,7 +314,13 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl
|
||||
controller.malibu_button_phase = (controller.malibu_button_phase + 1) % 4
|
||||
return msgs
|
||||
idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV
|
||||
return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruise_btn)]
|
||||
msgs = [create_buttons(packer, CanBus.POWERTRAIN, idx, cruise_btn)]
|
||||
|
||||
# Flashed camera-forward Volt CC installs also need the button spoof on the
|
||||
# camera side. Removed-camera installs set NO_CAMERA and keep this PT-only.
|
||||
if CS.CP.carFingerprint == CAR.CHEVROLET_VOLT_CC and not (CS.CP.flags & GMFlags.NO_CAMERA.value):
|
||||
msgs.append(create_buttons(packer, CanBus.CAMERA, idx, cruise_btn))
|
||||
return msgs
|
||||
else:
|
||||
return []
|
||||
|
||||
|
||||
@@ -612,7 +612,9 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.flags |= GMFlags.FORCE_BRAKE_C9.value
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_FORCE_BRAKE_C9.value
|
||||
|
||||
if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and candidate not in SDGM_CAR:
|
||||
# Exception for flashed cars, or cars whose camera was removed.
|
||||
missing_camera_msg = CAM_MSG not in fingerprint.get(CanBus.CAMERA, {})
|
||||
if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and missing_camera_msg and candidate not in SDGM_CAR:
|
||||
ret.flags |= GMFlags.NO_CAMERA.value
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_NO_CAMERA.value
|
||||
|
||||
|
||||
@@ -2,12 +2,15 @@ import pytest
|
||||
from types import SimpleNamespace
|
||||
from parameterized import parameterized
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm.carcontroller import should_spoof_dash_speed
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.gm.carcontroller import should_send_cc_button_spam, should_spoof_dash_speed
|
||||
import opendbc.car.gm.interface as gm_interface
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.fingerprints import FINGERPRINTS
|
||||
from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, GM_RX_OFFSET
|
||||
from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, GMFlags, GMSafetyFlags
|
||||
|
||||
CAMERA_DIAGNOSTIC_ADDRESS = 0x24b
|
||||
VOLT_CARS = (
|
||||
@@ -78,6 +81,19 @@ class TestGMInterface:
|
||||
assert car_params.startingState
|
||||
assert car_params.startAccel == pytest.approx(1.15)
|
||||
|
||||
def test_volt_cc_sparse_fingerprint_without_camera_sets_no_camera(self):
|
||||
CarInterface = interfaces[CAR.CHEVROLET_VOLT_CC]
|
||||
fingerprint = {
|
||||
0: FINGERPRINTS[CAR.CHEVROLET_VOLT][0].copy(),
|
||||
1: {},
|
||||
}
|
||||
|
||||
car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_CC, fingerprint, [], alpha_long=False, is_release=False, docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles())
|
||||
|
||||
assert car_params.flags & GMFlags.NO_CAMERA.value
|
||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_CAMERA.value
|
||||
|
||||
|
||||
class TestGMCarController:
|
||||
def test_dash_speed_spoof_respects_live_stock_acc_toggles(self):
|
||||
@@ -91,3 +107,80 @@ class TestGMCarController:
|
||||
cp = SimpleNamespace(openpilotLongitudinalControl=True, enableGasInterceptorDEPRECATED=False)
|
||||
|
||||
assert should_spoof_dash_speed(cp, SimpleNamespace(disable_openpilot_long=False))
|
||||
|
||||
def test_cc_button_spam_does_not_require_stock_cruise_enabled(self):
|
||||
cp = SimpleNamespace(flags=GMFlags.CC_LONG.value, minEnableSpeed=10.0)
|
||||
cc = SimpleNamespace(longActive=True)
|
||||
cs = SimpleNamespace(out=SimpleNamespace(vEgo=11.0, cruiseState=SimpleNamespace(enabled=False)))
|
||||
|
||||
assert should_send_cc_button_spam(cp, cc, cs)
|
||||
|
||||
def test_cc_button_spam_requires_cc_long_and_speed(self):
|
||||
cc = SimpleNamespace(longActive=True)
|
||||
cs = SimpleNamespace(out=SimpleNamespace(vEgo=9.0, cruiseState=SimpleNamespace(enabled=True)))
|
||||
|
||||
assert not should_send_cc_button_spam(SimpleNamespace(flags=GMFlags.CC_LONG.value, minEnableSpeed=10.0), cc, cs)
|
||||
assert not should_send_cc_button_spam(SimpleNamespace(flags=0, minEnableSpeed=10.0), cc, cs)
|
||||
|
||||
def test_volt_cc_redneck_spam_is_mirrored_to_camera_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=0,
|
||||
minEnableSpeed=24 * CV.MPH_TO_MS,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=25.0,
|
||||
cruiseState=SimpleNamespace(speed=20.0),
|
||||
),
|
||||
)
|
||||
actuators = SimpleNamespace(accel=1.0)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False))
|
||||
|
||||
assert [msg[2] for msg in msgs] == [0, 2]
|
||||
|
||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
minEnableSpeed=24 * CV.MPH_TO_MS,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=25.0,
|
||||
cruiseState=SimpleNamespace(speed=20.0),
|
||||
),
|
||||
)
|
||||
actuators = SimpleNamespace(accel=1.0)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False))
|
||||
|
||||
assert [msg[2] for msg in msgs] == [0]
|
||||
|
||||
def test_non_volt_cc_redneck_spam_stays_on_powertrain_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_BOLT_CC_2018_2021][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_BOLT_CC_2018_2021,
|
||||
flags=GMFlags.CC_LONG.value,
|
||||
minEnableSpeed=24 * CV.MPH_TO_MS,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=25.0,
|
||||
cruiseState=SimpleNamespace(speed=20.0),
|
||||
),
|
||||
)
|
||||
actuators = SimpleNamespace(accel=1.0)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False))
|
||||
|
||||
assert [msg[2] for msg in msgs] == [0]
|
||||
|
||||
@@ -7,13 +7,15 @@ import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_modul
|
||||
from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
|
||||
|
||||
|
||||
def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False):
|
||||
def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False,
|
||||
lead_status: bool = False, lead_d_rel: float = float("inf")):
|
||||
planner = SimpleNamespace(
|
||||
params=None,
|
||||
params_memory=None,
|
||||
model_length=model_length,
|
||||
model_stopped=model_stopped,
|
||||
tracking_lead=tracking_lead,
|
||||
lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel),
|
||||
)
|
||||
return ConditionalExperimentalMode(planner)
|
||||
|
||||
@@ -24,6 +26,12 @@ def make_sm(traffic_mode_enabled: bool = False):
|
||||
}
|
||||
|
||||
|
||||
def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = False):
|
||||
for _ in range(steps):
|
||||
cem.starpilot_planner.tracking_lead = tracking_lead
|
||||
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
|
||||
|
||||
|
||||
def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped():
|
||||
v_ego = 10 * CV.MPH_TO_MS
|
||||
model_length = v_ego * 10.0
|
||||
@@ -39,7 +47,41 @@ def test_predicted_stop_within_threshold_triggers_stop_light():
|
||||
model_length = v_ego * 4.0
|
||||
|
||||
cem = make_cem(model_length=model_length)
|
||||
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
|
||||
run_stop_light_detector(cem, v_ego, steps=20)
|
||||
|
||||
assert cem.stop_light_detected
|
||||
|
||||
|
||||
def test_chattering_lead_does_not_trigger_stop_light():
|
||||
v_ego = 22 * CV.MPH_TO_MS
|
||||
model_length = v_ego * 4.0
|
||||
|
||||
cem = make_cem(model_length=model_length)
|
||||
for i in range(30):
|
||||
cem.starpilot_planner.tracking_lead = (i % 2 == 0)
|
||||
cem.starpilot_planner.lead_one.status = (i % 2 == 0)
|
||||
cem.starpilot_planner.lead_one.dRel = model_length + 5.0
|
||||
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
|
||||
|
||||
assert not cem.stop_light_detected
|
||||
|
||||
|
||||
def test_close_visible_but_untracked_lead_blocks_stop_light():
|
||||
v_ego = 22 * CV.MPH_TO_MS
|
||||
model_length = v_ego * 4.0
|
||||
|
||||
cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=model_length + 5.0)
|
||||
run_stop_light_detector(cem, v_ego, steps=30)
|
||||
|
||||
assert not cem.stop_light_detected
|
||||
|
||||
|
||||
def test_far_visible_lead_does_not_block_stop_light():
|
||||
v_ego = 22 * CV.MPH_TO_MS
|
||||
model_length = v_ego * 4.0
|
||||
|
||||
cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=v_ego * 7.0 + 30.0)
|
||||
run_stop_light_detector(cem, v_ego, steps=30)
|
||||
|
||||
assert cem.stop_light_detected
|
||||
|
||||
@@ -110,6 +152,7 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
|
||||
planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0),
|
||||
}
|
||||
|
||||
planner.tracking_lead_filter.x = 1.0
|
||||
planner.update(0.0, False, sm, starpilot_toggles)
|
||||
|
||||
assert seen == {
|
||||
|
||||
@@ -21,9 +21,10 @@ MIN_DRAW_DISTANCE = 10.0
|
||||
MAX_DRAW_DISTANCE = 100.0
|
||||
RAINBOW_GRADIENT_COLOR_COUNT = 19
|
||||
RAINBOW_SCROLL_SPEED_DEG_PER_SEC = 60.0
|
||||
ACCEL_PATH_MIN_LIGHTNESS = 0.78
|
||||
ACCEL_PATH_MIN_SATURATION = 0.50
|
||||
STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255)
|
||||
DEFAULT_LANE_LINES_WIDTH = 4.0
|
||||
DEFAULT_PATH_WIDTH = 6.1
|
||||
DEFAULT_ROAD_EDGES_WIDTH = 2.0
|
||||
LANE_LINE_COLORS = {
|
||||
UIStatus.DISENGAGED: rl.Color(200, 200, 200, 255),
|
||||
UIStatus.OVERRIDE: rl.Color(255, 255, 255, 255),
|
||||
@@ -199,14 +200,13 @@ class ModelRenderer(Widget):
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
model_ui_enabled = self._params.get_bool("ModelUI", default=True)
|
||||
if model_ui_enabled:
|
||||
path_width = self._path_width_to_half_m(self._params.get_float("PathWidth", return_default=True, default=6.1))
|
||||
lane_line_width = self._small_distance_to_half_m(self._params.get_float("LaneLinesWidth", return_default=True, default=4.0))
|
||||
road_edge_width = self._small_distance_to_half_m(self._params.get_float("RoadEdgesWidth", return_default=True, default=2.0))
|
||||
else:
|
||||
path_width = 0.9
|
||||
lane_line_width = 0.025
|
||||
road_edge_width = 0.025
|
||||
custom_path_width = model_ui_enabled and self._param_float_changed("PathWidth", DEFAULT_PATH_WIDTH)
|
||||
custom_lane_line_width = model_ui_enabled and self._param_float_changed("LaneLinesWidth", DEFAULT_LANE_LINES_WIDTH)
|
||||
custom_road_edge_width = model_ui_enabled and self._param_float_changed("RoadEdgesWidth", DEFAULT_ROAD_EDGES_WIDTH)
|
||||
|
||||
path_width = self._path_width_to_half_m(self._params.get_float("PathWidth", default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9
|
||||
lane_line_width = self._small_distance_to_half_m(self._params.get_float("LaneLinesWidth", default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else None
|
||||
road_edge_width = self._small_distance_to_half_m(self._params.get_float("RoadEdgesWidth", default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else None
|
||||
|
||||
if model_ui_enabled and self._params.get_bool("DynamicPathWidth", default=False):
|
||||
if ui_state.status == UIStatus.ENGAGED:
|
||||
@@ -220,14 +220,19 @@ class ModelRenderer(Widget):
|
||||
max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance)
|
||||
|
||||
# Update lane lines using raw points
|
||||
line_width_factor = 0.12
|
||||
for i, lane_line in enumerate(self._lane_lines):
|
||||
if i in (1, 2):
|
||||
line_width_factor = 0.16
|
||||
line_width = lane_line_width if lane_line_width is not None else line_width_factor
|
||||
lane_line.projected_points = self._map_line_to_polygon(
|
||||
lane_line.raw_points, lane_line_width * self._lane_line_probs[i], 0.0, max_idx
|
||||
lane_line.raw_points, line_width * self._lane_line_probs[i], 0.0, max_idx
|
||||
)
|
||||
|
||||
# Update road edges using raw points
|
||||
edge_width = road_edge_width if road_edge_width is not None else line_width_factor
|
||||
for road_edge in self._road_edges:
|
||||
road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, road_edge_width, 0.0, max_idx)
|
||||
road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, edge_width, 0.0, max_idx)
|
||||
|
||||
# Update path using raw points
|
||||
if lead and lead.status:
|
||||
@@ -255,17 +260,15 @@ class ModelRenderer(Widget):
|
||||
def _update_experimental_gradient(self):
|
||||
"""Pre-calculate experimental mode gradient colors"""
|
||||
use_rainbow = self._params.get_bool("RainbowPath", default=False)
|
||||
use_acceleration = not use_rainbow and (self._experimental_mode or self._params.get_bool("AccelerationPath", default=True))
|
||||
|
||||
if not (use_rainbow or use_acceleration):
|
||||
return
|
||||
|
||||
max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x))
|
||||
if use_rainbow:
|
||||
gradient_bottom, gradient_top = self._get_visible_gradient_bounds()
|
||||
self._exp_gradient = self._build_rainbow_gradient(gradient_bottom, gradient_top)
|
||||
return
|
||||
|
||||
if not self._experimental_mode or not self._params.get_bool("AccelerationPath", default=True):
|
||||
return
|
||||
|
||||
max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x))
|
||||
segment_colors = []
|
||||
gradient_stops = []
|
||||
|
||||
@@ -282,9 +285,9 @@ class ModelRenderer(Widget):
|
||||
|
||||
# speed up: 120, slow down: 0
|
||||
path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120)
|
||||
accel_magnitude = np.clip(abs(self._acceleration_x[i]) * 1.5, 0.0, 1.0)
|
||||
saturation = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_SATURATION, 1.0])
|
||||
lightness = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_LIGHTNESS, 0.62])
|
||||
|
||||
saturation = min(abs(self._acceleration_x[i] * 1.5), 1)
|
||||
lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62])
|
||||
alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0])
|
||||
|
||||
# Use HSL to RGB conversion
|
||||
@@ -361,12 +364,13 @@ class ModelRenderer(Widget):
|
||||
def _get_ll_color(self, prob: float, adjacent: bool, left: bool):
|
||||
alpha = np.clip(prob, 0.0, 0.7)
|
||||
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
|
||||
if adjacent:
|
||||
override = get_param_color(self._params, "PathEdgesColor", 255)
|
||||
if override is not None:
|
||||
color = with_alpha(override, int(alpha * override.a))
|
||||
elif stock_scheme:
|
||||
base_color = LANE_LINE_COLORS.get(ui_state.status, LANE_LINE_COLORS[UIStatus.DISENGAGED])
|
||||
base_color = LANE_LINE_COLORS.get(line_status, LANE_LINE_COLORS[UIStatus.DISENGAGED])
|
||||
color = rl.Color(base_color.r, base_color.g, base_color.b, int(alpha * 255))
|
||||
else:
|
||||
base_color = get_theme_color("PathEdge", rl.Color(0, 255, 64, 255))
|
||||
@@ -391,7 +395,7 @@ class ModelRenderer(Widget):
|
||||
lane_lines_color = get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR)
|
||||
color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a))
|
||||
|
||||
if stock_scheme and ui_state.status == UIStatus.DISENGAGED:
|
||||
if stock_scheme and ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active:
|
||||
color = rl.Color(0, 0, 0, int(alpha * 255))
|
||||
|
||||
return color
|
||||
@@ -424,17 +428,36 @@ class ModelRenderer(Widget):
|
||||
self._blend_filter.update(int(allow_throttle))
|
||||
use_rainbow = self._params.get_bool("RainbowPath", default=False)
|
||||
use_accel_path = not use_rainbow and self._params.get_bool("AccelerationPath", default=True)
|
||||
path_override = get_param_color(self._params, "PathColor", 255)
|
||||
custom_theme_selected = (self._params.get("ColorScheme", encoding="utf-8", default="stock") or "stock").lower() != "stock"
|
||||
|
||||
if use_rainbow or self._experimental_mode or use_accel_path:
|
||||
# Draw with acceleration coloring
|
||||
if use_rainbow:
|
||||
if len(self._exp_gradient.colors) > 1:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
|
||||
else:
|
||||
fallback = get_border_color(ui_state)
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(fallback.r, fallback.g, fallback.b, 90))
|
||||
elif path_override is not None or custom_theme_selected:
|
||||
elif use_accel_path:
|
||||
if self._experimental_mode:
|
||||
if len(self._exp_gradient.colors) > 1:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
|
||||
else:
|
||||
fallback = get_border_color(ui_state)
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(fallback.r, fallback.g, fallback.b, 90))
|
||||
else:
|
||||
blend_factor = round(self._blend_filter.x * 100) / 100
|
||||
blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor)
|
||||
if lateral_ui_active and blend_factor < 1.0:
|
||||
blended_colors = self._blend_colors(blended_colors, THROTTLE_COLORS, 0.65)
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0),
|
||||
end=(0.0, 0.0),
|
||||
colors=blended_colors,
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
if ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active:
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90))
|
||||
else:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
else:
|
||||
path_color = get_visual_color(self._params, "PathColor", "Path", rl.Color(48, 255, 156, 255))
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0),
|
||||
@@ -447,19 +470,6 @@ class ModelRenderer(Widget):
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
else:
|
||||
# Blend throttle/no throttle colors based on transition
|
||||
blend_factor = round(self._blend_filter.x * 100) / 100
|
||||
blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor)
|
||||
if lateral_ui_active and blend_factor < 1.0:
|
||||
blended_colors = self._blend_colors(blended_colors, THROTTLE_COLORS, 0.65)
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0), # Bottom of path
|
||||
end=(0.0, 0.0), # Top of path
|
||||
colors=blended_colors,
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
|
||||
def _draw_lead_indicator(self):
|
||||
# Draw lead vehicles if available
|
||||
@@ -599,3 +609,12 @@ class ModelRenderer(Widget):
|
||||
if self._params.get_bool("IsMetric"):
|
||||
return value / 2.0
|
||||
return value * CV.FOOT_TO_METER / 2.0
|
||||
|
||||
def _param_float_changed(self, key: str, default: float) -> bool:
|
||||
value = self._params.get(key, encoding="utf-8")
|
||||
if value in (None, ""):
|
||||
return False
|
||||
try:
|
||||
return not np.isclose(float(value), default)
|
||||
except (TypeError, ValueError):
|
||||
return False
|
||||
|
||||
@@ -19,9 +19,11 @@ MIN_DRAW_DISTANCE = 10.0
|
||||
MAX_DRAW_DISTANCE = 100.0
|
||||
RAINBOW_GRADIENT_COLOR_COUNT = 19
|
||||
RAINBOW_SCROLL_SPEED_DEG_PER_SEC = 60.0
|
||||
ACCEL_PATH_MIN_LIGHTNESS = 0.78
|
||||
ACCEL_PATH_MIN_SATURATION = 0.50
|
||||
STOCK_LANE_LINES_COLOR = rl.Color(255, 255, 255, 255)
|
||||
DEFAULT_LANE_LINES_WIDTH = 4.0
|
||||
DEFAULT_PATH_EDGE_WIDTH = 20.0
|
||||
DEFAULT_PATH_WIDTH = 6.1
|
||||
DEFAULT_ROAD_EDGES_WIDTH = 2.0
|
||||
|
||||
THROTTLE_COLORS = [
|
||||
rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4)
|
||||
@@ -35,7 +37,6 @@ NO_THROTTLE_COLORS = [
|
||||
rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0)
|
||||
]
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelPoints:
|
||||
raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
|
||||
@@ -189,16 +190,15 @@ class ModelRenderer(Widget):
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
model_ui_enabled = self._params.get_bool('ModelUI', default=True)
|
||||
if model_ui_enabled:
|
||||
path_width = self._path_width_to_half_m(self._params.get_float('PathWidth', return_default=True, default=6.1))
|
||||
lane_line_width_m = self._small_distance_to_half_m(self._params.get_float('LaneLinesWidth', return_default=True, default=4.0))
|
||||
road_edge_width_m = self._small_distance_to_half_m(self._params.get_float('RoadEdgesWidth', return_default=True, default=2.0))
|
||||
path_edge_width_pct = np.clip(self._params.get_float('PathEdgeWidth', return_default=True, default=20.0) / 100.0, 0.0, 1.0)
|
||||
else:
|
||||
path_width = 0.9
|
||||
lane_line_width_m = 0.025
|
||||
road_edge_width_m = 0.025
|
||||
path_edge_width_pct = 0.0
|
||||
custom_path_width = model_ui_enabled and self._param_float_changed('PathWidth', DEFAULT_PATH_WIDTH)
|
||||
custom_lane_line_width = model_ui_enabled and self._param_float_changed('LaneLinesWidth', DEFAULT_LANE_LINES_WIDTH)
|
||||
custom_road_edge_width = model_ui_enabled and self._param_float_changed('RoadEdgesWidth', DEFAULT_ROAD_EDGES_WIDTH)
|
||||
custom_path_edge_width = model_ui_enabled and self._param_float_changed('PathEdgeWidth', DEFAULT_PATH_EDGE_WIDTH)
|
||||
|
||||
path_width = self._path_width_to_half_m(self._params.get_float('PathWidth', default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9
|
||||
lane_line_width_m = self._small_distance_to_half_m(self._params.get_float('LaneLinesWidth', default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else 0.025
|
||||
road_edge_width_m = self._small_distance_to_half_m(self._params.get_float('RoadEdgesWidth', default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else 0.025
|
||||
path_edge_width_pct = np.clip(self._params.get_float('PathEdgeWidth', default=DEFAULT_PATH_EDGE_WIDTH) / 100.0, 0.0, 1.0) if custom_path_edge_width else 0.0
|
||||
|
||||
# Dynamic path width
|
||||
if model_ui_enabled and self._params.get_bool('DynamicPathWidth', default=False):
|
||||
@@ -246,17 +246,15 @@ class ModelRenderer(Widget):
|
||||
def _update_experimental_gradient(self):
|
||||
"""Pre-calculate experimental mode gradient colors"""
|
||||
use_rainbow = self._params.get_bool('RainbowPath', default=False)
|
||||
use_acceleration = not use_rainbow and (self._experimental_mode or self._params.get_bool('AccelerationPath', default=True))
|
||||
|
||||
if not use_acceleration and not use_rainbow:
|
||||
return
|
||||
|
||||
max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x))
|
||||
if use_rainbow:
|
||||
gradient_bottom, gradient_top = self._get_visible_gradient_bounds()
|
||||
self._exp_gradient = self._build_rainbow_gradient(gradient_bottom, gradient_top)
|
||||
return
|
||||
|
||||
if not self._experimental_mode or not self._params.get_bool('AccelerationPath', default=True):
|
||||
return
|
||||
|
||||
max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x))
|
||||
segment_colors = []
|
||||
gradient_stops = []
|
||||
|
||||
@@ -273,9 +271,9 @@ class ModelRenderer(Widget):
|
||||
|
||||
# speed up: 120, slow down: 0
|
||||
path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120)
|
||||
accel_magnitude = np.clip(abs(self._acceleration_x[i]) * 1.5, 0.0, 1.0)
|
||||
saturation = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_SATURATION, 1.0])
|
||||
lightness = np.interp(accel_magnitude, [0.0, 1.0], [ACCEL_PATH_MIN_LIGHTNESS, 0.62])
|
||||
|
||||
saturation = min(abs(self._acceleration_x[i] * 1.5), 1)
|
||||
lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62])
|
||||
alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0])
|
||||
|
||||
# Use HSL to RGB conversion
|
||||
@@ -387,14 +385,28 @@ class ModelRenderer(Widget):
|
||||
|
||||
use_rainbow = self._params.get_bool('RainbowPath', default=False)
|
||||
use_accel_path = not use_rainbow and self._params.get_bool('AccelerationPath', default=True)
|
||||
path_override = get_param_color(self._params, 'PathColor', 255)
|
||||
custom_theme_selected = (self._params.get('ColorScheme', encoding='utf-8', default='stock') or 'stock').lower() != 'stock'
|
||||
if use_rainbow or self._experimental_mode or use_accel_path:
|
||||
if use_rainbow:
|
||||
if len(self._exp_gradient.colors) > 1:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
|
||||
else:
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30))
|
||||
elif path_override is not None or custom_theme_selected:
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(48, 255, 156, 90))
|
||||
elif use_accel_path:
|
||||
if self._experimental_mode:
|
||||
if len(self._exp_gradient.colors) > 1:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
|
||||
else:
|
||||
draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30))
|
||||
else:
|
||||
blend_factor = round(self._blend_filter.x * 100) / 100
|
||||
blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor)
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0),
|
||||
end=(0.0, 0.0),
|
||||
colors=blended_colors,
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
else:
|
||||
path_color = get_visual_color(self._params, "PathColor", "Path", rl.Color(48, 255, 156, 255))
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0),
|
||||
@@ -407,17 +419,6 @@ class ModelRenderer(Widget):
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
else:
|
||||
# Blend throttle/no throttle colors based on transition
|
||||
blend_factor = round(self._blend_filter.x * 100) / 100
|
||||
blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor)
|
||||
gradient = Gradient(
|
||||
start=(0.0, 1.0), # Bottom of path
|
||||
end=(0.0, 0.0), # Top of path
|
||||
colors=blended_colors,
|
||||
stops=[0.0, 0.5, 1.0],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
|
||||
def _draw_lead_indicator(self):
|
||||
# Draw lead vehicles if available
|
||||
@@ -683,6 +684,15 @@ class ModelRenderer(Widget):
|
||||
return value / 2.0
|
||||
return value * CV.FOOT_TO_METER / 2.0
|
||||
|
||||
def _param_float_changed(self, key: str, default: float) -> bool:
|
||||
value = self._params.get(key, encoding="utf-8")
|
||||
if value in (None, ""):
|
||||
return False
|
||||
try:
|
||||
return not np.isclose(float(value), default)
|
||||
except (TypeError, ValueError):
|
||||
return False
|
||||
|
||||
@staticmethod
|
||||
def _blend_colors(begin_colors, end_colors, t):
|
||||
if t >= 1.0:
|
||||
|
||||
@@ -38,6 +38,12 @@ class ConditionalExperimentalMode:
|
||||
LIGHT_SPEED_LOW = 50 * CV.MPH_TO_MS # 50 mph threshold
|
||||
LIGHT_SPEED_HIGH = 60 * CV.MPH_TO_MS # 60 mph threshold
|
||||
LIGHT_MAX_TIME = 9 # Balanced max time preserving city performance
|
||||
LOW_SPEED_LIGHT_FILTER_TIME = 0.35
|
||||
LEAD_CLEAR_FILTER_TIME_LOW = 0.6
|
||||
LEAD_CLEAR_FILTER_TIME_HIGH = 0.35
|
||||
STOP_LIGHT_ON_MARGIN = 2.5
|
||||
STOP_LIGHT_OFF_MARGIN = 4.0
|
||||
STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0
|
||||
|
||||
# ===== END TUNING PARAMETERS =====
|
||||
|
||||
@@ -66,11 +72,13 @@ class ConditionalExperimentalMode:
|
||||
self.curvature_filter = FirstOrderFilter(0, self.FILTER_TIME_CURVE, DT_MDL)
|
||||
self.slow_lead_filter = FirstOrderFilter(0, self.FILTER_TIME_LEAD, DT_MDL)
|
||||
self.stop_light_filter = FirstOrderFilter(0, self.FILTER_TIME_LIGHT, DT_MDL)
|
||||
self.lead_clear_filter = FirstOrderFilter(0, self.LEAD_CLEAR_FILTER_TIME_LOW, DT_MDL)
|
||||
|
||||
self.curve_detected = False
|
||||
self.slow_lead_detected = False
|
||||
self.experimental_mode = False
|
||||
self.stop_light_detected = False
|
||||
self.stop_light_model_detected = False
|
||||
self.prev_experimental_mode = False # For hysteresis
|
||||
self.mode_hold_until = 0.0
|
||||
self.mode_false_since = 0.0
|
||||
@@ -177,7 +185,8 @@ class ConditionalExperimentalMode:
|
||||
|
||||
filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves])
|
||||
filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads])
|
||||
filter_time_lights = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_lights])
|
||||
filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights])
|
||||
lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH])
|
||||
light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost])
|
||||
cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor])
|
||||
|
||||
@@ -185,11 +194,14 @@ class ConditionalExperimentalMode:
|
||||
self.curvature_filter = FirstOrderFilter(self.curvature_filter.x, filter_time_curves, DT_MDL)
|
||||
self.slow_lead_filter = FirstOrderFilter(self.slow_lead_filter.x, filter_time_leads, DT_MDL)
|
||||
self.stop_light_filter = FirstOrderFilter(self.stop_light_filter.x, filter_time_lights, DT_MDL)
|
||||
self.lead_clear_filter.update_alpha(lead_clear_filter_time)
|
||||
|
||||
# Disable stoplight detection at very high speeds to prevent false positives
|
||||
if speed_mph > 75: # Disable above 75 mph
|
||||
self.stop_light_filter.x = 0
|
||||
self.stop_light_detected = False
|
||||
self.stop_light_model_detected = False
|
||||
self.lead_clear_filter.x = 0
|
||||
return
|
||||
|
||||
# Adjust model time with interp boost and gradual cap
|
||||
@@ -197,15 +209,30 @@ class ConditionalExperimentalMode:
|
||||
if cap_factor > 0:
|
||||
adjusted_model_time = min(adjusted_model_time, self.LIGHT_MAX_TIME * cap_factor + model_time * (1 - cap_factor)) # Gradual cap
|
||||
|
||||
model_stopping = self.starpilot_planner.model_length < v_ego * adjusted_model_time
|
||||
stop_threshold = max(v_ego * adjusted_model_time, 0.0)
|
||||
if self.stop_light_model_detected:
|
||||
model_stopping = self.starpilot_planner.model_length < stop_threshold + self.STOP_LIGHT_OFF_MARGIN
|
||||
else:
|
||||
model_stopping = self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_ON_MARGIN, 0.0)
|
||||
self.stop_light_model_detected = model_stopping
|
||||
|
||||
# `model_stopped` is a coarse horizon-length check (< 50 m with current constants)
|
||||
# used elsewhere for force-stop/green-light behavior. Reusing it here causes
|
||||
# ordinary low-speed cruising to look like a stop prediction and can latch the
|
||||
# STOP_LIGHT CEM trigger. For the CEM detector, key strictly off the configured
|
||||
# "predicted stop within N seconds" threshold.
|
||||
self.stop_light_filter.update(model_stopping)
|
||||
self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and not self.starpilot_planner.tracking_lead)
|
||||
# Key off relevant raw lead presence, not trackingLead. Vision-only GM can
|
||||
# flap trackingLead around the model-length threshold while leadOne remains
|
||||
# present; far/stale leads should not suppress true stop-light detection.
|
||||
lead = getattr(self.starpilot_planner, "lead_one", None)
|
||||
lead_distance = float(getattr(lead, "dRel", float("inf")))
|
||||
lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN
|
||||
self.lead_clear_filter.update(not lead_relevant)
|
||||
lead_cleared = self.lead_clear_filter.x >= THRESHOLD
|
||||
self.stop_light_filter.update(model_stopping and lead_cleared)
|
||||
self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared)
|
||||
else:
|
||||
self.stop_light_filter.x = 0
|
||||
self.stop_light_detected = False
|
||||
self.stop_light_model_detected = False
|
||||
self.lead_clear_filter.x = 0
|
||||
|
||||
Reference in New Issue
Block a user