sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
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
vw-mqb-aeb
Adeeb Shihadeh 1 year ago committed by GitHub
parent 53288d4dbf
commit bd2b09c7b6
18 changed files with 60 additions and 74 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 26
      cereal/log.capnp
  2. 14
      selfdrive/controls/controlsd.py
  3. 10
      selfdrive/controls/lib/longitudinal_planner.py
  4. 2
      selfdrive/controls/plannerd.py
  5. 2
      selfdrive/monitoring/dmonitoringd.py
  6. 4
      selfdrive/monitoring/helpers.py
  7. 4
      selfdrive/pandad/pandad.cc
  8. 7
      selfdrive/test/longitudinal_maneuvers/plant.py
  9. 4
      selfdrive/test/process_replay/migration.py
  10. 8
      selfdrive/test/process_replay/process_replay.py
  11. 2
      selfdrive/test/process_replay/ref_commit
  12. 4
      selfdrive/test/test_onroad.py
  13. 6
      selfdrive/test/test_time_to_onroad.py
  14. 6
      system/hardware/hardwared.py
  15. 24
      tools/replay/logreader.cc
  16. 5
      tools/sim/bridge/common.py
  17. 2
      tools/sim/lib/simulated_car.py
  18. 4
      tools/sim/tests/test_sim_bridge.py

26
cereal/log.capnp
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape View File

@ -1 +1 @@
073dcca6932c5c66cdadf9aee9b531b623795888
ca8cca8eeca938c3802109d6ea25cb719dcc649a

4
selfdrive/test/test_onroad.py
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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:

Write Preview
Loading…
Cancel
Save