From d4b79ff36a7b047c8350fa221b03f79c9006aec2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 28 Sep 2024 13:38:58 -0400 Subject: [PATCH] Revert "temp: build tf" This reverts commit 0570833ad3ee4a3d13b77e357cab50c2430fefd5. --- 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, 67 insertions(+), 24 deletions(-) diff --git a/common/params.cc b/common/params.cc index 7cd3664818..4ce6afe57d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -239,6 +239,7 @@ 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 0ac513a9e9..538472ca81 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, 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) + 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) # next 0, 2, 4, 6, 8, 10 seconds - LEFT_BLINKER = slice(41, 53, 2) - RIGHT_BLINKER = slice(42, 53, 2) + GAS_PRESS = slice(31, 55, 4) + BRAKE_PRESS = slice(32, 55, 4) + LEFT_BLINKER = slice(33, 55, 4) + RIGHT_BLINKER = slice(34, 55, 4) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index d0e355fcc3..7a3eb81512 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,9 +1,10 @@ from enum import IntFlag -import os +import os, sys from cereal import custom from openpilot.common.params import Params +DRIVING_MODEL_SELECTOR_VERSION = 6 SIMULATION = "SIMULATION" in os.environ ModelGeneration = custom.ModelGeneration @@ -33,7 +34,9 @@ class ModelCapabilities(IntFlag): In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output. """ - TemporalPoseV1 = 2 ** 5 + PlanTemporalPose = 2 ** 5 + + ModelOutputSlicesV1 = 2 ** 6 class CustomModelMetadata: @@ -47,14 +50,18 @@ 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.six: - return ModelCapabilities.TemporalPose + if self.generation == ModelGeneration.seven: + return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanTemporalPose | \ + ModelCapabilities.ModelOutputSlicesV1 + elif self.generation == ModelGeneration.six: + return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanTemporalPose elif self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 elif self.generation == ModelGeneration.four: @@ -68,3 +75,29 @@ 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 f66adfff1c..92a419eed5 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 ModelCapabilities +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -55,7 +55,8 @@ 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: CustomModelMetadata) -> None: + custom_model.custom_meta() 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 @@ -69,7 +70,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]) @@ -79,7 +80,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 @@ -186,11 +187,17 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # temporal pose temporal_pose = modelV2.temporalPose - 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() + 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() 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 bb883c32cb..b1f1b3c4e0 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.valid, custom_model_metadata.capabilities) + custom_model_metadata) 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 34d78923a1..daa91cab40 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -11,6 +11,7 @@ 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 @@ -64,6 +65,7 @@ def manager_init() -> None: ("DisableOnroadUploads", "0"), ("DisengageLateralOnBrake", "0"), ("DrivingModelGeneration", "0"), + ("DrivingModelSelectorVersion", str(DRIVING_MODEL_SELECTOR_VERSION)), ("DynamicLaneProfile", "1"), ("DynamicPersonality", "0"), ("EnableMads", "1"),