mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 05:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Deprecate controlsState state fields (#33437)

Browse Source 
* Deprecate controlsState state fields

* sim works

* update refs

* one more

* these too

* update sim
old-commit-hash: 3924ac587b735d1e735af4cb77faf6ccf053f656
This commit is contained in:
Adeeb Shihadeh
2024-09-03 14:40:23 -07:00
committed by GitHub
parent 53288d4dbf
commit bd2b09c7b6
18 changed files with 60 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
cereal/log.capnp
View File

@@ -739,21 +739,7 @@ struct ControlsState @0x97ff69c53601abf1 {
aTarget @35 :Float32;
curvature @37 :Float32; # path curvature from vehicle model
desiredCurvature @61 :Float32; # lag adjusted curvatures used by lateral controllers
# TODO: remove these, they're now in selfdriveState
alertText1 @24 :Text;
alertText2 @25 :Text;
alertStatus @38 :SelfdriveState.AlertStatus;
alertSize @39 :SelfdriveState.AlertSize;
alertType @44 :Text;
alertSound @56 :Car.CarControl.HUDControl.AudibleAlert;
engageable @41 :Bool; # can OP be engaged?
forceDecel @51 :Bool;
state @31 :SelfdriveState.OpenpilotState;
enabled @19 :Bool;
active @36 :Bool;
experimentalMode @64 :Bool;
personality @66 :LongitudinalPersonality;
lateralControlState :union {
indiState @52 :LateralINDIState;
@@ -880,6 +866,18 @@ struct ControlsState @0x97ff69c53601abf1 {
canErrorCounterDEPRECATED @57 :UInt32;
vPidDEPRECATED @2 :Float32;
alertBlinkingRateDEPRECATED @42 :Float32;
alertText1DEPRECATED @24 :Text;
alertText2DEPRECATED @25 :Text;
alertStatusDEPRECATED @38 :SelfdriveState.AlertStatus;
alertSizeDEPRECATED @39 :SelfdriveState.AlertSize;
alertTypeDEPRECATED @44 :Text;
alertSound2DEPRECATED @56 :Car.CarControl.HUDControl.AudibleAlert;
engageableDEPRECATED @41 :Bool; # can OP be engaged?
stateDEPRECATED @31 :SelfdriveState.OpenpilotState;
enabledDEPRECATED @19 :Bool;
activeDEPRECATED @36 :Bool;
experimentalModeDEPRECATED @64 :Bool;
personalityDEPRECATED @66 :LongitudinalPersonality;
vCruiseDEPRECATED @22 :Float32; # actual set speed
vCruiseClusterDEPRECATED @63 :Float32; # set speed to display in the UI
}

14
selfdrive/controls/controlsd.py
View File

@@ -733,22 +733,10 @@ class Controls:
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
if current_alert:
controlsState.alertText1 = current_alert.alert_text_1
controlsState.alertText2 = current_alert.alert_text_2
controlsState.alertSize = current_alert.alert_size
controlsState.alertStatus = current_alert.alert_status
controlsState.alertType = current_alert.alert_type
controlsState.alertSound = current_alert.audible_alert
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.desiredCurvature = self.desired_curvature
controlsState.state = self.state
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
@@ -756,8 +744,6 @@ class Controls:
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.experimentalMode = self.experimental_mode
controlsState.personality = self.personality
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:

10
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -96,7 +96,7 @@ class LongitudinalPlanner:
return x, v, a, j
def update(self, sm):
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
v_ego = sm['carState'].vEgo
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
@@ -106,7 +106,7 @@ class LongitudinalPlanner:
force_slow_decel = sm['controlsState'].forceDecel
# Reset current state when not engaged, or user is controlling the speed
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
@@ -134,11 +134,11 @@ class LongitudinalPlanner:
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, 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)
@@ -157,7 +157,7 @@ class LongitudinalPlanner:
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']

2
selfdrive/controls/plannerd.py
View File

@@ -17,7 +17,7 @@ def plannerd_thread():
longitudinal_planner = LongitudinalPlanner(CP)
pm = messaging.PubMaster(['longitudinalPlan'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'],
poll='modelV2', ignore_avg_freq=['radarState'])
while True:

2
selfdrive/monitoring/dmonitoringd.py
View File

@@ -13,7 +13,7 @@ def dmonitoringd_thread():
params = Params()
pm = messaging.PubMaster(['driverMonitoringState'])
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll='driverStateV2')
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))

4
selfdrive/monitoring/helpers.py
View File

@@ -403,13 +403,13 @@ class DriverMonitoring:
driver_state=sm['driverStateV2'],
cal_rpy=sm['liveCalibration'].rpyCalib,
car_speed=sm['carState'].vEgo,
op_engaged=sm['controlsState'].enabled
op_engaged=sm['selfdriveState'].enabled
)
# Update distraction events
self._update_events(
driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed,
op_engaged=sm['controlsState'].enabled,
op_engaged=sm['selfdriveState'].enabled,
standstill=sm['carState'].standstill,
wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park],
car_speed=sm['carState'].vEgo

