From 058de20c46375a0fb825121e7a286fb98ab12585 Mon Sep 17 00:00:00 2001 From: Yassine Date: Thu, 5 Oct 2023 15:19:40 -0700 Subject: [PATCH] 1b4308b7-a659-4ebd-b4c6-c81c1c3890f8/700 --- selfdrive/modeld/modeld.py | 17 +++++++++++++++-- selfdrive/modeld/models/driving.h | 2 ++ selfdrive/modeld/models/driving.pxd | 2 ++ selfdrive/modeld/models/driving_pyx.pyx | 3 +++ selfdrive/modeld/models/supercombo.onnx | 4 ++-- 5 files changed, 24 insertions(+), 4 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 3c416ce939..3704035eda 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -9,15 +9,19 @@ from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.system.swaglog import cloudlog from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.common.numpy_fast import interp from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime +from openpilot.selfdrive.modeld.constants import T_IDXS +from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext from openpilot.selfdrive.modeld.models.driving_pyx import ( PublishState, create_model_msg, create_pose_msg, FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN, - OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ) + LAT_PLANNER_STATE_LEN, SIM_SPEED_LEN, OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ) MODEL_PATHS = { ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed', @@ -48,6 +52,8 @@ class ModelState: self.inputs = { 'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32), 'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32), + 'lat_planner_state': np.zeros(LAT_PLANNER_STATE_LEN, dtype=np.float32), + 'sim_speed': np.zeros(SIM_SPEED_LEN, dtype=np.float32), 'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32), 'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32), 'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32), @@ -70,6 +76,7 @@ class ModelState: self.inputs['traffic_convention'][:] = inputs['traffic_convention'] self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] + self.inputs['sim_speed'][:] = inputs['sim_speed'] # self.inputs['driving_style'][:] = inputs['driving_style'] # if getCLBuffer is not None, frame will be None @@ -82,7 +89,12 @@ class ModelState: self.model.execute() self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:] + # TODO: self.output should be cast to the modeld output struct then these 2 tricky slicings can be removed self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN] + lat_planning_output = self.output[OUTPUT_SIZE-2*LAT_PLANNER_STATE_LEN*TRAJECTORY_SIZE:OUTPUT_SIZE-LAT_PLANNER_STATE_LEN*TRAJECTORY_SIZE] + lat_planning_output = lat_planning_output.reshape((TRAJECTORY_SIZE, LAT_PLANNER_STATE_LEN)) + self.inputs['lat_planner_state'][2] = interp(DT_MDL, T_IDXS, lat_planning_output[:, 2]) + self.inputs['lat_planner_state'][3] = interp(DT_MDL, T_IDXS, lat_planning_output[:, 3]) return self.output @@ -120,7 +132,7 @@ def main(): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"]) + sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carState"]) state = PublishState() params = Params() @@ -234,6 +246,7 @@ def main(): inputs:Dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, + 'sim_speed': sm['carState'].vEgo, 'driving_style': driving_style, 'nav_features': nav_features, 'nav_instructions': nav_instructions} diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index ebdd205ea9..9c502d066f 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -14,6 +14,8 @@ constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int LAT_PLANNER_STATE_LEN = 4; +constexpr int SIM_SPEED_LEN = 1; constexpr int NAV_FEATURE_LEN = 256; constexpr int NAV_INSTRUCTION_LEN = 150; constexpr int DRIVING_STYLE_LEN = 12; diff --git a/selfdrive/modeld/models/driving.pxd b/selfdrive/modeld/models/driving.pxd index 8d6b8d755e..d35fc63d8b 100644 --- a/selfdrive/modeld/models/driving.pxd +++ b/selfdrive/modeld/models/driving.pxd @@ -13,6 +13,8 @@ cdef extern from "selfdrive/modeld/models/driving.h": cdef int HISTORY_BUFFER_LEN cdef int DESIRE_LEN cdef int TRAFFIC_CONVENTION_LEN + cdef int LAT_PLANNER_STATE_LEN + cdef int SIM_SPEED_LEN cdef int DRIVING_STYLE_LEN cdef int NAV_FEATURE_LEN cdef int NAV_INSTRUCTION_LEN diff --git a/selfdrive/modeld/models/driving_pyx.pyx b/selfdrive/modeld/models/driving_pyx.pyx index b98a8f3ff1..453f928adc 100644 --- a/selfdrive/modeld/models/driving_pyx.pyx +++ b/selfdrive/modeld/models/driving_pyx.pyx @@ -11,6 +11,7 @@ from .commonmodel cimport mat3 from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \ TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \ NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \ + LAT_PLANNER_STATE_LEN as CPP_LAT_PLANNER_STATE_LEN, SIM_SPEED_LEN as CPP_SIM_SPEED_LEN, \ OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ from .driving cimport MessageBuilder, PublishState as cppPublishState from .driving cimport fill_model_msg, fill_pose_msg @@ -19,6 +20,8 @@ FEATURE_LEN = CPP_FEATURE_LEN HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN DESIRE_LEN = CPP_DESIRE_LEN TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN +LAT_PLANNER_STATE_LEN = CPP_LAT_PLANNER_STATE_LEN +SIM_SPEED_LEN = CPP_SIM_SPEED_LEN DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 5ac790b715..81101ef446 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:e448b7d38b9e8536525de3addc62cf945277a743af39261fc19f23bd077f9404 -size 52936875 +oid sha256:4606e4ff971dcd0de771ba4e90ea4857f87508c8a26e4b252a56f9ef2839920a +size 52939645