From 0570833ad3ee4a3d13b77e357cab50c2430fefd5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 28 Sep 2024 13:31:48 -0400 Subject: [PATCH] temp: build tf --- common/params.cc | 1 - selfdrive/modeld/constants.py | 20 +++++------ selfdrive/modeld/custom_model_metadata.py | 41 +++-------------------- selfdrive/modeld/fill_model_msg.py | 25 +++++--------- selfdrive/modeld/modeld.py | 2 +- system/manager/manager.py | 2 -- 6 files changed, 24 insertions(+), 67 deletions(-) diff --git a/common/params.cc b/common/params.cc index 4ce6afe57d..7cd3664818 100644 --- a/common/params.cc +++ b/common/params.cc @@ -239,7 +239,6 @@ std::unordered_map keys = { {"DrivingModelGeneration", PERSISTENT}, {"DrivingModelMetadataText", PERSISTENT}, {"DrivingModelName", PERSISTENT}, - {"DrivingModelSelectorVersion", CLEAR_ON_MANAGER_START}, {"DrivingModelText", PERSISTENT}, {"DrivingModelUrl", PERSISTENT}, {"DynamicExperimentalControl", PERSISTENT | BACKUP}, diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 538472ca81..0ac513a9e9 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -76,14 +76,14 @@ class Plan: class Meta: ENGAGED = slice(0, 1) # next 2, 4, 6, 8, 10 seconds - GAS_DISENGAGE = slice(1, 31, 6) - BRAKE_DISENGAGE = slice(2, 31, 6) - STEER_OVERRIDE = slice(3, 31, 6) - HARD_BRAKE_3 = slice(4, 31, 6) - HARD_BRAKE_4 = slice(5, 31, 6) - HARD_BRAKE_5 = slice(6, 31, 6) + GAS_DISENGAGE = slice(1, 41, 8) + BRAKE_DISENGAGE = slice(2, 41, 8) + STEER_OVERRIDE = slice(3, 41, 8) + HARD_BRAKE_3 = slice(4, 41, 8) + HARD_BRAKE_4 = slice(5, 41, 8) + HARD_BRAKE_5 = slice(6, 41, 8) + GAS_PRESS = slice(7, 41, 8) + BRAKE_PRESS = slice(8, 41, 8) # next 0, 2, 4, 6, 8, 10 seconds - GAS_PRESS = slice(31, 55, 4) - BRAKE_PRESS = slice(32, 55, 4) - LEFT_BLINKER = slice(33, 55, 4) - RIGHT_BLINKER = slice(34, 55, 4) + LEFT_BLINKER = slice(41, 53, 2) + RIGHT_BLINKER = slice(42, 53, 2) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 7a3eb81512..d0e355fcc3 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,10 +1,9 @@ from enum import IntFlag -import os, sys +import os from cereal import custom from openpilot.common.params import Params -DRIVING_MODEL_SELECTOR_VERSION = 6 SIMULATION = "SIMULATION" in os.environ ModelGeneration = custom.ModelGeneration @@ -34,9 +33,7 @@ class ModelCapabilities(IntFlag): In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output. """ - PlanTemporalPose = 2 ** 5 - - ModelOutputSlicesV1 = 2 ** 6 + TemporalPoseV1 = 2 ** 5 class CustomModelMetadata: @@ -50,18 +47,14 @@ class CustomModelMetadata: self.capabilities: ModelCapabilities = self.get_model_capabilities() self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ self.capabilities != ModelCapabilities.Default - self.initialized = False def read_model_generation_param(self) -> ModelGeneration: return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) def get_model_capabilities(self) -> ModelCapabilities: """Returns the model capabilities for a given generation.""" - if self.generation == ModelGeneration.seven: - return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanTemporalPose | \ - ModelCapabilities.ModelOutputSlicesV1 - elif self.generation == ModelGeneration.six: - return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanTemporalPose + if self.generation == ModelGeneration.six: + return ModelCapabilities.TemporalPose elif self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 elif self.generation == ModelGeneration.four: @@ -75,29 +68,3 @@ class CustomModelMetadata: else: # Default model is meant to represent the capabilities of the prebuilt model return ModelCapabilities.Default - - def custom_meta(self): - if not self.initialized: - if self.capabilities & ModelCapabilities.ModelOutputSlicesV1: - class Meta: - ENGAGED = slice(0, 1) - # next 2, 4, 6, 8, 10 seconds - GAS_DISENGAGE = slice(1, 41, 8) - BRAKE_DISENGAGE = slice(2, 41, 8) - STEER_OVERRIDE = slice(3, 41, 8) - HARD_BRAKE_3 = slice(4, 41, 8) - HARD_BRAKE_4 = slice(5, 41, 8) - HARD_BRAKE_5 = slice(6, 41, 8) - GAS_PRESS = slice(7, 41, 8) - BRAKE_PRESS = slice(8, 41, 8) - # next 0, 2, 4, 6, 8, 10 seconds - LEFT_BLINKER = slice(41, 53, 2) - RIGHT_BLINKER = slice(42, 53, 2) - - module = 'openpilot.selfdrive.modeld.constants' - if module in sys.modules: - sys.modules[module].Meta = Meta - else: - raise ImportError(f"The module '{module}' is not imported yet or does not exist.") - - self.initialized = True diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 92a419eed5..f66adfff1c 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,7 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta -from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities +from openpilot.selfdrive.modeld.custom_model_metadata import ModelCapabilities SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -55,8 +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: CustomModelMetadata) -> None: - custom_model.custom_meta() + 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 @@ -70,7 +69,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.modelExecutionTime = model_execution_time action = driving_model_data.action - model_use_lateral_planner = custom_model.valid and custom_model.capabilities & ModelCapabilities.LateralPlannerSolution + 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]) @@ -80,7 +79,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.frameAge = frame_age modelV2.frameDropPerc = frame_drop_perc modelV2.timestampEof = timestamp_eof - model_use_nav = custom_model.valid and custom_model.capabilities & ModelCapabilities.NoO + model_use_nav = custom_model_valid and custom_model_capabilities & ModelCapabilities.NoO if model_use_nav: modelV2.locationMonoTimeDEPRECATED = timestamp_llk modelV2.modelExecutionTime = model_execution_time @@ -187,17 +186,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # temporal pose temporal_pose = modelV2.temporalPose - if custom_model.valid: - if custom_model.capabilities & ModelCapabilities.PlanTemporalPose: - temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist() - temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() - temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist() - temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist() - else: - temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist() - temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist() - temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist() - temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist() + if custom_model_valid and custom_model_capabilities & ModelCapabilities.TemporalPoseV1: + temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist() + temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist() + temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist() + temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist() else: temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist() temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index b1f1b3c4e0..bb883c32cb 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -377,7 +377,7 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen, - custom_model_metadata) + custom_model_metadata.valid, custom_model_metadata.capabilities) if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): desire_state = modelv2_send.modelV2.meta.desireState diff --git a/system/manager/manager.py b/system/manager/manager.py index daa91cab40..34d78923a1 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -11,7 +11,6 @@ import openpilot.system.sentry as sentry from openpilot.common.api.sunnylink import UNREGISTERED_SUNNYLINK_DONGLE_ID from openpilot.common.params import Params, ParamKeyType from openpilot.common.text_window import TextWindow -from openpilot.selfdrive.modeld.custom_model_metadata import DRIVING_MODEL_SELECTOR_VERSION from openpilot.system.hardware import HARDWARE, PC from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog from openpilot.system.manager.mapd_installer import VERSION @@ -65,7 +64,6 @@ def manager_init() -> None: ("DisableOnroadUploads", "0"), ("DisengageLateralOnBrake", "0"), ("DrivingModelGeneration", "0"), - ("DrivingModelSelectorVersion", str(DRIVING_MODEL_SELECTOR_VERSION)), ("DynamicLaneProfile", "1"), ("DynamicPersonality", "0"), ("EnableMads", "1"),