This commit is contained in:
rav4kumar
2025-01-04 13:17:37 -07:00
parent 63f0237e7a
commit 306685f1ab
13 changed files with 460 additions and 4 deletions
+11 -1
View File
@@ -8,10 +8,20 @@ $Cxx.namespace("cereal");
# cereal, so use these if you want custom events in your fork.
# you can rename the struct, but don't change the identifier
enum MpcSource {
acc @0;
blended @1;
}
struct CustomReserved0 @0x81c2f05a394cf4af {
}
struct CustomReserved1 @0xaedffd8f31e7b55d {
struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
e2eBlended @0 :Text;
e2eStatus @1 :Bool;
mpcSource @2 :MpcSource;
dynamicExperimentalControl @3 :Bool;
}
struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+1 -1
View File
@@ -2559,7 +2559,7 @@ struct Event {
# *********** Custom: reserved for forks ***********
customReserved0 @107 :Custom.CustomReserved0;
customReserved1 @108 :Custom.CustomReserved1;
longitudinalPlanSP @108 :Custom.LongitudinalPlanSP;
customReserved2 @109 :Custom.CustomReserved2;
customReserved3 @110 :Custom.CustomReserved3;
customReserved4 @111 :Custom.CustomReserved4;
+2
View File
@@ -74,6 +74,8 @@ _services: dict[str, tuple] = {
"userFlag": (True, 0., 1),
"microphone": (True, 10., 10),
"longitudinalPlanSP": (True, 20., 5),
# debug
"uiDebug": (True, 0., 1),
"testJoystick": (True, 0.),
+2
View File
@@ -201,6 +201,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"UpdaterLastFetchTime", PERSISTENT},
{"Version", PERSISTENT},
{"EnableGithubRunner", PERSISTENT},
{"DynamicExperimentalControl", PERSISTENT},
};
} // namespace
+3
View File
@@ -74,6 +74,8 @@ class Car:
self.CC_prev = car.CarControl.new_message()
self.initialized_prev = False
self.dynamic_experimental_control = False
self.last_actuators_output = structs.CarControl.Actuators()
self.params = Params()
@@ -151,6 +153,7 @@ class Car:
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
# card is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
+1 -1
View File
@@ -37,7 +37,7 @@ class Controls:
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'longitudinalPlanSP'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
self.steer_limited = False
@@ -2,6 +2,8 @@
import math
import numpy as np
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from cereal import custom
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
@@ -16,6 +18,9 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -28,6 +33,7 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
MpcSource = custom.MpcSource
def get_max_accel(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
@@ -84,6 +90,18 @@ class LongitudinalPlanner:
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
self.params = Params()
self.param_read_counter = 0
self.read_param()
self.dynamic_experimental_controller = DynamicExperimentalController()
def read_param(self):
try:
self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
except AttributeError:
self.dynamic_experimental_controller = DynamicExperimentalController()
@staticmethod
def parse_model(model_msg, model_error):
if (len(model_msg.position.x) == ModelConstants.IDX_N and
@@ -104,6 +122,17 @@ class LongitudinalPlanner:
throttle_prob = 1.0
return x, v, a, j, throttle_prob
def update(self, sm):
if self.param_read_counter % 50 == 0:
self.read_param()
self.param_read_counter += 1
if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) #, sm['navInstruction'].maneuverDistance)
self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
else:
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
def update(self, sm):
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
@@ -206,3 +235,20 @@ class LongitudinalPlanner:
longitudinalPlan.allowThrottle = self.allow_throttle
pm.send('longitudinalPlan', plan_send)
plan_sp_send = messaging.new_message('longitudinalPlanSP')
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
# DEC
longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
print(f"mpcSource: {longitudinalPlanSP.mpcSource}")
longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}")
pm.send('longitudinalPlanSP', plan_sp_send)
+1 -1
View File
@@ -18,7 +18,7 @@ def main():
ldw = LaneDepartureWarning()
longitudinal_planner = LongitudinalPlanner(CP)
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
poll='modelV2', ignore_avg_freq=['radarState'])
+6
View File
@@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"",
"../assets/img_experimental_white.svg",
},
{
"DynamicExperimentalControl",
tr("Enable Dynamic Experimental Control"),
tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
"../assets/offroad/icon_blank.png",
},
{
"DisengageOnAccelerator",
tr("Disengage on Accelerator Pedal"),
+6
View File
@@ -106,6 +106,12 @@ void ModelRenderer::drawLaneLines(QPainter &painter) {
}
void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) {
//auto *s = uiState();
//auto &sm = *(s->sm);
//const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
//bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
// (!long_plan_sp.getDynamicExperimentalControl() && sm["selfdriveState"].getSelfdriveState().getExperimentalMode());
QLinearGradient bg(0, height, 0, 0);
if (experimental_mode) {
// The first half of track_vertices are the points for the right side of the path
+1
View File
@@ -95,6 +95,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
//"longitudinalPlanSP",
});
prime_state = new PrimeState(this);
language = QString::fromStdString(Params().get("LanguageSetting"));
@@ -0,0 +1,379 @@
#!/usr/bin/env python3
# The MIT License
#
# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#
# Version = 2024-7-11
from common.numpy_fast import interp
import numpy as np
# d-e2e, from modeldata.h
TRAJECTORY_SIZE = 33
LEAD_WINDOW_SIZE = 4
LEAD_PROB = 0.6
SLOW_DOWN_WINDOW_SIZE = 4
SLOW_DOWN_PROB = 0.6
SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
SLOWNESS_WINDOW_SIZE = 12
SLOWNESS_PROB = 0.5
SLOWNESS_CRUISE_OFFSET = 1.05
DANGEROUS_TTC_WINDOW_SIZE = 3
DANGEROUS_TTC = 2.3
HIGHWAY_CRUISE_KPH = 70
STOP_AND_GO_FRAME = 60
SET_MODE_TIMEOUT = 10
MPC_FCW_WINDOW_SIZE = 10
MPC_FCW_PROB = 0.5
V_ACC_MIN = 9.72
class SNG_State:
off = 0
stopped = 1
going = 2
class GenericMovingAverageCalculator:
def __init__(self, window_size):
self.window_size = window_size
self.data = []
self.total = 0
def add_data(self, value):
if len(self.data) == self.window_size:
self.total -= self.data.pop(0)
self.data.append(value)
self.total += value
def get_moving_average(self):
if len(self.data) == 0:
return None
return self.total / len(self.data)
def reset_data(self):
self.data = []
self.total = 0
class WeightedMovingAverageCalculator:
def __init__(self, window_size):
self.window_size = window_size
self.data = []
self.weights = np.linspace(1, 3, window_size) # Linear weights, adjust as needed
def add_data(self, value):
if len(self.data) == self.window_size:
self.data.pop(0)
self.data.append(value)
def get_weighted_average(self):
if len(self.data) == 0:
return None
weighted_sum = np.dot(self.data, self.weights[-len(self.data):])
weight_total = np.sum(self.weights[-len(self.data):])
return weighted_sum / weight_total
def reset_data(self):
self.data = []
class DynamicExperimentalController:
def __init__(self):
self._is_enabled = False
self._mode = 'acc'
self._mode_prev = 'acc'
self._mode_changed = False
self._frame = 0
# Use weighted moving average for filtering leads
self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
self._has_lead_filtered = False
self._has_lead_filtered_prev = False
self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
self._has_slow_down = False
self._has_blinkers = False
self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
self._has_slowness = False
self._has_nav_instruction = False
self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
self._has_dangerous_ttc = False
self._v_ego_kph = 0.
self._v_cruise_kph = 0.
self._has_lead = False
self._has_standstill = False
self._has_standstill_prev = False
self._sng_transit_frame = 0
self._sng_state = SNG_State.off
self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
self._has_mpc_fcw = False
self._mpc_fcw_crash_cnt = 0
self._set_mode_timeout = 0
pass
def _adaptive_slowdown_threshold(self):
"""
Adapts the slow down threshold based on vehicle speed and recent behavior.
"""
return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data)))
def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
"""
Basic anomaly detection using standard deviation.
"""
if len(recent_data) < 5:
return False
mean = np.mean(recent_data)
std_dev = np.std(recent_data)
anomaly = recent_data[-1] > mean + threshold * std_dev
# Context check to ensure repeated anomaly
if context_check:
return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
return anomaly
def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2):
"""
Smoothing the lead detection to avoid erratic behavior.
"""
self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
return self._has_lead_filtered > LEAD_PROB
def _adaptive_lead_prob_threshold(self):
"""
Adapts lead probability threshold based on driving conditions.
"""
if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
return LEAD_PROB + 0.1 # Increase the threshold on highways
return LEAD_PROB
def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance):
self._v_ego_kph = car_state.vEgo * 3.6
self._v_cruise_kph = controls_state.vCruise
self._has_lead = lead_one.status
self._has_standstill = car_state.standstill
# fcw detection
self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB
# nav enable detection
#self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
# lead detection with smoothing
self._lead_gmac.add_data(lead_one.status)
self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB
#lead_prob = self._lead_gmac.get_weighted_average() or 0
#self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
# adaptive slow down detection
adaptive_threshold = self._adaptive_slowdown_threshold()
slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
self._slow_down_gmac.add_data(slow_down_trigger)
self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB
# anomaly detection for slow down events
if self._anomaly_detection(self._slow_down_gmac.data):
# Handle anomaly: potentially log it, adjust behavior, or issue a warning
self._has_slow_down = False # Reset slow down if anomaly detected
# blinker detection
self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
# sng detection
if self._has_standstill:
self._sng_state = SNG_State.stopped
self._sng_transit_frame = 0
else:
if self._sng_transit_frame == 0:
if self._sng_state == SNG_State.stopped:
self._sng_state = SNG_State.going
self._sng_transit_frame = STOP_AND_GO_FRAME
elif self._sng_state == SNG_State.going:
self._sng_state = SNG_State.off
elif self._sng_transit_frame > 0:
self._sng_transit_frame -= 1
# slowness detection
if not self._has_standstill:
self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB
# dangerous TTC detection
if not self._has_lead_filtered and self._has_lead_filtered_prev:
self._dangerous_ttc_gmac.reset_data()
self._has_dangerous_ttc = False
if self._has_lead and car_state.vEgo >= 0.01:
self._dangerous_ttc_gmac.add_data(lead_one.dRel/car_state.vEgo)
self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
# keep prev values
self._has_standstill_prev = self._has_standstill
self._has_lead_filtered_prev = self._has_lead_filtered
self._frame += 1
def _radarless_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
self._set_mode('blended')
return
# Nav enabled and distance to upcoming turning is 300 or below
#if self._has_nav_instruction:
# self._set_mode('blended')
# return
# when blinker is on and speed is driving below V_ACC_MIN: blended
# we dont want it to switch mode at higher speed, blended may trigger hard brake
#if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
# self._set_mode('blended')
# return
# when at highway cruise and SNG: blended
# ensuring blended mode is used because acc is bad at catching SNG lead car
# especially those who accel very fast and then brake very hard.
#if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
# self._set_mode('blended')
# return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
if self._has_standstill:
self._set_mode('blended')
return
# when detecting slow down scenario: blended
# e.g. traffic light, curve, stop sign etc.
if self._has_slow_down:
self._set_mode('blended')
return
# when detecting lead slow down: blended
# use blended for higher braking capability
if self._has_dangerous_ttc:
self._set_mode('blended')
return
# car driving at speed lower than set speed: acc
if self._has_slowness:
self._set_mode('acc')
return
self._set_mode('acc')
def _radar_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
self._set_mode('blended')
return
# If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
if self._has_lead_filtered and not self._has_standstill:
self._set_mode('acc')
return
# when blinker is on and speed is driving below V_ACC_MIN: blended
# we dont want it to switch mode at higher speed, blended may trigger hard brake
#if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
# self._set_mode('blended')
# return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
if self._has_standstill:
self._set_mode('blended')
return
# when detecting slow down scenario: blended
# e.g. traffic light, curve, stop sign etc.
if self._has_slow_down:
self._set_mode('blended')
return
# car driving at speed lower than set speed: acc
if self._has_slowness:
self._set_mode('acc')
return
# Nav enabled and distance to upcoming turning is 300 or below
#if self._has_nav_instruction:
# self._set_mode('blended')
# return
self._set_mode('acc')
def update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance):
if self._is_enabled:
self._update(car_state, lead_one, md, controls_state) #, maneuver_distance)
if radar_unavailable:
self._radarless_mode()
else:
self._radar_mode()
self._mode_changed = self._mode != self._mode_prev
self._mode_prev = self._mode
def get_mpc_mode(self):
return self._mode
def has_changed(self):
return self._mode_changed
def set_enabled(self, enabled):
self._is_enabled = enabled
def is_enabled(self):
return self._is_enabled
def set_mpc_fcw_crash_cnt(self, crash_cnt):
self._mpc_fcw_crash_cnt = crash_cnt
def _set_mode(self, mode):
if self._set_mode_timeout == 0:
self._mode = mode
if mode == "blended":
self._set_mode_timeout = SET_MODE_TIMEOUT
if self._set_mode_timeout > 0:
self._set_mode_timeout -= 1
+1
View File
@@ -40,6 +40,7 @@ def manager_init() -> None:
("LanguageSetting", "main_en"),
("OpenpilotEnabledToggle", "1"),
("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
("DynamicExperimentalControl", "0"),
]
if params.get_bool("RecordFrontLock"):