mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-09 01:25:11 +08:00
Compare commits
3 Commits
accel-cont
...
feat/relc
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
e1f2f2b43e | ||
|
|
e66840ac5b | ||
|
|
a23cd7dddc |
@@ -1,6 +1,3 @@
|
||||
sunnypilot Version 2026.002.000 (2026-xx-xx)
|
||||
========================
|
||||
|
||||
sunnypilot Version 2026.001.000 (2026-05-06)
|
||||
========================
|
||||
* What's Changed (sunnypilot/sunnypilot)
|
||||
|
||||
@@ -194,13 +194,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelPersonality @8 :AccelerationPersonality;
|
||||
|
||||
enum AccelerationPersonality {
|
||||
sport @0;
|
||||
normal @1;
|
||||
eco @2;
|
||||
}
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -349,6 +342,7 @@ struct OnroadEventSP @0xda96579883444c35 {
|
||||
speedLimitChanged @21;
|
||||
speedLimitPending @22;
|
||||
e2eChime @23;
|
||||
laneChangeRoadEdge @24;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -455,6 +449,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
|
||||
|
||||
struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||
laneTurnDirection @0 :TurnDirection;
|
||||
leftLaneChangeEdgeBlock @1 :Bool;
|
||||
rightLaneChangeEdgeBlock @2 :Bool;
|
||||
|
||||
enum TurnDirection {
|
||||
none @0;
|
||||
|
||||
@@ -2054,14 +2054,16 @@ struct DriverStateV2 {
|
||||
facePosition @2 :List(Float32);
|
||||
facePositionStd @3 :List(Float32);
|
||||
faceProb @4 :Float32;
|
||||
leftEyeProb @5 :Float32;
|
||||
rightEyeProb @6 :Float32;
|
||||
leftBlinkProb @7 :Float32;
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
eyesVisibleProb @14 :Float32;
|
||||
eyesClosedProb @15 :Float32;
|
||||
phoneProb @13 :Float32;
|
||||
|
||||
deprecated :group {
|
||||
leftEyeProb @5 :Float32;
|
||||
rightEyeProb @6 :Float32;
|
||||
leftBlinkProb @7 :Float32;
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
notReadyProb @12 :List(Float32);
|
||||
occludedProb @10 :Float32;
|
||||
readyProb @11 :List(Float32);
|
||||
|
||||
@@ -135,8 +135,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"Version", {PERSISTENT, STRING}},
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
@@ -180,6 +178,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
Binary file not shown.
@@ -56,7 +56,7 @@ class DesireHelper:
|
||||
def get_lane_change_direction(CS):
|
||||
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob):
|
||||
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
|
||||
self.alc.update_params()
|
||||
self.lane_turn_controller.update_params()
|
||||
v_ego = carstate.vEgo
|
||||
@@ -88,8 +88,8 @@ class DesireHelper:
|
||||
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
||||
|
||||
|
||||
@@ -313,14 +313,11 @@ class LongitudinalMpc:
|
||||
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
|
||||
return lead_xv
|
||||
|
||||
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, a_cruise_min=None):
|
||||
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
v_ego = self.x0[1]
|
||||
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
|
||||
|
||||
if a_cruise_min is None:
|
||||
a_cruise_min = CRUISE_MIN_ACCEL
|
||||
|
||||
lead_xv_0 = self.process_lead(radarstate.leadOne)
|
||||
lead_xv_1 = self.process_lead(radarstate.leadTwo)
|
||||
|
||||
@@ -332,7 +329,7 @@ class LongitudinalMpc:
|
||||
|
||||
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
|
||||
# when the leads are no factor.
|
||||
v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
|
||||
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
|
||||
# TODO does this make sense when max_a is negative?
|
||||
v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
|
||||
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
|
||||
|
||||
@@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
accel_clip = self.get_accel_clip(v_ego) or [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
|
||||
|
||||
@@ -138,8 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
|
||||
a_cruise_min=self.get_cruise_min_accel(v_ego))
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
|
||||
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
|
||||
@@ -83,7 +83,7 @@ def parse_model_output(model_output):
|
||||
face_descs = model_output[f'face_descs_{ds_suffix}']
|
||||
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
return parsed
|
||||
|
||||
@@ -93,11 +93,8 @@ def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
|
||||
msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
|
||||
msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.eyesVisibleProb = model_output[f'eyes_visible_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.eyesClosedProb = model_output[f'eyes_closed_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
|
||||
Binary file not shown.
@@ -32,9 +32,8 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
|
||||
self._FACE_THRESHOLD = 0.7
|
||||
self._EYE_THRESHOLD = 0.65
|
||||
self._SG_THRESHOLD = 0.9
|
||||
self._BLINK_THRESHOLD = 0.865
|
||||
self._EYE_THRESHOLD = 0.5
|
||||
self._BLINK_THRESHOLD = 0.5
|
||||
self._PHONE_THRESH = 0.5
|
||||
|
||||
self._POSE_PITCH_THRESHOLD = 0.3133
|
||||
@@ -111,11 +110,6 @@ class DriverProb:
|
||||
self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
|
||||
self.prob_calibrated = False
|
||||
|
||||
class DriverBlink:
|
||||
def __init__(self):
|
||||
self.left = 0.
|
||||
self.right = 0.
|
||||
|
||||
|
||||
# model output refers to center of undistorted+leveled image
|
||||
EFL = 598.0 # focal length in K
|
||||
@@ -150,7 +144,7 @@ class DriverMonitoring:
|
||||
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
|
||||
self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
|
||||
self.pose = DriverPose(settings=self.settings)
|
||||
self.blink = DriverBlink()
|
||||
self.blink_prob = 0.
|
||||
self.phone_prob = 0.
|
||||
|
||||
self.always_on = always_on
|
||||
@@ -253,7 +247,7 @@ class DriverMonitoring:
|
||||
if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
|
||||
distracted_types.append(DistractedType.DISTRACTED_POSE)
|
||||
|
||||
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
|
||||
if self.blink_prob > self.settings._BLINK_THRESHOLD:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.phone_prob > self.settings._PHONE_THRESH:
|
||||
@@ -294,10 +288,7 @@ class DriverMonitoring:
|
||||
self.pose.yaw_std = driver_data.faceOrientationStd[1]
|
||||
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
|
||||
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
|
||||
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
|
||||
self.phone_prob = driver_data.phoneProb
|
||||
|
||||
self.distracted_types = self._get_distracted_types()
|
||||
|
||||
@@ -20,10 +20,8 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
|
||||
ds.leftDriverData.faceOrientation = [0., 0., 0.]
|
||||
ds.leftDriverData.facePosition = [0., 0.]
|
||||
ds.leftDriverData.faceProb = 1. * face_detected
|
||||
ds.leftDriverData.leftEyeProb = 1.
|
||||
ds.leftDriverData.rightEyeProb = 1.
|
||||
ds.leftDriverData.leftBlinkProb = 1. * distracted
|
||||
ds.leftDriverData.rightBlinkProb = 1. * distracted
|
||||
ds.leftDriverData.eyesVisibleProb = 1.
|
||||
ds.leftDriverData.eyesClosedProb = 1. * distracted
|
||||
ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
|
||||
ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
|
||||
# TODO: test both separately when e2e is used
|
||||
|
||||
@@ -298,9 +298,16 @@ class SelfdriveD(CruiseHelper):
|
||||
# Handle lane change
|
||||
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
|
||||
direction = self.sm['modelV2'].meta.laneChangeDirection
|
||||
mdv2sp = self.sm['modelDataV2SP']
|
||||
|
||||
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
|
||||
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
||||
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
||||
self.events.add(EventName.laneChangeBlocked)
|
||||
|
||||
elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
|
||||
(mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
|
||||
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
|
||||
|
||||
else:
|
||||
if direction == LaneChangeDirection.left:
|
||||
self.events.add(EventName.preLaneChangeLeft)
|
||||
|
||||
@@ -76,7 +76,7 @@ def generate_report(proposed, master, tmp, commit):
|
||||
(lambda x: get_idx_if_non_empty(x.wheelOnRightProb), "wheelOnRightProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceProb), "leftDriverData.faceProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceOrientation, 0), "leftDriverData.faceOrientation0"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.leftBlinkProb), "leftDriverData.leftBlinkProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.eyesClosedProb), "leftDriverData.eyesClosedProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.phoneProb), "leftDriverData.phoneProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.rightDriverData.faceProb), "rightDriverData.faceProb"),
|
||||
], "driverStateV2")
|
||||
|
||||
@@ -39,8 +39,6 @@ class BaseDriverCameraDialog(Widget):
|
||||
self._eye_fill_texture = None
|
||||
self._eye_orange_texture = None
|
||||
self._eye_size = 74
|
||||
self._glasses_texture = None
|
||||
self._glasses_size = 171
|
||||
|
||||
self._load_eye_textures()
|
||||
|
||||
@@ -154,8 +152,6 @@ class BaseDriverCameraDialog(Widget):
|
||||
self._eye_fill_texture = gui_app.texture("icons_mici/onroad/eye_fill.png", self._eye_size, self._eye_size)
|
||||
if self._eye_orange_texture is None:
|
||||
self._eye_orange_texture = gui_app.texture("icons_mici/onroad/eye_orange.png", self._eye_size, self._eye_size)
|
||||
if self._glasses_texture is None:
|
||||
self._glasses_texture = gui_app.texture("icons_mici/onroad/glasses.png", self._glasses_size, self._glasses_size)
|
||||
|
||||
def _draw_face_detection(self, rect: rl.Rectangle):
|
||||
dm_state = ui_state.sm["driverMonitoringState"]
|
||||
@@ -202,31 +198,21 @@ class BaseDriverCameraDialog(Widget):
|
||||
eye_offset_x = 10
|
||||
eye_offset_y = 10
|
||||
eye_spacing = self._eye_size + 15
|
||||
eyes_prob = driver_data.eyesVisibleProb
|
||||
|
||||
left_eye_x = rect.x + eye_offset_x
|
||||
left_eye_y = rect.y + eye_offset_y
|
||||
left_eye_prob = driver_data.leftEyeProb
|
||||
|
||||
right_eye_x = rect.x + eye_offset_x + eye_spacing
|
||||
right_eye_y = rect.y + eye_offset_y
|
||||
right_eye_prob = driver_data.rightEyeProb
|
||||
|
||||
# Draw eyes with opacity based on probability
|
||||
for eye_x, eye_y, eye_prob in [(left_eye_x, left_eye_y, left_eye_prob), (right_eye_x, right_eye_y, right_eye_prob)]:
|
||||
fill_opacity = eye_prob
|
||||
orange_opacity = 1.0 - eye_prob
|
||||
|
||||
fill_opacity = eyes_prob
|
||||
orange_opacity = 1.0 - eyes_prob
|
||||
for eye_x, eye_y in [(left_eye_x, left_eye_y), (right_eye_x, right_eye_y)]:
|
||||
rl.draw_texture_v(self._eye_orange_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * orange_opacity)))
|
||||
rl.draw_texture_v(self._eye_fill_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * fill_opacity)))
|
||||
|
||||
# Draw sunglasses indicator based on sunglasses probability
|
||||
# Position glasses centered between the two eyes at top left
|
||||
glasses_x = rect.x + eye_offset_x - 4
|
||||
glasses_y = rect.y
|
||||
glasses_pos = rl.Vector2(glasses_x, glasses_y)
|
||||
glasses_prob = driver_data.sunglassesProb
|
||||
rl.draw_texture_v(self._glasses_texture, glasses_pos, rl.Color(70, 80, 161, int(255 * glasses_prob)))
|
||||
|
||||
|
||||
class DriverCameraDialog(NavWidget, BaseDriverCameraDialog):
|
||||
def __init__(self):
|
||||
|
||||
@@ -51,11 +51,17 @@ class LaneChangeSettingsLayout(Widget):
|
||||
description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
|
||||
"(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
|
||||
)
|
||||
self._road_edge_block = toggle_item_sp(
|
||||
param="RoadEdgeLaneChangeEnabled",
|
||||
title=lambda: tr("Block Lane Change: Road Edge Detection"),
|
||||
description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
|
||||
)
|
||||
|
||||
items = [
|
||||
self._lane_change_timer,
|
||||
LineSeparatorSP(40),
|
||||
self._bsm_delay,
|
||||
self._road_edge_block,
|
||||
]
|
||||
|
||||
return items
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define SUNNYPILOT_VERSION "2026.002.000"
|
||||
#define SUNNYPILOT_VERSION "2026.001.000"
|
||||
|
||||
@@ -34,6 +34,7 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
|
||||
|
||||
@@ -246,6 +247,9 @@ def main(demo=False):
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController(DH)
|
||||
|
||||
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -349,7 +353,10 @@ def main(demo=False):
|
||||
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
|
||||
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
|
||||
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
|
||||
@@ -1,156 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from cereal import custom
|
||||
import numpy as np
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
|
||||
|
||||
A_MAX_BP = [0.0, 4.0, 8.0, 16.0, 40.0]
|
||||
A_MAX_V = {
|
||||
AccelPersonality.eco: [1.20, 1.40, 1.20, 0.40, 0.08],
|
||||
AccelPersonality.normal: [1.80, 1.80, 1.35, 0.50, 0.15],
|
||||
AccelPersonality.sport: [2.20, 2.20, 1.60, 0.70, 0.25],
|
||||
}
|
||||
|
||||
COAST_DRAG_BP = [0.0, 10.0, 25.0, 40.0]
|
||||
COAST_DRAG_V = {
|
||||
AccelPersonality.eco: [-0.03, -0.05, -0.08, -0.12],
|
||||
AccelPersonality.normal: [-0.04, -0.07, -0.12, -0.18],
|
||||
AccelPersonality.sport: [-0.06, -0.10, -0.18, -0.28],
|
||||
}
|
||||
|
||||
A_MIN_FLOOR_BP = [0.0, 5.0, 15.0, 40.0]
|
||||
A_MIN_FLOOR_V = {
|
||||
AccelPersonality.eco: [-0.20, -0.35, -0.55, -0.50],
|
||||
AccelPersonality.normal: [-0.25, -0.45, -0.75, -0.65],
|
||||
AccelPersonality.sport: [-0.35, -0.65, -1.00, -0.95],
|
||||
}
|
||||
|
||||
DEFICIT_TO_FLOOR = 8.5
|
||||
COAST_DEADBAND = 1.0
|
||||
RAMP_OFF_RANGE = 5.0
|
||||
|
||||
A_MIN_TIGHTEN_RATE = 0.6
|
||||
A_MIN_RELAX_RATE = 0.9
|
||||
A_MAX_RATE_UP = 1.5
|
||||
A_MAX_RATE_DOWN = 0.6
|
||||
|
||||
MIN_MAX_GAP = 0.05
|
||||
|
||||
PARAM_REFRESH_FRAMES = max(1, int(1.0 / DT_MDL))
|
||||
|
||||
|
||||
class AccelPersonalityController:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = 0
|
||||
self._first = True
|
||||
|
||||
val = self.params.get('AccelPersonality')
|
||||
self._personality = val if val is not None else AccelPersonality.normal
|
||||
self._enabled = self.params.get_bool('AccelPersonalityEnabled')
|
||||
|
||||
self._v_cruise = 0.0
|
||||
self._a_min = -0.05
|
||||
self._a_max = 1.50
|
||||
|
||||
self._cache_v: float | None = None
|
||||
self._cache_v_cruise: float | None = None
|
||||
self._cache_a_min = self._a_min
|
||||
self._cache_a_max = self._a_max
|
||||
|
||||
def update(self, sm=None):
|
||||
self.frame += 1
|
||||
self._cache_v = None
|
||||
self._cache_v_cruise = None
|
||||
|
||||
if sm is not None:
|
||||
vc = sm['carState'].vCruise
|
||||
self._v_cruise = float(vc) * (1000.0 / 3600.0) if vc != V_CRUISE_UNSET else 0.0
|
||||
|
||||
if self.frame % PARAM_REFRESH_FRAMES == 0:
|
||||
val = self.params.get('AccelPersonality')
|
||||
self._personality = val if val is not None else AccelPersonality.normal
|
||||
new_enabled = self.params.get_bool('AccelPersonalityEnabled')
|
||||
if new_enabled and not self._enabled:
|
||||
self._first = True
|
||||
self._enabled = new_enabled
|
||||
|
||||
def get_accel_personality(self) -> int:
|
||||
return int(self._personality)
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
|
||||
v_ego = max(0.0, v_ego)
|
||||
if (self._cache_v is not None
|
||||
and abs(self._cache_v - v_ego) < 0.01
|
||||
and self._cache_v_cruise == self._v_cruise):
|
||||
return self._cache_a_min, self._cache_a_max
|
||||
self._cache_a_min, self._cache_a_max = self._step(v_ego)
|
||||
self._cache_v = v_ego
|
||||
self._cache_v_cruise = self._v_cruise
|
||||
return self._cache_a_min, self._cache_a_max
|
||||
|
||||
def get_min_accel(self, v_ego: float) -> float:
|
||||
return self.get_accel_limits(v_ego)[0]
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
return self.get_accel_limits(v_ego)[1]
|
||||
|
||||
def _ramp_off(self, v_ego: float) -> float:
|
||||
if self._v_cruise <= 0.0:
|
||||
return 1.0
|
||||
return float(np.clip((self._v_cruise - v_ego) / RAMP_OFF_RANGE, 0.0, 1.0))
|
||||
|
||||
def _target_max(self, v_ego: float) -> float:
|
||||
base = float(np.interp(v_ego, A_MAX_BP, A_MAX_V[self._personality]))
|
||||
return base * self._ramp_off(v_ego)
|
||||
|
||||
def _target_min(self, v_ego: float) -> float:
|
||||
coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
|
||||
if self._v_cruise <= 0.0 or v_ego >= self._v_cruise:
|
||||
return coast
|
||||
floor = float(np.interp(v_ego, A_MIN_FLOOR_BP, A_MIN_FLOOR_V[self._personality]))
|
||||
deficit = self._v_cruise - v_ego
|
||||
t = float(np.clip(deficit / DEFICIT_TO_FLOOR, 0.0, 1.0)) ** 1.5
|
||||
return coast + t * (floor - coast)
|
||||
|
||||
def _apply_coast_deadband(self, v_ego: float, t_min: float, t_max: float) -> tuple[float, float]:
|
||||
if self._v_cruise <= 0.0 or abs(v_ego - self._v_cruise) >= COAST_DEADBAND:
|
||||
return t_min, t_max
|
||||
coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
|
||||
return coast, max(0.05, t_max * 0.25)
|
||||
|
||||
def _rate_limit(self, last: float, target: float, rate_down: float, rate_up: float) -> float:
|
||||
rate = rate_up if target > last else rate_down
|
||||
step = rate * DT_MDL
|
||||
return float(np.clip(target, last - step, last + step))
|
||||
|
||||
def _step(self, v_ego: float) -> tuple[float, float]:
|
||||
t_max = self._target_max(v_ego)
|
||||
t_min = self._target_min(v_ego)
|
||||
t_min, t_max = self._apply_coast_deadband(v_ego, t_min, t_max)
|
||||
|
||||
if self._first:
|
||||
self._a_min, self._a_max = t_min, t_max
|
||||
self._first = False
|
||||
return self._a_min, self._a_max
|
||||
|
||||
new_min = self._rate_limit(self._a_min, t_min, rate_down=A_MIN_TIGHTEN_RATE, rate_up=A_MIN_RELAX_RATE)
|
||||
new_max = self._rate_limit(self._a_max, t_max, rate_down=A_MAX_RATE_DOWN, rate_up=A_MAX_RATE_UP)
|
||||
|
||||
new_min = min(new_min, new_max - MIN_MAX_GAP)
|
||||
|
||||
self._a_min, self._a_max = new_min, new_max
|
||||
return self._a_min, self._a_max
|
||||
@@ -17,9 +17,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
|
||||
from opendbc.car.interfaces import ACCEL_MIN
|
||||
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
|
||||
@@ -29,7 +26,6 @@ class LongitudinalPlannerSP:
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.accel_controller = AccelPersonalityController()
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.sla = SpeedLimitAssist(CP, CP_SP)
|
||||
@@ -47,17 +43,6 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def get_accel_clip(self, v_ego: float) -> list[float] | None:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
a_max = self.accel_controller.get_max_accel(v_ego)
|
||||
return [ACCEL_MIN, max(ACCEL_MIN, a_max)]
|
||||
|
||||
def get_cruise_min_accel(self, v_ego: float) -> float | None:
|
||||
if self.accel_controller.is_enabled():
|
||||
return self.accel_controller.get_min_accel(v_ego)
|
||||
return None
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -92,7 +77,6 @@ class LongitudinalPlannerSP:
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
self.accel_controller.update(sm)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
@@ -111,8 +95,6 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
longitudinalPlanSP.accelPersonality = int(self.accel_controller.get_accel_personality())
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
# Vision Control
|
||||
|
||||
84
sunnypilot/selfdrive/controls/lib/relc.py
Normal file
84
sunnypilot/selfdrive/controls/lib/relc.py
Normal file
@@ -0,0 +1,84 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
|
||||
NEARSIDE_PROB = 0.2
|
||||
EDGE_PROB = 0.35
|
||||
EDGE_REACTION_TIME = 1.0
|
||||
EDGE_CLEAR_TIME = 0.3
|
||||
MIN_SPEED = 20 * CV.MPH_TO_MS
|
||||
|
||||
|
||||
class RoadEdgeLaneChangeController:
|
||||
def __init__(self, desire_helper):
|
||||
self.DH = desire_helper
|
||||
self.params = Params()
|
||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
||||
self.param_read_counter = 0
|
||||
self.left_edge_detected = False
|
||||
self.right_edge_detected = False
|
||||
self.left_edge_timer = 0.0
|
||||
self.right_edge_timer = 0.0
|
||||
self.left_clear_timer = 0.0
|
||||
self.right_clear_timer = 0.0
|
||||
|
||||
def read_params(self) -> None:
|
||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self.param_read_counter % 50 == 0:
|
||||
self.read_params()
|
||||
self.param_read_counter += 1
|
||||
|
||||
def reset(self) -> None:
|
||||
self.left_edge_detected = False
|
||||
self.right_edge_detected = False
|
||||
self.left_edge_timer = 0.0
|
||||
self.right_edge_timer = 0.0
|
||||
self.left_clear_timer = 0.0
|
||||
self.right_clear_timer = 0.0
|
||||
|
||||
def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
|
||||
self.update_params()
|
||||
|
||||
if not self.enabled or v_ego < MIN_SPEED:
|
||||
self.reset()
|
||||
return
|
||||
|
||||
left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
|
||||
right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
|
||||
left_lane_prob = lane_line_probs[0]
|
||||
right_lane_prob = lane_line_probs[3]
|
||||
|
||||
left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
|
||||
right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
|
||||
|
||||
if left_cond:
|
||||
self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
|
||||
self.left_clear_timer = 0.0
|
||||
if self.left_edge_timer > EDGE_REACTION_TIME:
|
||||
self.left_edge_detected = True
|
||||
else:
|
||||
self.left_clear_timer += DT_MDL
|
||||
if self.left_clear_timer > EDGE_CLEAR_TIME:
|
||||
self.left_edge_timer = 0.0
|
||||
self.left_edge_detected = False
|
||||
|
||||
if right_cond:
|
||||
self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
|
||||
self.right_clear_timer = 0.0
|
||||
if self.right_edge_timer > EDGE_REACTION_TIME:
|
||||
self.right_edge_detected = True
|
||||
else:
|
||||
self.right_clear_timer += DT_MDL
|
||||
if self.right_clear_timer > EDGE_CLEAR_TIME:
|
||||
self.right_edge_timer = 0.0
|
||||
self.right_edge_detected = False
|
||||
@@ -1,147 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
Coverage for AccelPersonalityController:
|
||||
- live param flip via auto-refresh (no Python set_enabled() call needed)
|
||||
- V_CRUISE_UNSET guard
|
||||
- enable-transition snap to fresh target
|
||||
- per-personality accel limit deltas vs stock get_max_accel
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from opendbc.car.interfaces import ACCEL_MIN
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel as stock_get_max_accel
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
|
||||
AccelPersonalityController,
|
||||
PARAM_REFRESH_FRAMES,
|
||||
)
|
||||
|
||||
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
|
||||
|
||||
class FakeCarState:
|
||||
def __init__(self, v_cruise=30.0):
|
||||
self.vCruise = v_cruise
|
||||
|
||||
|
||||
class FakeSM:
|
||||
def __init__(self, v_cruise=30.0):
|
||||
self._data = {'carState': FakeCarState(v_cruise)}
|
||||
|
||||
def __getitem__(self, k):
|
||||
return self._data[k]
|
||||
|
||||
|
||||
def _print_table(title, header, rows):
|
||||
print(f"\n--- {title} ---")
|
||||
print(" | ".join(f"{h:>12}" for h in header))
|
||||
print("-" * (15 * len(header)))
|
||||
for row in rows:
|
||||
print(" | ".join(f"{v:>12.3f}" if isinstance(v, float) else f"{v:>12}" for v in row))
|
||||
|
||||
|
||||
class TestAccelLiveFlip:
|
||||
def test_enable_via_param(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
c = AccelPersonalityController()
|
||||
assert not c.is_enabled()
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert c.is_enabled()
|
||||
|
||||
def test_disable_via_param(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
c = AccelPersonalityController()
|
||||
assert c.is_enabled()
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert not c.is_enabled()
|
||||
|
||||
def test_personality_change_via_param(self):
|
||||
Params().put('AccelPersonality', AccelPersonality.normal)
|
||||
c = AccelPersonalityController()
|
||||
assert c.get_accel_personality() == AccelPersonality.normal
|
||||
Params().put('AccelPersonality', AccelPersonality.sport)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert c.get_accel_personality() == AccelPersonality.sport
|
||||
|
||||
def test_refresh_boundary_below_threshold(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
c = AccelPersonalityController()
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES - 1):
|
||||
c.update(FakeSM())
|
||||
assert not c.is_enabled()
|
||||
|
||||
def test_enable_transition_snaps_to_target(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
Params().put('AccelPersonality', AccelPersonality.sport)
|
||||
c = AccelPersonalityController()
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
c.get_accel_limits(25.0)
|
||||
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
assert not c.is_enabled()
|
||||
|
||||
Params().put('AccelPersonality', AccelPersonality.eco)
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
assert c._first
|
||||
|
||||
def test_vcruise_unset_treated_as_zero(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
c = AccelPersonalityController()
|
||||
c.update(FakeSM(v_cruise=V_CRUISE_UNSET))
|
||||
assert c._v_cruise == 0.0
|
||||
|
||||
|
||||
class TestAccelUsageDiff:
|
||||
def test_accel_clip_per_personality(self, capsys):
|
||||
rows = []
|
||||
speeds = [3.0, 10.0, 20.0, 30.0]
|
||||
personalities = [
|
||||
('eco', AccelPersonality.eco),
|
||||
('normal', AccelPersonality.normal),
|
||||
('sport', AccelPersonality.sport),
|
||||
]
|
||||
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
sm = FakeSM(v_cruise=35.0)
|
||||
|
||||
any_delta = False
|
||||
for label, p in personalities:
|
||||
Params().put('AccelPersonality', p)
|
||||
c = AccelPersonalityController()
|
||||
c.update(sm)
|
||||
for v_ego in speeds:
|
||||
stock_hi = float(stock_get_max_accel(v_ego))
|
||||
c_lo, c_hi = c.get_accel_limits(v_ego)
|
||||
delta_hi = c_hi - stock_hi
|
||||
delta_lo = c_lo - ACCEL_MIN
|
||||
if abs(delta_hi) > 0.01 or abs(delta_lo) > 0.01:
|
||||
any_delta = True
|
||||
rows.append((label, v_ego, stock_hi, c_hi, delta_hi, c_lo, delta_lo))
|
||||
|
||||
with capsys.disabled():
|
||||
_print_table(
|
||||
"AccelPersonalityController: a_max stock vs controller",
|
||||
["personality", "v_ego", "stock_hi", "ctrl_hi", "delta_hi", "ctrl_lo", "delta_lo"],
|
||||
rows,
|
||||
)
|
||||
assert any_delta
|
||||
@@ -5,6 +5,8 @@ from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
|
||||
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
||||
|
||||
@@ -107,7 +109,11 @@ def set_lane_turn_params():
|
||||
])
|
||||
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
|
||||
dh = DesireHelper()
|
||||
relc = RoadEdgeLaneChangeController(dh)
|
||||
relc.enabled = True
|
||||
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
|
||||
for _ in range(10):
|
||||
dh.update(carstate, lateral_active, lane_change_prob)
|
||||
dh.update(carstate, lateral_active, lane_change_prob,
|
||||
left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
|
||||
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
|
||||
|
||||
|
||||
99
sunnypilot/selfdrive/controls/lib/tests/test_relc.py
Normal file
99
sunnypilot/selfdrive/controls/lib/tests/test_relc.py
Normal file
@@ -0,0 +1,99 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import pytest
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
|
||||
RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
|
||||
)
|
||||
|
||||
V_HIGH = MIN_SPEED + 2.0
|
||||
V_LOW = MIN_SPEED - 1.0
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def relc(mocker):
|
||||
mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
|
||||
mock_params.return_value.get_bool.return_value = True
|
||||
controller = RoadEdgeLaneChangeController(DesireHelper())
|
||||
controller.enabled = True
|
||||
return controller
|
||||
|
||||
|
||||
def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
|
||||
for _ in range(int(seconds / DT_MDL) + 1):
|
||||
controller.update(road_edge_stds, lane_line_probs, v_ego)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
|
||||
([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
|
||||
([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
|
||||
])
|
||||
def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
|
||||
drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
|
||||
assert getattr(relc, attr)
|
||||
|
||||
|
||||
def test_edge_detection_requires_time(relc):
|
||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
|
||||
assert not relc.left_edge_detected
|
||||
|
||||
|
||||
def test_both_edges_detected(relc):
|
||||
drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
|
||||
assert relc.left_edge_detected
|
||||
assert relc.right_edge_detected
|
||||
|
||||
|
||||
def test_noise_doesnt_clear(relc):
|
||||
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
|
||||
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
|
||||
|
||||
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
relc.update(*clear, V_HIGH)
|
||||
relc.update(*edge, V_HIGH)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
|
||||
def test_clears_after_window(relc):
|
||||
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
|
||||
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
|
||||
|
||||
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
|
||||
assert not relc.left_edge_detected
|
||||
assert relc.left_edge_timer == 0.0
|
||||
|
||||
|
||||
def test_low_speed_skips(relc):
|
||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
|
||||
assert not relc.left_edge_detected
|
||||
assert relc.left_edge_timer == 0.0
|
||||
|
||||
|
||||
def test_speed_drop_resets(relc):
|
||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
|
||||
assert not relc.left_edge_detected
|
||||
|
||||
|
||||
def test_param_off_resets(relc):
|
||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
|
||||
assert relc.left_edge_detected
|
||||
|
||||
relc.params.get_bool.return_value = False
|
||||
relc.read_params()
|
||||
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
|
||||
assert not relc.left_edge_detected
|
||||
assert not relc.right_edge_detected
|
||||
@@ -243,4 +243,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
|
||||
},
|
||||
|
||||
EventNameSP.laneChangeRoadEdge: {
|
||||
ET.WARNING: Alert(
|
||||
"Lane Change Unavailable: Road Edge",
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
|
||||
},
|
||||
}
|
||||
|
||||
@@ -1,26 +1,4 @@
|
||||
{
|
||||
"AccelPersonality": {
|
||||
"title": "Acceleration Personality",
|
||||
"description": "Select the acceleration personality profile. Sport provides more aggressive acceleration, Eco provides gentler acceleration.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Sport"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Eco"
|
||||
}
|
||||
]
|
||||
},
|
||||
"AccelPersonalityEnabled": {
|
||||
"title": "Custom Acceleration Personality",
|
||||
"description": "Enable custom acceleration and braking profiles that adjust max acceleration and min deceleration based on speed and selected personality."
|
||||
},
|
||||
"AccessToken": {
|
||||
"title": "AccessTokenIsNice",
|
||||
"description": ""
|
||||
@@ -1136,6 +1114,10 @@
|
||||
"title": "Record Front Lock",
|
||||
"description": ""
|
||||
},
|
||||
"RoadEdgeLaneChangeEnabled": {
|
||||
"title": "Block Lane Change: Road Edge Detection",
|
||||
"description": ""
|
||||
},
|
||||
"RoadName": {
|
||||
"title": "Road Name",
|
||||
"description": ""
|
||||
|
||||
@@ -519,6 +519,12 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "RoadEdgeLaneChangeEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Block Lane Change: Road Edge Detection",
|
||||
"description": "Blocks lane change when the model sees a road edge on the side you signal."
|
||||
},
|
||||
{
|
||||
"key": "AutoLaneChangeBsmDelay",
|
||||
"widget": "toggle",
|
||||
@@ -620,65 +626,6 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Acceleration Personality",
|
||||
"description": "Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Sport"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Eco"
|
||||
}
|
||||
],
|
||||
"visibility": [
|
||||
{
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
},
|
||||
{
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
@@ -1723,13 +1670,13 @@
|
||||
{
|
||||
"id": "updates",
|
||||
"title": "Updates",
|
||||
"description": "Control software updates",
|
||||
"description": "Control automatic software updates",
|
||||
"items": [
|
||||
{
|
||||
"key": "DisableUpdates",
|
||||
"widget": "toggle",
|
||||
"title": "Disable Updates",
|
||||
"description": "When enabled, software updates will be off. This requires a reboot to take effect.",
|
||||
"description": "When enabled, automatic software updates will be off. This requires a reboot to take effect.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
|
||||
@@ -43,34 +43,6 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Acceleration Personality
|
||||
description: Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.
|
||||
options:
|
||||
- value: 0
|
||||
label: Sport
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 2
|
||||
label: Eco
|
||||
visibility:
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -9,12 +9,12 @@ description: Software update preferences
|
||||
sections:
|
||||
- id: updates
|
||||
title: Updates
|
||||
description: Control software updates
|
||||
description: Control automatic software updates
|
||||
items:
|
||||
- key: DisableUpdates
|
||||
widget: toggle
|
||||
title: Disable Updates
|
||||
description: When enabled, software updates will be off. This requires a reboot to take effect.
|
||||
description: When enabled, automatic software updates will be off. This requires a reboot to take effect.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- $ref: '#/macros/advanced_only'
|
||||
|
||||
Reference in New Issue
Block a user