4
selfdrive/pandad/pandad.cc
View File

@@ -313,7 +313,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
}
void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started) {
static SubMaster sm({"controlsState"});
static SubMaster sm({"selfdriveState"});
std::vector<std::string> connected_serials;
for (Panda *p : pandas) {
@@ -351,7 +351,7 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoof
}
sm.update(0);
const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
for (const auto &panda : pandas) {
panda->send_heartbeat(engaged);
}

7
selfdrive/test/longitudinal_maneuvers/plant.py
View File

@@ -21,6 +21,7 @@ class Plant:
if not Plant.messaging_initialized:
Plant.radar = messaging.pub_sock('radarState')
Plant.controls_state = messaging.pub_sock('controlsState')
Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
Plant.car_state = messaging.pub_sock('carState')
Plant.plan = messaging.sub_sock('longitudinalPlan')
Plant.messaging_initialized = True
@@ -61,6 +62,7 @@ class Plant:
# note that this is worst case for MPC, since model will delay long mpc by one time step
radar = messaging.new_message('radarState')
control = messaging.new_message('controlsState')
ss = messaging.new_message('selfdriveState')
car_state = messaging.new_message('carState')
model = messaging.new_message('modelV2')
a_lead = (v_lead - self.v_lead_prev)/self.ts
@@ -111,8 +113,8 @@ class Plant:
model.modelV2.acceleration = acceleration
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
control.controlsState.experimentalMode = self.e2e
control.controlsState.personality = self.personality
ss.selfdriveState.experimentalMode = self.e2e
ss.selfdriveState.personality = self.personality
control.controlsState.forceDecel = self.force_decel
car_state.carState.vEgo = float(self.speed)
car_state.carState.standstill = self.speed < 0.01
@@ -122,6 +124,7 @@ class Plant:
sm = {'radarState': radar.radarState,
'carState': car_state.carState,
'controlsState': control.controlsState,
'selfdriveState': ss.selfdriveState,
'modelV2': model.modelV2}
self.planner.update(sm)
self.speed = self.planner.v_desired_filter.x

4
selfdrive/test/process_replay/migration.py
View File

@@ -39,9 +39,9 @@ def migrate_controlsState(lr):
m.logMonoTime = msg.logMonoTime
ss = m.selfdriveState
for field in ("enabled", "active", "state", "engageable", "alertText1", "alertText2",
"alertStatus", "alertSize", "alertType", "alertSound", "experimentalMode",
"alertStatus", "alertSize", "alertType", "experimentalMode",
"personality"):
setattr(ss, field, getattr(msg.controlsState, field))
setattr(ss, field, getattr(msg.controlsState, field+"DEPRECATED"))
ret.append(m.as_reader())
elif msg.which() == 'carState' and last_cs is not None:
if last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruise > 0.1:

8
selfdrive/test/process_replay/process_replay.py
View File

@@ -506,7 +506,7 @@ CONFIGS = [
),
ProcessConfig(
proc_name="plannerd",
pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"],
pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"],
subs=["longitudinalPlan"],
ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
init_callback=get_car_params_callback,
@@ -522,7 +522,7 @@ CONFIGS = [
),
ProcessConfig(
proc_name="dmonitoringd",
pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"],
pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"],
subs=["driverMonitoringState"],
ignore=["logMonoTime"],
should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"),
@@ -810,8 +810,8 @@ def check_openpilot_enabled(msgs: LogIterable) -> bool:
if msg.which() == "carParams":
if msg.carParams.notCar:
return True
elif msg.which() == "controlsState":
if msg.controlsState.active:
elif msg.which() == "selfdriveState":
if msg.selfdriveState.active:
cur_enabled_count += 1
else:
cur_enabled_count = 0

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
073dcca6932c5c66cdadf9aee9b531b623795888
ca8cca8eeca938c3802109d6ea25cb719dcc649a

4
selfdrive/test/test_onroad.py
View File

@@ -429,6 +429,6 @@ class TestOnroad:
if evt.noEntry:
no_entries[evt.name] += 1
eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']]
eng = [m.selfdriveState.engageable for m in self.service_msgs['selfdriveState']]
assert all(eng), \
f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}"
f"Not engageable for whole segment:\n- selfdriveState.engageable: {Counter(eng)}\n- No entry events: {no_entries}"

6
selfdrive/test/test_time_to_onroad.py
View File

@@ -20,7 +20,7 @@ def test_time_to_onroad():
proc = subprocess.Popen(["python", manager_path])
start_time = time.monotonic()
sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents'])
sm = messaging.SubMaster(['selfdriveState', 'controlsState', 'deviceState', 'onroadEvents'])
try:
# wait for onroad. timeout assumes panda is up to date
with Timeout(10, "timed out waiting to go onroad"):
@@ -39,7 +39,7 @@ def test_time_to_onroad():
if initialized:
sm.update(100)
assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}"
assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
break
finally:
print(f"onroad events: {sm['onroadEvents']}")
@@ -50,7 +50,7 @@ def test_time_to_onroad():
while (time.monotonic() - st) < 10.:
sm.update(100)
assert sm.all_alive(), sm.alive
assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}"
assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
assert sm['controlsState'].cumLagMs < 10.
finally:
proc.terminate()

