From bc1aad6f6a66fbeb490997ee7f8a282feb5f6dfd Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 12 Jul 2024 21:36:28 +0000 Subject: [PATCH 1/7] git-crypt: add collaborators --- .git-crypt/.gitattributes | 4 ++++ .../106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg | Bin 0 -> 725 bytes .../E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg | Bin 0 -> 735 bytes 3 files changed, 4 insertions(+) create mode 100644 .git-crypt/.gitattributes create mode 100644 .git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg create mode 100644 .git-crypt/keys/default/0/E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg diff --git a/.git-crypt/.gitattributes b/.git-crypt/.gitattributes new file mode 100644 index 0000000000..665b10e8f0 --- /dev/null +++ b/.git-crypt/.gitattributes @@ -0,0 +1,4 @@ +# Do not edit this file. To specify the files to encrypt, create your own +# .gitattributes file in the directory where your files are. +* !filter !diff +*.gpg binary diff --git a/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg b/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg new file mode 100644 index 0000000000000000000000000000000000000000..97b89be16eef468dea1688448132cd49bc1a9205 GIT binary patch literal 725 zcmV;`0xJE50t^Eiti!r3*)-+>5CEM;BTRKyWmz_3gO6PMXMbPN8+OdA6CadusYg9^ zWEg3Hz?fKrSv+uXx{Tssm0G=WQZdR4NKEO3c+2^|&X>&>$KG$#mLSy6=!V~-Blgi< z4rbWQe~-g;X9ORi=kxVsUp%TuJzT8$KfwLD=!;G22C0wERyD)W8*Cqx$nwrUQ}{`SO70zzskdift(^gjR0vt;&(*}}_sKQrIoG1GHILGca1FFrY23uBQ6ca*) zN2ZBxDgh+>4NJ=^wpG%=0|AIJdW$G(>Cj!#x{XK+oplLEf2E9Ui6cd(9*v zH2SrDh492MO=i<_&_X5mlVw}px}?7bLxm!ItMLI`Vl-3GhjO*>3*S^}x#3+R_LqTs z0@tS3baUe2=5JPH*zgRlaWq~6WGGaggBmy;;TaC7@~U7}3DrU33Z9lF@?;bC*_7)rtkjg8j6|?1VHGyK^gn?l6xnAgEx#Dza?w=UzyMmxvM+Qe+SWlz+UQ?Q5m53(MN8h>Y>D@RZ-j5M)P(Kq>$`lN{RFr{12*0F2 znlSADOAt%tK)Tu_aX3x5P=wumS{_Dv-_i20k{~-J6SYQl>~UN0JXnmf;WYQ+ zl=rpmLzyLjcWaVY!<;pOvWuLA(=M<#_tbnAXLS)ki|#8FaJ)whiG&coR=Z_ikWR>? zFBgrx0Tgi?(Q}^(EIoM>nkS(~sCSo~1hPsEAML=nFNbsqHIK9R^QNoeBBF7c`X>Pv z2tZj&5Q(v;8Tl^wn9uy`m|qfw_BRewH3nw`$fLi)1Zji~Pel2nzBJymh@fGl!s)#%;n5V(c*u;yCs?@dDe?5QF{V6*~;eqdI%ym|q}v^cIxfgJah zqPTravGaxq?yS&u?A~3E+J@@>Am<`ioGZhA`B?4FL%P5Ju*TZc+T%>mQZjHZ3PF!AeqNfQ7bGbH`6kQ_jEj zxk`Gih-)5$|KoWu-$MlpAwwb|lTjPXCnRW-KEjg+vFbb8bf`&zYCkC94P{ekg-Euy zRaURgwANGWf>8S~<}tx-0%z3@hQAg0*mp#WlH>^*hKrdC1X+)1(5^$P0q!#8j`fGP zk8f_lbc5xFGx{2PXsOrdN6YR9AJA=?S=57 RnAcT|%MwwGhU?B@M#$H9W Date: Mon, 8 Jul 2024 19:10:26 +0200 Subject: [PATCH 2/7] Processor definition check for __APPLE__ has a typo on replay (#32930) (cherry picked from commit 3c74ad145e0145429d782219ac255a26aa2f6135) --- tools/replay/util.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/util.cc b/tools/replay/util.cc index a08b3b3d5e..aaf73749d5 100644 --- a/tools/replay/util.cc +++ b/tools/replay/util.cc @@ -323,7 +323,7 @@ void precise_nano_sleep(int64_t nanoseconds, std::atomic &should_exit) { req.tv_sec = nanoseconds / 1000000000; req.tv_nsec = nanoseconds % 1000000000; while (!should_exit) { -#ifdef __APPLE_ +#ifdef __APPLE__ int ret = nanosleep(&req, &rem); if (ret == 0 || errno != EINTR) break; From 03fd6050f752fff00b3f7ce7ca31f7b8b919dc53 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 13 Jul 2024 11:38:48 -0400 Subject: [PATCH 3/7] Driving Model Selector v5: Bug fixes and revert to param checks --- cereal/custom.capnp | 12 ----------- selfdrive/modeld/custom_model_metadata.py | 24 ++++++++++++++-------- selfdrive/modeld/fill_model_msg.py | 8 +++++--- selfdrive/modeld/modeld.py | 3 --- selfdrive/ui/qt/onroad/annotated_camera.cc | 3 ++- selfdrive/ui/qt/onroad/annotated_camera.h | 3 ++- selfdrive/ui/qt/onroad/buttons.cc | 2 +- selfdrive/ui/qt/onroad_settings.cc | 4 ++-- selfdrive/ui/ui.cc | 10 +++------ selfdrive/ui/ui.h | 3 +-- 10 files changed, 31 insertions(+), 41 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8825cf6f07..3b36532ac9 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -25,15 +25,6 @@ enum AccelerationPersonality { stock @3; } -enum ModelGeneration { - default @0; - one @1; - two @2; - three @3; - four @4; - five @5; -} - struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; @@ -198,9 +189,6 @@ struct E2eLongStateSP @0xa5cd762cd951a455 { struct ModelDataV2SP @0xf98d843bfd7004a3 { laneChangePrev @0 :Bool; laneChangeEdgeBlock @1 :Bool; - customModel @2 :Bool; - modelGeneration @3 :ModelGeneration; - modelCapabilities @4 :UInt32; } struct CustomReserved7 @0xb86e6369214c01c8 { diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 360f7046ad..685d9655e4 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,12 +1,18 @@ -from enum import IntFlag +from enum import IntFlag, IntEnum import os -from cereal import custom from openpilot.common.params import Params SIMULATION = "SIMULATION" in os.environ -ModelGeneration = custom.ModelGeneration + +class ModelGeneration(IntEnum): + DEFAULT = 0 + ONE = 1 + TWO = 2 + THREE = 3 + FOUR = 4 + FIVE = 5 class ModelCapabilities(IntFlag): @@ -47,19 +53,19 @@ class CustomModelMetadata: self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: - return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) + return int(self.params.get('DrivingModelGeneration') or ModelGeneration.DEFAULT) def get_model_capabilities(self) -> int: """Returns the model capabilities for a given generation.""" - if self.generation == ModelGeneration.five: + if self.generation == ModelGeneration.FIVE: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.four: + elif self.generation == ModelGeneration.FOUR: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.three: + elif self.generation == ModelGeneration.THREE: return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.two: + elif self.generation == ModelGeneration.TWO: return ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.one: + elif self.generation == ModelGeneration.ONE: return ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO else: # Default model is meant to represent the capabilities of the prebuilt model diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 39a161ed89..95099d27c3 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -55,7 +55,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, timestamp_llk: int, model_execution_time: float, nav_enabled: bool, valid: bool, - custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> None: + custom_model_valid: bool, custom_model_capabilities: int) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 frame_drop_perc = frame_drop * 100 extended_msg.valid = valid @@ -68,7 +68,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution + if not model_use_lateral_planner: + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -100,7 +102,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution: + if model_use_lateral_planner: solution = modelV2.lateralPlannerSolutionDEPRECATED solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ff3d9ec633..b6880d4d8d 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -402,9 +402,6 @@ def main(demo=False): if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): modelV2SP.laneChangePrev = DH.prev_lane_change modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED - modelV2SP.customModel = custom_model_metadata.valid - modelV2SP.modelGeneration = custom_model_metadata.generation - modelV2SP.modelCapabilities = int(custom_model_metadata.capabilities) pm.send('modelV2SP', modelv2_sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 009ff76138..7e27449dc5 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -375,6 +375,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { featureStatusToggle = s.scene.feature_status_toggle; experimental_btn->setVisible(!(showDebugUI && showVTC)); + customModelValid = s.scene.custom_driving_model_valid; drivingModelGen = s.scene.driving_model_generation; } @@ -1209,7 +1210,7 @@ void AnnotatedCameraWidget::drawFeatureStatusText(QPainter &p, int x, int y) { } // Dynamic Lane Profile - if (drivingModelGen == cereal::ModelGeneration::ONE) { + if (customModelValid && drivingModelGen == 1) { // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO drawFeatureStatusElement(dynamicLaneProfile, feature_text.dlp_list_text, feature_color.dlp_list_color, true, "OFF", "DLP"); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 29b9c4833b..5c3697aaf3 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -203,7 +203,8 @@ private: bool featureStatusToggle; - cereal::ModelGeneration drivingModelGen; + bool customModelValid; + int drivingModelGen; protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index fbca42da7b..57de6c12ee 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -99,7 +99,7 @@ void OnroadSettingsButton::paintEvent(QPaintEvent *event) { void OnroadSettingsButton::updateState(const UIState &s) { const auto cp = (*s.sm)["carParams"].getCarParams(); - auto dlp_enabled = s.scene.driving_model_generation == cereal::ModelGeneration::ONE; + auto dlp_enabled = s.scene.custom_driving_model_valid && s.scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO bool allow_btn = s.scene.onroad_settings_toggle && (dlp_enabled || hasLongitudinalControl(cp) || !cp.getPcmCruiseSpeed()); setVisible(allow_btn); diff --git a/selfdrive/ui/qt/onroad_settings.cc b/selfdrive/ui/qt/onroad_settings.cc index 00810294ba..d39d908662 100644 --- a/selfdrive/ui/qt/onroad_settings.cc +++ b/selfdrive/ui/qt/onroad_settings.cc @@ -119,7 +119,7 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent) void OnroadSettings::changeDynamicLaneProfile() { UIScene &scene = uiState()->scene; - bool can_change = scene.driving_model_generation == cereal::ModelGeneration::ONE; + bool can_change = scene.custom_driving_model_valid && scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO if (can_change) { scene.dynamic_lane_profile++; scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile; @@ -229,7 +229,7 @@ void OnroadSettings::refresh() { // Dynamic Lane Profile dlp_widget->updateDynamicLaneProfile("DynamicLaneProfile"); - dlp_widget->setVisible(scene.driving_model_generation == cereal::ModelGeneration::ONE); + dlp_widget->setVisible(scene.custom_driving_model_valid && scene.driving_model_generation == 1); // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO // Gap Adjust Cruise gac_widget->updateGapAdjustCruise("LongitudinalPersonality"); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 126be0d6b1..b0ef6d4431 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -230,12 +230,6 @@ static void update_state(UIState *s) { scene.e2eX[i] = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getE2eX()[i]; } } - if (sm.updated("modelV2SP")) { - auto model_v2_sp = sm["modelV2SP"].getModelV2SP(); - scene.custom_driving_model_valid = model_v2_sp.getCustomModel(); - scene.driving_model_generation = model_v2_sp.getModelGeneration(); - scene.driving_model_capabilities = model_v2_sp.getModelCapabilities(); - } } void ui_update_params(UIState *s) { @@ -269,6 +263,8 @@ void ui_update_params(UIState *s) { s->scene.speed_limit_control_enabled = params.getBool("EnableSlc"); s->scene.feature_status_toggle = params.getBool("FeatureStatus"); s->scene.onroad_settings_toggle = params.getBool("OnroadSettings"); + s->scene.custom_driving_model_valid = params.getBool("CustomDrivingModel"); + s->scene.driving_model_generation = std::atoi(params.get("DrivingModelGeneration").c_str()); // Handle Onroad Screen Off params if (s->scene.onroadScreenOff > 0) { @@ -393,7 +389,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks", "longitudinalPlanSP", "liveMapDataSP", "carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", - "controlsStateSP", "modelV2SP" + "controlsStateSP" }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index a9fb6f0671..49a38a3e14 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -202,8 +202,7 @@ typedef struct UIScene { int speed_limit_warning_value_offset; bool custom_driving_model_valid; - cereal::ModelGeneration driving_model_generation; - uint32_t driving_model_capabilities; + int driving_model_generation; bool feature_status_toggle; bool onroad_settings_toggle; From 3cdbde6dbe5871796bade3e67031495267c860f8 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 13 Jul 2024 11:45:36 -0400 Subject: [PATCH 4/7] fix types --- selfdrive/modeld/custom_model_metadata.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 685d9655e4..783d11be22 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -48,14 +48,14 @@ class CustomModelMetadata: self.params: Params = params self.generation: ModelGeneration = self.read_model_generation_param() - self.capabilities: int = self.get_model_capabilities() + self.capabilities: ModelCapabilities = self.get_model_capabilities() self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: - return int(self.params.get('DrivingModelGeneration') or ModelGeneration.DEFAULT) + return ModelGeneration(int(self.params.get("DrivingModelGeneration", encoding='utf8')) or ModelGeneration.DEFAULT) - def get_model_capabilities(self) -> int: + def get_model_capabilities(self) -> ModelCapabilities: """Returns the model capabilities for a given generation.""" if self.generation == ModelGeneration.FIVE: return ModelCapabilities.DesiredCurvatureV2 From 1b419974b04fdb25e15b45cea38cf5cf7e090c72 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 13 Jul 2024 11:52:44 -0400 Subject: [PATCH 5/7] Revert "fix types" This reverts commit 3cdbde6dbe5871796bade3e67031495267c860f8. --- selfdrive/modeld/custom_model_metadata.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 783d11be22..685d9655e4 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -48,14 +48,14 @@ class CustomModelMetadata: self.params: Params = params self.generation: ModelGeneration = self.read_model_generation_param() - self.capabilities: ModelCapabilities = self.get_model_capabilities() + self.capabilities: int = self.get_model_capabilities() self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: - return ModelGeneration(int(self.params.get("DrivingModelGeneration", encoding='utf8')) or ModelGeneration.DEFAULT) + return int(self.params.get('DrivingModelGeneration') or ModelGeneration.DEFAULT) - def get_model_capabilities(self) -> ModelCapabilities: + def get_model_capabilities(self) -> int: """Returns the model capabilities for a given generation.""" if self.generation == ModelGeneration.FIVE: return ModelCapabilities.DesiredCurvatureV2 From 73994d4bb17dbb8e61c6feb1827192bae9ea293d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 13 Jul 2024 11:52:44 -0400 Subject: [PATCH 6/7] Revert "Driving Model Selector v5: Bug fixes and revert to param checks" This reverts commit 03fd6050f752fff00b3f7ce7ca31f7b8b919dc53. --- cereal/custom.capnp | 12 +++++++++++ selfdrive/modeld/custom_model_metadata.py | 24 ++++++++-------------- selfdrive/modeld/fill_model_msg.py | 8 +++----- selfdrive/modeld/modeld.py | 3 +++ selfdrive/ui/qt/onroad/annotated_camera.cc | 3 +-- selfdrive/ui/qt/onroad/annotated_camera.h | 3 +-- selfdrive/ui/qt/onroad/buttons.cc | 2 +- selfdrive/ui/qt/onroad_settings.cc | 4 ++-- selfdrive/ui/ui.cc | 10 ++++++--- selfdrive/ui/ui.h | 3 ++- 10 files changed, 41 insertions(+), 31 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3b36532ac9..8825cf6f07 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -25,6 +25,15 @@ enum AccelerationPersonality { stock @3; } +enum ModelGeneration { + default @0; + one @1; + two @2; + three @3; + four @4; + five @5; +} + struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; @@ -189,6 +198,9 @@ struct E2eLongStateSP @0xa5cd762cd951a455 { struct ModelDataV2SP @0xf98d843bfd7004a3 { laneChangePrev @0 :Bool; laneChangeEdgeBlock @1 :Bool; + customModel @2 :Bool; + modelGeneration @3 :ModelGeneration; + modelCapabilities @4 :UInt32; } struct CustomReserved7 @0xb86e6369214c01c8 { diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 685d9655e4..360f7046ad 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,18 +1,12 @@ -from enum import IntFlag, IntEnum +from enum import IntFlag import os +from cereal import custom from openpilot.common.params import Params SIMULATION = "SIMULATION" in os.environ - -class ModelGeneration(IntEnum): - DEFAULT = 0 - ONE = 1 - TWO = 2 - THREE = 3 - FOUR = 4 - FIVE = 5 +ModelGeneration = custom.ModelGeneration class ModelCapabilities(IntFlag): @@ -53,19 +47,19 @@ class CustomModelMetadata: self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: - return int(self.params.get('DrivingModelGeneration') or ModelGeneration.DEFAULT) + return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) def get_model_capabilities(self) -> int: """Returns the model capabilities for a given generation.""" - if self.generation == ModelGeneration.FIVE: + if self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.FOUR: + elif self.generation == ModelGeneration.four: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.THREE: + elif self.generation == ModelGeneration.three: return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.TWO: + elif self.generation == ModelGeneration.two: return ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.ONE: + elif self.generation == ModelGeneration.one: return ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO else: # Default model is meant to represent the capabilities of the prebuilt model diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 95099d27c3..39a161ed89 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -55,7 +55,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, timestamp_llk: int, model_execution_time: float, nav_enabled: bool, valid: bool, - custom_model_valid: bool, custom_model_capabilities: int) -> None: + custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 frame_drop_perc = frame_drop * 100 extended_msg.valid = valid @@ -68,9 +68,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc action = driving_model_data.action - model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution - if not model_use_lateral_planner: - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -102,7 +100,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - if model_use_lateral_planner: + if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution: solution = modelV2.lateralPlannerSolutionDEPRECATED solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index b6880d4d8d..ff3d9ec633 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -402,6 +402,9 @@ def main(demo=False): if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): modelV2SP.laneChangePrev = DH.prev_lane_change modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED + modelV2SP.customModel = custom_model_metadata.valid + modelV2SP.modelGeneration = custom_model_metadata.generation + modelV2SP.modelCapabilities = int(custom_model_metadata.capabilities) pm.send('modelV2SP', modelv2_sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 7e27449dc5..009ff76138 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -375,7 +375,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { featureStatusToggle = s.scene.feature_status_toggle; experimental_btn->setVisible(!(showDebugUI && showVTC)); - customModelValid = s.scene.custom_driving_model_valid; drivingModelGen = s.scene.driving_model_generation; } @@ -1210,7 +1209,7 @@ void AnnotatedCameraWidget::drawFeatureStatusText(QPainter &p, int x, int y) { } // Dynamic Lane Profile - if (customModelValid && drivingModelGen == 1) { // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + if (drivingModelGen == cereal::ModelGeneration::ONE) { drawFeatureStatusElement(dynamicLaneProfile, feature_text.dlp_list_text, feature_color.dlp_list_color, true, "OFF", "DLP"); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 5c3697aaf3..29b9c4833b 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -203,8 +203,7 @@ private: bool featureStatusToggle; - bool customModelValid; - int drivingModelGen; + cereal::ModelGeneration drivingModelGen; protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 57de6c12ee..fbca42da7b 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -99,7 +99,7 @@ void OnroadSettingsButton::paintEvent(QPaintEvent *event) { void OnroadSettingsButton::updateState(const UIState &s) { const auto cp = (*s.sm)["carParams"].getCarParams(); - auto dlp_enabled = s.scene.custom_driving_model_valid && s.scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + auto dlp_enabled = s.scene.driving_model_generation == cereal::ModelGeneration::ONE; bool allow_btn = s.scene.onroad_settings_toggle && (dlp_enabled || hasLongitudinalControl(cp) || !cp.getPcmCruiseSpeed()); setVisible(allow_btn); diff --git a/selfdrive/ui/qt/onroad_settings.cc b/selfdrive/ui/qt/onroad_settings.cc index d39d908662..00810294ba 100644 --- a/selfdrive/ui/qt/onroad_settings.cc +++ b/selfdrive/ui/qt/onroad_settings.cc @@ -119,7 +119,7 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent) void OnroadSettings::changeDynamicLaneProfile() { UIScene &scene = uiState()->scene; - bool can_change = scene.custom_driving_model_valid && scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + bool can_change = scene.driving_model_generation == cereal::ModelGeneration::ONE; if (can_change) { scene.dynamic_lane_profile++; scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile; @@ -229,7 +229,7 @@ void OnroadSettings::refresh() { // Dynamic Lane Profile dlp_widget->updateDynamicLaneProfile("DynamicLaneProfile"); - dlp_widget->setVisible(scene.custom_driving_model_valid && scene.driving_model_generation == 1); // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + dlp_widget->setVisible(scene.driving_model_generation == cereal::ModelGeneration::ONE); // Gap Adjust Cruise gac_widget->updateGapAdjustCruise("LongitudinalPersonality"); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b0ef6d4431..126be0d6b1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -230,6 +230,12 @@ static void update_state(UIState *s) { scene.e2eX[i] = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getE2eX()[i]; } } + if (sm.updated("modelV2SP")) { + auto model_v2_sp = sm["modelV2SP"].getModelV2SP(); + scene.custom_driving_model_valid = model_v2_sp.getCustomModel(); + scene.driving_model_generation = model_v2_sp.getModelGeneration(); + scene.driving_model_capabilities = model_v2_sp.getModelCapabilities(); + } } void ui_update_params(UIState *s) { @@ -263,8 +269,6 @@ void ui_update_params(UIState *s) { s->scene.speed_limit_control_enabled = params.getBool("EnableSlc"); s->scene.feature_status_toggle = params.getBool("FeatureStatus"); s->scene.onroad_settings_toggle = params.getBool("OnroadSettings"); - s->scene.custom_driving_model_valid = params.getBool("CustomDrivingModel"); - s->scene.driving_model_generation = std::atoi(params.get("DrivingModelGeneration").c_str()); // Handle Onroad Screen Off params if (s->scene.onroadScreenOff > 0) { @@ -389,7 +393,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks", "longitudinalPlanSP", "liveMapDataSP", "carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", - "controlsStateSP" + "controlsStateSP", "modelV2SP" }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 49a38a3e14..a9fb6f0671 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -202,7 +202,8 @@ typedef struct UIScene { int speed_limit_warning_value_offset; bool custom_driving_model_valid; - int driving_model_generation; + cereal::ModelGeneration driving_model_generation; + uint32_t driving_model_capabilities; bool feature_status_toggle; bool onroad_settings_toggle; From 5c90b31f721617608697b697942c354258a82f73 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 13 Jul 2024 11:57:06 -0400 Subject: [PATCH 7/7] Driving Model Selector v5: Bug fixes --- selfdrive/modeld/custom_model_metadata.py | 4 ++-- selfdrive/modeld/fill_model_msg.py | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 360f7046ad..0469bd189c 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -42,14 +42,14 @@ class CustomModelMetadata: self.params: Params = params self.generation: ModelGeneration = self.read_model_generation_param() - self.capabilities: int = self.get_model_capabilities() + self.capabilities: ModelCapabilities = self.get_model_capabilities() self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) - def get_model_capabilities(self) -> int: + def get_model_capabilities(self) -> ModelCapabilities: """Returns the model capabilities for a given generation.""" if self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 39a161ed89..8d2bcc0ef4 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -68,7 +68,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution + if not model_use_lateral_planner: + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -100,7 +102,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution: + if model_use_lateral_planner: solution = modelV2.lateralPlannerSolutionDEPRECATED solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]