diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5d75d0a9a6..92353eb309 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -33,6 +33,7 @@ enum ModelGeneration { four @4; five @5; six @6; + seven @7; } struct ControlsStateSP @0x81c2f05a394cf4af { 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..916a569010 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,5 +1,5 @@ from enum import IntFlag -import os +import os, sys from cereal import custom from openpilot.common.params import Params @@ -33,7 +33,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: @@ -53,8 +55,11 @@ class CustomModelMetadata: 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 +73,22 @@ class CustomModelMetadata: else: # Default model is meant to represent the capabilities of the prebuilt model return ModelCapabilities.Default + + def custom_meta(self): + 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) + + sys.modules['constants'].Meta = Meta diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index f66adfff1c..e6bf69ac95 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -186,11 +186,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['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() + 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['plan'][0,0,Plan.VELOCITY].tolist() temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 2e1a60a23e..f8002d5a91 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4 -size 51453312 +oid sha256:0dab94dc94db5e372f56d26f9dda98bd31fd76e4c16027da611132263848d0c1 +size 62486347