6
system/hardware/hardwared.py
View File

@@ -164,7 +164,7 @@ def hw_state_thread(end_event, hw_queue):
def hardware_thread(end_event, hw_queue) -> None:
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll="pandaStates")
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates")
count = 0
@@ -341,8 +341,8 @@ def hardware_thread(end_event, hw_queue) -> None:
engaged_prev = False
HARDWARE.set_power_save(not should_start)
if sm.updated['controlsState']:
engaged = sm['controlsState'].enabled
if sm.updated['selfdriveState']:
engaged = sm['selfdriveState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged)
engaged_prev = engaged

24
tools/replay/logreader.cc
View File

@@ -90,18 +90,18 @@ void LogReader::migrateOldEvents() {
new_evt.setLogMonoTime(old_evt.getLogMonoTime());
auto new_state = new_evt.initSelfdriveState();
new_state.setActive(old_state.getActive());
new_state.setAlertSize(old_state.getAlertSize());
new_state.setAlertSound(old_state.getAlertSound());
new_state.setAlertStatus(old_state.getAlertStatus());
new_state.setAlertText1(old_state.getAlertText1());
new_state.setAlertText2(old_state.getAlertText2());
new_state.setAlertType(old_state.getAlertType());
new_state.setEnabled(old_state.getEnabled());
new_state.setEngageable(old_state.getEngageable());
new_state.setExperimentalMode(old_state.getExperimentalMode());
new_state.setPersonality(old_state.getPersonality());
new_state.setState(old_state.getState());
new_state.setActive(old_state.getActiveDEPRECATED());
new_state.setAlertSize(old_state.getAlertSizeDEPRECATED());
new_state.setAlertSound(old_state.getAlertSound2DEPRECATED());
new_state.setAlertStatus(old_state.getAlertStatusDEPRECATED());
new_state.setAlertText1(old_state.getAlertText1DEPRECATED());
new_state.setAlertText2(old_state.getAlertText2DEPRECATED());
new_state.setAlertType(old_state.getAlertTypeDEPRECATED());
new_state.setEnabled(old_state.getEnabledDEPRECATED());
new_state.setEngageable(old_state.getEngageableDEPRECATED());
new_state.setExperimentalMode(old_state.getExperimentalModeDEPRECATED());
new_state.setPersonality(old_state.getPersonalityDEPRECATED());
new_state.setState(old_state.getStateDEPRECATED());
// Serialize the new event to the buffer
auto buf_size = msg.getSerializedSize();

5
tools/sim/bridge/common.py
View File

@@ -167,8 +167,7 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
self.simulated_sensors.update(self.simulator_state, self.world)
self.simulated_car.sm.update(0)
controlsState = self.simulated_car.sm['controlsState']
self.simulator_state.is_engaged = controlsState.active
self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
if self.simulator_state.is_engaged:
throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
@@ -176,7 +175,7 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
self.past_startup_engaged = True
elif not self.past_startup_engaged and controlsState.engageable:
elif not self.past_startup_engaged and self.simulated_car.sm['selfdriveState'].engageable:
self.simulator_state.cruise_button = CruiseButtons.DECEL_SET if self.startup_button_prev else CruiseButtons.MAIN # force engagement on startup
self.startup_button_prev = not self.startup_button_prev

2
tools/sim/lib/simulated_car.py
View File

@@ -14,7 +14,7 @@ class SimulatedCar:
def __init__(self):
self.pm = messaging.PubMaster(['can', 'pandaStates'])
self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams'])
self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams', 'selfdriveState'])
self.cp = self.get_car_can_parser()
self.idx = 0
self.params = Params()

4
tools/sim/tests/test_sim_bridge.py
View File

@@ -25,7 +25,7 @@ class TestSimBridgeBase:
p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR)
self.processes.append(p_manager)
sm = messaging.SubMaster(['controlsState', 'onroadEvents', 'managerState'])
sm = messaging.SubMaster(['selfdriveState', 'onroadEvents', 'managerState'])
q = Queue()
bridge = self.create_bridge()
p_bridge = bridge.run(q, retries=10)
@@ -63,7 +63,7 @@ class TestSimBridgeBase:
while time.monotonic() < start_time + max_time_per_step:
sm.update()
if sm.all_alive() and sm['controlsState'].active:
if sm.all_alive() and sm['selfdriveState'].active:
control_active += 1
if control_active == min_counts_control_active: