mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-08-08 09:45:50 +08:00
model: Refactor modeld with modular runners and split model support (#877)
* Refactor model runner methods for improved abstraction.
Moved slicing logic to a private `_slice_outputs` method and decoupled `_run_model` for clearer subclass implementation. Removed redundant `output` attribute in `ModelState` to streamline data handling.
* Add output parsing to model_runner and remove duplicate logic
Integrates an output parser directly into `model_runner` for streamlined inference and parsing. Removes redundant parser initialization from `modeld` to avoid duplication and enhance maintainability.
* Reordering
* linter
* linter
* Refactor model handling with `ModelData` abstraction
Introduce a `ModelData` class to encapsulate model and metadata logic, improving code clarity and modularity. Refactor `ModelRunner` to manage multiple models and add conditional handling for fallback scenarios. Adjust `TinygradRunner` to validate and use the new `ModelData` structure.
* Refactor model handling to use dictionary-based structure
Replaces the model list with a dictionary keyed by model type to improve clarity and maintainability. Updates related logic and ensures consistent handling of model metadata and inputs. Adds `slice_outputs` implementation to the `TinygradRunner` for proper output parsing.
* Refactor model runners to support policy and vision separation
Introduced `TinygradPolicyRunner`, `TinygradVisionRunner`, and `TinygradSplitRunner` to enable separate handling of policy and vision models. Updated `TinygradRunner` initialization and input preparation to accommodate modular processing. Adjusted `modeld` to utilize the new runners, ensuring compatibility with separated model workflows.
* Refactor model runner initialization and simplify logic
Introduce `get_model_runner` to centralize model runner selection logic, replacing multiple conditional instantiations. Simplify the handling of model metadata by removing fallback logic and restructuring output slicing to enforce proper loading of model data. These changes improve code maintainability and clarity.
* Refactor model data access for TinygradRunner initialization
Update references to access nested artifact properties and align with structural changes to the model data schema. This simplifies input shapes handling and ensures compatibility with updated model attributes.
* Refactor imports and clean up redundant code.
Removed unused imports and improved formatting for clarity and maintainability. These changes simplify the codebase by eliminating unnecessary dependencies and ensuring consistency.
* Refactor model output parsing with specialized parsers.
Introduce abstract `_parse_outputs` method to standardize parsing logic. Add `SplitParser` for specialized parsing in `TinygradVisionRunner` and `TinygradPolicyRunner`. This improves modularity and paves the way for easier parser customization.
* Add parser for model output processing in modeld_v2
Introduce a new `Parser` class to handle parsing and processing of model outputs, including MDN, binary cross-entropy, and categorical cross-entropy outputs. This modularizes the logic, improves clarity, and prepares for handling various types of model data.
* Add `input_shapes` property to model runners
Introduce a new `input_shapes` property in the abstract base class and its implementation in derived classes. This provides a standardized way to access the input shapes of models, improving clarity and consistency in the model runners.
* Refactor model runner to use private `_model_data` attribute
Replaced public `model_data` with private `_model_data` for improved encapsulation. Updated all references and property accessors accordingly. Simplified model type handling by using raw types where applicable.
* Remove debug print statement from model_runner.py
The unnecessary `print(model_type)` statement was removed as it served no functional purpose in the code. This improves code cleanliness and avoids unintended console output during execution.
* Refactor `_parse_outputs` call in `run_model`.
Replaced the use of `self.parser.parse_outputs` with `self._parse_outputs` for clarity and consistency. Updated method signature to align with the revised usage.
* Refactor model output parsing for clarity and scope separation
Moved specific parsing logic (e.g., lane_lines, lead) from `parse_model_outputs` to `parse_policy_outputs` to better align with functional responsibilities. This improves modularity and readability while maintaining existing functionality.
* Refactor model_runner to simplify result handling
Renamed variable `result` to `parsed_result` for clarity and removed unnecessary slicing during model output parsing. These changes improve code readability and maintain consistency within the `run_model` method.
* Adjust _parse_outputs method signature in model_runner
Updated the method signature of _parse_outputs to accept a single np.ndarray instead of a dictionary. This aligns with the intended data structure and ensures consistency across subclasses implementing this abstract method.
* Refactor ModelRunner to enforce abstract base class compliance
Updated `ModelRunner` and its subclasses to properly inherit from `ABC` while refactoring methods to ensure compliance with Python's abstract base class standards. Streamlined the handling of `_parse_outputs` and added a new `input_shapes` property for improved functionality.
* Fix buffer length issue in 20Hz model initialization
Adjusted `FULL_HISTORY_BUFFER_LEN` by adding +1 for `full_features_20Hz` to address compatibility issues with the current FoF model. Added a comment noting potential failure for other models with this adjustment.
* Refactor TinygradRunner to remove abstract methods.
Simplified the TinygradRunner class by removing unnecessary @abstractmethod decorators and redundant method definitions. This streamlines the code and aligns it more effectively with its current usage and implementation.
* Refactor model runner classes and enhance type annotations
Simplified model runner implementations, added type annotations, and improved code readability and maintainability. Introduced new type definitions, updated metadata handling, and standardized input/output parsing across all runner classes. Minor comment update in `modeld.py` for clarity.
* Refactor model runner classes with detailed docstrings.
Enhanced class and function docstrings across model_runner.py for better clarity and maintainability. Descriptions now include detailed explanations of attributes, purposes, and workflows to aid understanding and future development.
* Refactor model runner classes and add TICI hardware optimization
Simplified and clarified class definitions, comments, and functionality for ModelRunner subclasses. Introduced the use of QCOM environment variable on TICI for potential hardware acceleration. Enhanced input/output handling and error reporting across Tinygrad and ONNX implementations.
* Update parser import and usage to use CombinedParser
Replaced the Parser class with CombinedParser in model_runner.py. This change ensures consistency with the updated parsing logic, aligning with the latest requirements for combined model output handling.
* Refactor TinygradRunner hierarchy for modular parsers
Reorganized the TinygradRunner and its specialized runners (Vision, Policy, and Supercombo) into a cleaner, modular structure using composable classes. This consolidates parser logic, removes redundancy, and simplifies initialization by leveraging a shared base class with a dictionary-based parser method.
* Refactor model runners to use ModularRunner as abstract base.
Introduce a new `ModularRunner` class to enforce a consistent interface across model runners. Updated existing runners, including `ModelRunner`, `SupercomboTinygrad`, `PolicyTinygrad`, and `VisionTinygrad`, to extend `ModularRunner`. Added abstract methods and properties to enhance modularity and code maintainability.
* Refactor model runners into modular components.
This commit separates the logic for Tinygrad, ONNX, and split runners into clearly defined modules and components. It introduces `PolicyTinygrad`, `VisionTinygrad`, `SupercomboTinygrad`, and centralized helpers for cleaner architecture. The changes improve modularity and maintainability of the model running and parsing workflows.
* Simplify imports and clean up unused code in ONNXRunner.
Removed unused imports and redundant environmental variables to streamline the codebase. Consolidated necessary imports and organized type definitions for improved readability and maintenance.
* Standardize imports and add model data validation.
Updated import paths to ensure consistency across modules by using `openpilot` as the base. Introduced validation in `_parse_outputs` methods to handle cases where `_model_data` is not initialized, preventing potential runtime errors.
* Remove unused import and fix whitespace in runners
The unused import `ModelData` was removed from `tinygrad_runner.py` to clean up the code. Additionally, extraneous whitespace was corrected in `onnx_runner.py` for improved readability and consistency.
* Remove unnecessary blank line in import statements
Cleaned up import section by removing an extra blank line. This helps maintain consistency and adheres to code style conventions.
* BROKEN!! Staging code but its not gonna work. Also I realized we need to run the split models in 2 stages because the output of one is immediately needed for the input of the other. We might handle it inside of the model_runner instead
* update smooth
* Revert "update smooth"
This reverts commit c335712e6e1ee189459ce34dfc9d4028feb9470f.
* match case made this very hard to read.
* shouldnt be there
* Refactor to allow TR (soon TM)
* TR 7 is .1
* metadata
* .2=3
* Remove redundant comments and clean up conditional blocks in modeld.py.
* Undoing wrong buffer
* Refactor model initialization and adjust ONNX runner import
Reorganized numpy input buffer initialization and updated `temporal_idxs` logic for better clarity and efficiency. Conditional import for ONNXRunner added for non-TICI platforms to optimize imports. These changes improve maintainability and compatibility across platforms.
* Update CURRENT_SELECTOR_VERSION to 4
Bump the CURRENT_SELECTOR_VERSION constant from 3 to 4 to reflect changes in the selector logic or requirements. This ensures compatibility with the updated selector version while maintaining the minimum required version as 2.
* Add output_slices property to model runners
Introduce output_slices property to provide access to the output slices for individual and combined models. This ensures consistent handling of output slices across vision and policy models, improving modularity and usability.
* Refactor imports to use SplitModelConstants consistently
Updated import references to use the renamed `SplitModelConstants` class for consistency across files. This change ensures clarity and better alignment with the updated class naming convention.
* Refactor buffer initialization and desired curvature handling
Refactored model input buffer initialization for improved clarity and consistency, leveraging dynamic shape calculations. Extracted `process_desired_curvature` method to encapsulate logic for handling 3D and non-3D cases. Simplified temporal index generation and related calculations for better maintainability.
* Refactor desire reshape dims logic in modeld.py
Adjust logic for setting desire reshape dimensions to handle `is_20hz_3d` separately. This improves clarity and ensures proper handling of different model runner configurations.
* Fix off-by-one error in full_desire buffer initialization
The full_desire buffer length was mistakenly set to full_history_buffer_len + 1. This change corrects it to match the intended full_history_buffer_len, ensuring proper alignment with other buffers.
* Simplify desire reshape logic by removing unused condition.
Removed the `is_20hz_3d` condition and associated reshape logic since it is no longer needed. This streamlines the code and avoids unnecessary checks for unused configurations.
* Refactor buffer initialization for 20Hz model variants
Reorganized buffer initialization logic to prioritize the 20Hz_3D condition. This improves clarity and ensures specific handling of different 20Hz configurations. Adjusted the order of conditions to streamline execution flow.
* Refactor: Move `get_action_from_model` and constants to `Model` class to improve encapsulation and readability.
* 12 line reshaping red diff
* .2 to match old models delay
* mypy fixes
* Revert "12 line reshaping red diff"
This reverts commit 8c7280f629043f0485749e2536a20af74c9209b2.
* mypy
* remove this
* Fix desired curvature for models which do not output desired_curvature
* fix FoF
* flip policy and vision outs to allow FoF and tomb raider to live in harmony using conditional `if 'this' in outs:'`
* noqa
* single
* sunnypilot modeld.py
* action
* overrides methodology
* combine split outputs to its own method
* comments
* Fix static checker line length
* static will fail on line length
lines:
286,
206,
70 - 77,
159,
168
* Address E501 line length violations
* This will make TR better while not effecting FoF/VFF at all
* Reduce this to one conditional and just call normally in vision/policy
* Align with upstream in our own way.
* check for desired curvature in outputs first
* outputs
* Use a cleaner import method
* Fix output
* Clean up some values
* Only call on init
* slight cleanup
* names!!!!!!!!!
* Refactor overrides structure to support key-value pairs.
The overrides structure now uses a list of key-value pairs instead of fixed lat/long fields. This change improves flexibility, allowing dynamic addition of override parameters. Code adjustments ensure backward compatibility and consistent behavior throughout the application.
* Refactor: Use local variable for SplitModelConstants
Introduce a local `constants` variable to replace repeated access to `SplitModelConstants`. This simplifies code readability and adheres to linter recommendations for line length.
* Refactor model constant handling to improve modularity
Replaced direct usage of model constants with dynamic access through model runners for better scalability and maintainability. This change centralizes constant definitions, reduces redundancy, and ensures clearer integration with different model types.
---------
Co-authored-by: discountchubbs <alexgrant990@gmail.com>
Co-authored-by: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com>
Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
DevTekVE
+7
-1
@@ -78,6 +78,11 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
stock @2;
|
||||
}
|
||||
|
||||
struct Override {
|
||||
key @0 :Text;
|
||||
value @1 :Text;
|
||||
}
|
||||
|
||||
struct ModelBundle {
|
||||
index @0 :UInt32;
|
||||
internalName @1 :Text;
|
||||
@@ -88,8 +93,9 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
environment @6 :Text;
|
||||
runner @7 :Runner;
|
||||
is20hz @8 :Bool;
|
||||
ref @9 :Text; # New field
|
||||
ref @9 :Text;
|
||||
minimumSelectorVersion @10 :UInt32;
|
||||
overrides @11 :List(Override);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -178,10 +178,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
|
||||
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
|
||||
if not self.is_stock:
|
||||
# To support non Tomb Raider models
|
||||
output_a_target, self.output_should_stop = output_a_target_mpc, output_should_stop_mpc
|
||||
|
||||
for idx in range(2):
|
||||
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
|
||||
self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
import numpy as np
|
||||
|
||||
def index_function(idx, max_val=192, max_idx=32):
|
||||
return (max_val) * ((idx/max_idx)**2)
|
||||
return max_val * ((idx/max_idx)**2)
|
||||
|
||||
|
||||
class ModelConstants:
|
||||
# time and distance indices
|
||||
@@ -63,6 +64,7 @@ class ModelConstants:
|
||||
|
||||
POLY_PATH_DEGREE = 4
|
||||
|
||||
|
||||
# model outputs slices
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
@@ -71,6 +73,7 @@ class Plan:
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
|
||||
class Meta:
|
||||
ENGAGED = slice(0, 1)
|
||||
# next 2, 4, 6, 8, 10 seconds
|
||||
|
||||
@@ -50,7 +50,7 @@ def fill_xyz_poly(builder, degree, x, y, z):
|
||||
builder.zCoefficients = coeffs[:, 2].tolist()
|
||||
|
||||
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
|
||||
net_output_data: dict[str, np.ndarray], publish_state: PublishState,
|
||||
net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState,
|
||||
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
|
||||
timestamp_eof: int, model_execution_time: float, valid: bool,
|
||||
v_ego: float, steer_delay: float, meta_const) -> None:
|
||||
@@ -76,8 +76,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
driving_model_data.frameDropPerc = frame_drop_perc
|
||||
driving_model_data.modelExecutionTime = model_execution_time
|
||||
|
||||
action = driving_model_data.action
|
||||
action.desiredCurvature = desired_curvature
|
||||
# Populate drivingModelData.action
|
||||
driving_model_data_action = driving_model_data.action
|
||||
driving_model_data_action.desiredAcceleration = action.desiredAcceleration
|
||||
driving_model_data_action.shouldStop = action.shouldStop
|
||||
driving_model_data_action.desiredCurvature = desired_curvature
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
@@ -111,8 +114,10 @@ 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
|
||||
action = modelV2.action
|
||||
action.desiredCurvature = desired_curvature
|
||||
modelV2_action = modelV2.action
|
||||
modelV2_action.desiredAcceleration = action.desiredAcceleration
|
||||
modelV2_action.shouldStop = action.shouldStop
|
||||
modelV2_action.desiredCurvature = desired_curvature
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
|
||||
@@ -18,12 +18,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
|
||||
|
||||
from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
|
||||
from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
from openpilot.sunnypilot.models.helpers import get_model_path, load_metadata, prepare_inputs, load_meta_constants
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -34,6 +37,7 @@ MODEL_PATHS = {
|
||||
|
||||
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
|
||||
|
||||
|
||||
class FrameMeta:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
@@ -55,6 +59,10 @@ class ModelState:
|
||||
self.frame = ModelFrame(context)
|
||||
self.wide_frame = ModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
bundle = get_active_bundle()
|
||||
overrides = {override.key: override.value for override in bundle.overrides}
|
||||
self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
|
||||
self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
|
||||
|
||||
model_paths = get_model_path()
|
||||
self.model_metadata = load_metadata()
|
||||
@@ -118,6 +126,15 @@ class ModelState:
|
||||
self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :]
|
||||
return outputs
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
long_action_t: float) -> log.ModelDataV2.Action:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], ModelConstants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
return log.ModelDataV2.Action(desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
cloudlog.warning("modeld init")
|
||||
@@ -158,7 +175,7 @@ def main(demo=False):
|
||||
|
||||
# messaging
|
||||
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
@@ -184,8 +201,10 @@ def main(demo=False):
|
||||
|
||||
cloudlog.info("modeld got CarParams: %s", CP.brand)
|
||||
|
||||
# TODO this needs more thought, use .2s extra for now to estimate other delays
|
||||
steer_delay = CP.steerActuatorDelay + .2
|
||||
# Enable lagd support for sunnypilot modeld
|
||||
steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS
|
||||
long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
|
||||
@@ -280,7 +299,8 @@ def main(demo=False):
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
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,
|
||||
action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL)
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
|
||||
v_ego, steer_delay, model.meta)
|
||||
|
||||
|
||||
@@ -3,30 +3,21 @@ import capnp
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
ConfidenceClass = log.ModelDataV2.ConfidenceClass
|
||||
|
||||
|
||||
def curv_from_psis(psi_target, psi_rate, vego, delay):
|
||||
vego = np.clip(vego, MIN_SPEED, np.inf)
|
||||
curv_from_psi = psi_target / (vego * delay) # epsilon to prevent divide-by-zero
|
||||
return 2 * curv_from_psi - psi_rate / vego
|
||||
def get_curvature_from_output(output, vego, lat_action_t, current_generation=None):
|
||||
if current_generation != 11:
|
||||
if desired_curv := output.get('desired_curvature'): # If the model outputs the desired curvature, use that directly
|
||||
return float(desired_curv[0, 0])
|
||||
|
||||
|
||||
def get_curvature_from_plan(plan, vego, delay):
|
||||
psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2])
|
||||
psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2]
|
||||
return curv_from_psis(psi_target, psi_rate, vego, delay)
|
||||
|
||||
|
||||
def get_curvature_from_output(output, vego, delay):
|
||||
if desired_curv := output.get('desired_curvature'): # If the model outputs the desired curvature, use that directly
|
||||
return float(desired_curv[0, 0])
|
||||
|
||||
return float(get_curvature_from_plan(output['plan'][0], vego, delay))
|
||||
plan_output = output['plan'][0]
|
||||
return float(get_curvature_from_plan(plan_output[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan_output[:, Plan.ORIENTATION_RATE][:, 2],
|
||||
ModelConstants.T_IDXS, vego, lat_action_t))
|
||||
|
||||
|
||||
class PublishState:
|
||||
@@ -76,7 +67,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
|
||||
builder.rightProb = lane_line_probs[2]
|
||||
|
||||
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
|
||||
net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
|
||||
net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
|
||||
publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
|
||||
frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
|
||||
valid: bool, model_meta) -> None:
|
||||
@@ -85,15 +76,13 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
extended_msg.valid = valid
|
||||
base_msg.valid = valid
|
||||
|
||||
desired_curvature = float(get_curvature_from_output(net_output_data, v_ego, delay))
|
||||
|
||||
driving_model_data = base_msg.drivingModelData
|
||||
|
||||
driving_model_data.frameId = vipc_frame_id
|
||||
driving_model_data.frameIdExtra = vipc_frame_id_extra
|
||||
driving_model_data.frameDropPerc = frame_drop_perc
|
||||
driving_model_data.modelExecutionTime = model_execution_time
|
||||
driving_model_data.action.desiredCurvature = desired_curvature
|
||||
driving_model_data.action = action
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
@@ -126,8 +115,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
# poly path
|
||||
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
|
||||
|
||||
# lateral planning
|
||||
modelV2.action.desiredCurvature = desired_curvature
|
||||
# action (includes lateral planning now)
|
||||
modelV2.action = action
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
|
||||
@@ -4,22 +4,34 @@ import pathlib
|
||||
import onnx
|
||||
import codecs
|
||||
import pickle
|
||||
from typing import Any
|
||||
|
||||
|
||||
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
|
||||
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
|
||||
name = value_info.name
|
||||
return name, shape
|
||||
|
||||
|
||||
def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any:
|
||||
for prop in model.metadata_props:
|
||||
if prop.key == name:
|
||||
return prop.value
|
||||
return None
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
model_path = pathlib.Path(sys.argv[1])
|
||||
model = onnx.load(str(model_path))
|
||||
i = [x.key for x in model.metadata_props].index('output_slices')
|
||||
output_slices = model.metadata_props[i].value
|
||||
output_slices = get_metadata_value_by_name(model, 'output_slices')
|
||||
assert output_slices is not None, 'output_slices not found in metadata'
|
||||
|
||||
metadata = {}
|
||||
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
|
||||
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
|
||||
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
|
||||
metadata = {
|
||||
'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
|
||||
'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
|
||||
'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
|
||||
'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
|
||||
}
|
||||
|
||||
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
|
||||
with open(metadata_path, 'wb') as f:
|
||||
|
||||
@@ -1,142 +0,0 @@
|
||||
import os
|
||||
import pickle
|
||||
from abc import ABC, abstractmethod
|
||||
import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.modeld_v2 import MODEL_PATH, MODEL_PKL_PATH, METADATA_PATH
|
||||
from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLMem
|
||||
from openpilot.sunnypilot.modeld_v2.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES
|
||||
from openpilot.sunnypilot.modeld_v2.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
if TICI:
|
||||
os.environ['QCOM'] = '1'
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
ModelManager = custom.ModelManagerSP
|
||||
|
||||
|
||||
class ModelRunner(ABC):
|
||||
"""Abstract base class for model runners that defines the interface for running ML models."""
|
||||
|
||||
def __init__(self):
|
||||
"""Initialize the model runner with paths to model and metadata files."""
|
||||
metadata_path = METADATA_PATH
|
||||
self.is_20hz = None
|
||||
self._drive_model = None
|
||||
self._metadata_model = None
|
||||
|
||||
if bundle := get_active_bundle():
|
||||
bundle_models = {model.type.raw: model for model in bundle.models}
|
||||
self._drive_model = bundle_models.get(ModelManager.Model.Type.supercombo)
|
||||
self._metadata_model = self._drive_model.metadata
|
||||
self.is_20hz = bundle.is20hz
|
||||
|
||||
# Override the metadata path if a metadata model is found in the active bundle
|
||||
if self._metadata_model:
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{self._metadata_model.fileName}"
|
||||
|
||||
with open(metadata_path, 'rb') as f:
|
||||
self.model_metadata = pickle.load(f)
|
||||
|
||||
self.input_shapes = self.model_metadata['input_shapes']
|
||||
self.output_slices = self.model_metadata['output_slices']
|
||||
self.inputs: dict = {}
|
||||
self.parser = Parser()
|
||||
|
||||
@abstractmethod
|
||||
def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
|
||||
"""Prepare inputs for model inference."""
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def _run_model(self):
|
||||
"""Run model inference with prepared inputs."""
|
||||
raise NotImplementedError("This method should be implemented in subclasses.")
|
||||
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> dict:
|
||||
"""Slice model outputs according to metadata configuration."""
|
||||
parsed_outputs = {k: model_outputs[np.newaxis, v] for k, v in self.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
parsed_outputs['raw_pred'] = model_outputs.copy()
|
||||
return parsed_outputs
|
||||
|
||||
def run_model(self) -> dict[str, np.ndarray]:
|
||||
"""Run model inference with prepared inputs and parse outputs."""
|
||||
result: dict[str, np.ndarray] = self.parser.parse_outputs(self._slice_outputs(self._run_model()))
|
||||
return result
|
||||
|
||||
|
||||
class TinygradRunner(ModelRunner):
|
||||
"""Tinygrad implementation of model runner for TICI hardware."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
model_pkl_path = MODEL_PKL_PATH
|
||||
if self._drive_model:
|
||||
model_pkl_path = f"{CUSTOM_MODEL_PATH}/{self._drive_model.artifact.fileName}"
|
||||
assert model_pkl_path.endswith('_tinygrad.pkl'), f"Invalid model file: {model_pkl_path} for TinygradRunner"
|
||||
|
||||
# Load Tinygrad model
|
||||
with open(model_pkl_path, "rb") as f:
|
||||
try:
|
||||
self.model_run = pickle.load(f)
|
||||
except FileNotFoundError as e:
|
||||
assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC"
|
||||
raise
|
||||
|
||||
self.input_to_dtype = {}
|
||||
self.input_to_device = {}
|
||||
|
||||
for idx, name in enumerate(self.model_run.captured.expected_names):
|
||||
self.input_to_dtype[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][2] # 2 is the dtype
|
||||
self.input_to_device[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][3] # 3 is the device
|
||||
|
||||
def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
|
||||
# Initialize image tensors if not already done
|
||||
for key in imgs_cl:
|
||||
if TICI and key not in self.inputs:
|
||||
self.inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=self.input_to_dtype[key])
|
||||
elif not TICI:
|
||||
shape = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key])
|
||||
self.inputs[key] = Tensor(shape, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
|
||||
|
||||
# Update numpy inputs
|
||||
for key, value in numpy_inputs.items():
|
||||
if key not in imgs_cl:
|
||||
self.inputs[key] = Tensor(value, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
|
||||
|
||||
return self.inputs
|
||||
|
||||
def _run_model(self):
|
||||
return self.model_run(**self.inputs).numpy().flatten()
|
||||
|
||||
|
||||
class ONNXRunner(ModelRunner):
|
||||
"""ONNX implementation of model runner for non-TICI hardware."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.runner = make_onnx_cpu_runner(MODEL_PATH)
|
||||
|
||||
self.input_to_nptype = {
|
||||
model_input.name: ORT_TYPES_TO_NP_TYPES[model_input.type]
|
||||
for model_input in self.runner.get_inputs()
|
||||
}
|
||||
|
||||
def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
|
||||
self.inputs = numpy_inputs
|
||||
for key in imgs_cl:
|
||||
self.inputs[key] = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]).astype(dtype=self.input_to_nptype[key])
|
||||
return self.inputs
|
||||
|
||||
def _run_model(self):
|
||||
return self.runner.run(None, self.inputs)[0].flatten()
|
||||
@@ -1,7 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
from openpilot.system.hardware import TICI
|
||||
|
||||
#
|
||||
import time
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
@@ -13,20 +10,22 @@ from opendbc.car.car_helpers import get_demo_car_params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
|
||||
from openpilot.sunnypilot.modeld_v2.constants import Plan
|
||||
from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.model_runner import ONNXRunner, TinygradRunner
|
||||
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
LAT_SMOOTH_SECONDS = 0.0
|
||||
|
||||
|
||||
class FrameMeta:
|
||||
@@ -38,23 +37,32 @@ class FrameMeta:
|
||||
if vipc is not None:
|
||||
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
|
||||
|
||||
|
||||
class ModelState:
|
||||
frames: dict[str, DrivingModelFrame]
|
||||
inputs: dict[str, np.ndarray]
|
||||
prev_desire: np.ndarray # for tracking the rising edge of the pulse
|
||||
temporal_idxs: slice | np.ndarray
|
||||
|
||||
def __init__(self, context: CLContext):
|
||||
try:
|
||||
self.model_runner = TinygradRunner() if TICI else ONNXRunner()
|
||||
self.model_runner = get_model_runner()
|
||||
self.constants = self.model_runner.constants
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"Failed to initialize model runner: {str(e)}")
|
||||
raise
|
||||
|
||||
model_bundle = get_active_bundle()
|
||||
self.generation = model_bundle.generation
|
||||
overrides = {override.key: override.value for override in model_bundle.overrides}
|
||||
|
||||
self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
|
||||
self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
|
||||
self.MIN_LAT_CONTROL_SPEED = 0.3
|
||||
|
||||
buffer_length = 5 if self.model_runner.is_20hz else 2
|
||||
self.frames = {'input_imgs': DrivingModelFrame(context, buffer_length), 'big_input_imgs': DrivingModelFrame(context, buffer_length)}
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
if self.model_runner.is_20hz:
|
||||
self.full_features_buffer = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
||||
self.full_desire = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
||||
self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
|
||||
|
||||
# img buffers are managed in openCL transform code
|
||||
self.numpy_inputs = {}
|
||||
@@ -63,11 +71,19 @@ class ModelState:
|
||||
if key not in self.frames: # Managed by opencl
|
||||
self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32)
|
||||
|
||||
if self.model_runner.is_20hz:
|
||||
if self.model_runner.is_20hz_3d: # split models
|
||||
self.full_features_buffer = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.FEATURE_LEN), dtype=np.float32)
|
||||
self.full_desire = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.DESIRE_LEN), dtype=np.float32)
|
||||
self.full_prev_desired_curv = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
|
||||
self.temporal_idxs = slice(-1-(self.constants.TEMPORAL_SKIP*(self.constants.INPUT_HISTORY_BUFFER_LEN-1)), None, self.constants.TEMPORAL_SKIP)
|
||||
elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d:
|
||||
self.full_features_buffer = np.zeros((self.constants.FULL_HISTORY_BUFFER_LEN + 1, self.constants.FEATURE_LEN), dtype=np.float32)
|
||||
self.full_desire = np.zeros((self.constants.FULL_HISTORY_BUFFER_LEN + 1, self.constants.DESIRE_LEN), dtype=np.float32)
|
||||
num_elements = self.numpy_inputs['features_buffer'].shape[1]
|
||||
step_size = int(-100 / num_elements)
|
||||
self.full_features_buffer_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1]
|
||||
self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2])
|
||||
self.temporal_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1]
|
||||
self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1,
|
||||
self.numpy_inputs['desire'].shape[2])
|
||||
|
||||
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
@@ -76,11 +92,15 @@ class ModelState:
|
||||
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
if self.model_runner.is_20hz:
|
||||
if self.model_runner.is_20hz_3d: # split models
|
||||
self.full_desire[0,:-1] = self.full_desire[0,1:]
|
||||
self.full_desire[0,-1] = new_desire
|
||||
self.numpy_inputs['desire'][:] = self.full_desire.reshape((1, self.constants.INPUT_HISTORY_BUFFER_LEN, self.constants.TEMPORAL_SKIP, -1)).max(axis=2)
|
||||
elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d: # 20hz supercombo
|
||||
self.full_desire[:-1] = self.full_desire[1:]
|
||||
self.full_desire[-1] = new_desire
|
||||
self.numpy_inputs['desire'][:] = self.full_desire.reshape(self.desire_reshape_dims).max(axis=2)
|
||||
else:
|
||||
else: # not 20hz
|
||||
length = inputs['desire'].shape[0]
|
||||
self.numpy_inputs['desire'][0, :-1] = self.numpy_inputs['desire'][0, 1:]
|
||||
self.numpy_inputs['desire'][0, -1, :length] = new_desire[:length]
|
||||
@@ -101,11 +121,15 @@ class ModelState:
|
||||
# Run model inference
|
||||
outputs = self.model_runner.run_model()
|
||||
|
||||
if self.model_runner.is_20hz:
|
||||
if self.model_runner.is_20hz_3d: # split models
|
||||
self.full_features_buffer[0, :-1] = self.full_features_buffer[0, 1:]
|
||||
self.full_features_buffer[0, -1] = outputs['hidden_state'][0, :]
|
||||
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
|
||||
elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d: # 20hz supercombo
|
||||
self.full_features_buffer[:-1] = self.full_features_buffer[1:]
|
||||
self.full_features_buffer[-1] = outputs['hidden_state'][0, :]
|
||||
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[self.full_features_buffer_idxs]
|
||||
else:
|
||||
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[self.temporal_idxs]
|
||||
else: # not 20hz
|
||||
feature_len = outputs['hidden_state'].shape[1]
|
||||
self.numpy_inputs['features_buffer'][0, :-1] = self.numpy_inputs['features_buffer'][0, 1:]
|
||||
self.numpy_inputs['features_buffer'][0, -1, :feature_len] = outputs['hidden_state'][0, :feature_len]
|
||||
@@ -119,11 +143,36 @@ class ModelState:
|
||||
input_name_prev = 'prev_desired_curv'
|
||||
|
||||
if input_name_prev is not None:
|
||||
length = outputs['desired_curvature'][0].size
|
||||
self.numpy_inputs[input_name_prev][0, :-length, 0] = self.numpy_inputs[input_name_prev][0, length:, 0]
|
||||
self.numpy_inputs[input_name_prev][0, -length:, 0] = outputs['desired_curvature'][0]
|
||||
self.process_desired_curvature(outputs, input_name_prev)
|
||||
return outputs
|
||||
|
||||
def process_desired_curvature(self, outputs, input_name_prev):
|
||||
if self.model_runner.is_20hz_3d: # split models
|
||||
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
|
||||
self.full_prev_desired_curv[0,-1,:] = outputs['desired_curvature'][0, :]
|
||||
self.numpy_inputs[input_name_prev][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
|
||||
if self.generation == 11:
|
||||
self.numpy_inputs[input_name_prev][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
|
||||
else:
|
||||
length = outputs['desired_curvature'][0].size
|
||||
self.numpy_inputs[input_name_prev][0, :-length, 0] = self.numpy_inputs[input_name_prev][0, length:, 0]
|
||||
self.numpy_inputs[input_name_prev][0, -length:, 0] = outputs['desired_curvature'][0]
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.generation)
|
||||
if v_ego > self.MIN_LAT_CONTROL_SPEED:
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
|
||||
else:
|
||||
desired_curvature = prev_action.desiredCurvature
|
||||
|
||||
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
cloudlog.warning("modeld init")
|
||||
@@ -170,7 +219,7 @@ def main(demo=False):
|
||||
params = Params()
|
||||
|
||||
# setup filter to track dropped frames
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
|
||||
frame_id = 0
|
||||
last_vipc_frame_id = 0
|
||||
run_count = 0
|
||||
@@ -189,8 +238,9 @@ def main(demo=False):
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("modeld got CarParams: %s", CP.brand)
|
||||
|
||||
# Enable lagd support for modeld_v2
|
||||
steer_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
# TODO Move smooth seconds to action function
|
||||
long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
|
||||
@@ -220,7 +270,7 @@ def main(demo=False):
|
||||
|
||||
if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
|
||||
cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\
|
||||
extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
|
||||
extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
|
||||
|
||||
else:
|
||||
# Use single camera
|
||||
@@ -232,18 +282,20 @@ def main(demo=False):
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["roadCameraState"].frameId
|
||||
v_ego = max(sm["carState"].vEgo, 0.)
|
||||
steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS
|
||||
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics,
|
||||
False).astype(np.float32)
|
||||
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
|
||||
live_calib_seen = True
|
||||
|
||||
traffic_convention = np.zeros(2)
|
||||
traffic_convention[int(is_rhd)] = 1
|
||||
|
||||
vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
|
||||
vec_desire = np.zeros(model.constants.DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < model.constants.DESIRE_LEN:
|
||||
vec_desire[desire] = 1
|
||||
|
||||
# tracked dropped frames
|
||||
@@ -276,7 +328,10 @@ def main(demo=False):
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
|
||||
|
||||
action = model.get_action_from_model(model_output, prev_action, steer_delay + DT_MDL, long_delay + DT_MDL, v_ego)
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, load_meta_constants())
|
||||
|
||||
|
||||
@@ -0,0 +1,127 @@
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
|
||||
|
||||
|
||||
def safe_exp(x, out=None):
|
||||
# -11 is around 10**14, more causes float16 overflow
|
||||
return np.exp(np.clip(x, -np.inf, 11), out=out)
|
||||
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + safe_exp(-x))
|
||||
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
safe_exp(x, out=x)
|
||||
else:
|
||||
x = safe_exp(x)
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
|
||||
def check_missing(self, outs, name):
|
||||
if name not in outs and not self.ignore_missing:
|
||||
raise ValueError(f"Missing output {name}")
|
||||
return name not in outs
|
||||
|
||||
def parse_categorical_crossentropy(self, name, outs, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
if out_shape is not None:
|
||||
raw = raw.reshape((raw.shape[0],) + out_shape)
|
||||
outs[name] = softmax(raw, axis=-1)
|
||||
|
||||
def parse_binary_crossentropy(self, name, outs):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
|
||||
if out_N == 1:
|
||||
for fidx in range(weights.shape[0]):
|
||||
idxs = np.argsort(weights[fidx][:,0])[::-1]
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
|
||||
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
for fidx in range(weights.shape[0]):
|
||||
for hidx in range(out_N):
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1:
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
else:
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
|
||||
if 'lane_lines' in outs:
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION,
|
||||
out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
for k in ['lead_prob', 'lane_lines_prob']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
|
||||
def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
self.split_outputs(outs)
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
|
||||
self.parse_binary_crossentropy('meta', outs)
|
||||
return outs
|
||||
|
||||
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION,
|
||||
out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
|
||||
self.split_outputs(outs)
|
||||
if 'lat_planner_solution' in outs:
|
||||
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
if 'desired_curvature' in outs:
|
||||
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
|
||||
return outs
|
||||
|
||||
def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
outs = self.parse_vision_outputs(outs)
|
||||
outs = self.parse_policy_outputs(outs)
|
||||
return outs
|
||||
@@ -43,6 +43,16 @@ class ModelParser:
|
||||
model.metadata = ModelParser._parse_artifact(metadata)
|
||||
return model
|
||||
|
||||
@staticmethod
|
||||
def _parse_overrides(overrides_data: dict[str, str]) -> list[custom.ModelManagerSP.Override]:
|
||||
overrides = []
|
||||
for key, value in overrides_data.items():
|
||||
override = custom.ModelManagerSP.Override()
|
||||
override.key = key
|
||||
override.value = value
|
||||
overrides.append(override)
|
||||
return overrides
|
||||
|
||||
@staticmethod
|
||||
def _parse_bundle(bundle) -> custom.ModelManagerSP.ModelBundle:
|
||||
model_bundle = custom.ModelManagerSP.ModelBundle()
|
||||
@@ -56,6 +66,7 @@ class ModelParser:
|
||||
model_bundle.runner = bundle.get("runner", custom.ModelManagerSP.Runner.snpe)
|
||||
model_bundle.is20hz = bundle.get("is_20hz", False)
|
||||
model_bundle.minimumSelectorVersion = int(bundle["minimum_selector_version"])
|
||||
model_bundle.overrides = ModelParser._parse_overrides(bundle.get("overrides", {}))
|
||||
|
||||
return model_bundle
|
||||
|
||||
@@ -149,8 +160,9 @@ if __name__ == "__main__":
|
||||
bundles = model_fetcher.get_available_bundles()
|
||||
for bundle in bundles:
|
||||
for model in bundle.models:
|
||||
model_overrides = {override.key: override.value for override in bundle.overrides}
|
||||
# Print model details
|
||||
print(f"Bundle: {bundle.internalName}, Type: {model.type}, Status: {bundle.status}")
|
||||
print(f"Bundle: {bundle.internalName}, Type: {model.type}, Status: {bundle.status}, Overrides: {model_overrides}")
|
||||
# Print artifact details
|
||||
print(f"Artifact: {model.artifact.fileName}, Download URI: {model.artifact.downloadUri.uri}")
|
||||
# Print metadata details
|
||||
|
||||
@@ -19,7 +19,7 @@ from openpilot.system.hardware import PC
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from pathlib import Path
|
||||
|
||||
CURRENT_SELECTOR_VERSION = 3
|
||||
CURRENT_SELECTOR_VERSION = 4
|
||||
REQUIRED_MIN_SELECTOR_VERSION = 2
|
||||
|
||||
USE_ONNX = os.getenv('USE_ONNX', PC)
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
import os
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLMem
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from cereal import custom
|
||||
|
||||
# Type definitions for clarity
|
||||
NumpyDict = dict[str, np.ndarray]
|
||||
ShapeDict = dict[str, tuple[int, ...]]
|
||||
SliceDict = dict[str, slice]
|
||||
CLMemDict = dict[str, CLMem]
|
||||
FrameDict = dict[str, DrivingModelFrame]
|
||||
|
||||
ModelType = custom.ModelManagerSP.Model.Type
|
||||
Model = custom.ModelManagerSP.Model
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
@@ -0,0 +1,35 @@
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType
|
||||
from openpilot.system.hardware import TICI
|
||||
|
||||
if not TICI:
|
||||
from openpilot.sunnypilot.models.runners.onnx.onnx_runner import ONNXRunner
|
||||
|
||||
def get_model_runner() -> ModelRunner:
|
||||
"""
|
||||
Factory function to create and return the appropriate ModelRunner instance.
|
||||
|
||||
Selects between ONNXRunner (for non-TICI platforms) and TinygradRunner
|
||||
(for TICI platforms), choosing TinygradSplitRunner if separate vision/policy
|
||||
models are detected in the active bundle.
|
||||
|
||||
:return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner).
|
||||
"""
|
||||
if not TICI:
|
||||
return ONNXRunner()
|
||||
|
||||
# On TICI platforms, use Tinygrad runners
|
||||
bundle = get_active_bundle()
|
||||
if bundle and bundle.models:
|
||||
model_types = {m.type.raw for m in bundle.models}
|
||||
# Check if the bundle uses separate vision and policy models
|
||||
if ModelType.vision in model_types or ModelType.policy in model_types:
|
||||
return TinygradSplitRunner()
|
||||
# Otherwise, assume a single model (likely supercombo)
|
||||
if bundle.models:
|
||||
return TinygradRunner(bundle.models[0].type.raw)
|
||||
|
||||
# Default fallback to TinygradRunner with the supercombo type if bundle info is missing/incomplete
|
||||
return TinygradRunner(ModelType.supercombo)
|
||||
@@ -0,0 +1,176 @@
|
||||
import os
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.sunnypilot.models.runners.constants import NumpyDict, ShapeDict, CLMemDict, FrameDict, Model, SliceDict, SEND_RAW_PRED
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
import pickle
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
# Set QCOM environment variable for TICI devices, potentially enabling hardware acceleration
|
||||
if TICI:
|
||||
os.environ['QCOM'] = '1'
|
||||
|
||||
|
||||
class ModelData:
|
||||
"""
|
||||
Stores metadata and configuration for a specific machine learning model.
|
||||
|
||||
This class loads model metadata (like input shapes and output slices)
|
||||
from a pickle file associated with a model instance.
|
||||
|
||||
:param model: The machine learning model object containing metadata.
|
||||
"""
|
||||
def __init__(self, model: Model):
|
||||
self.model = model
|
||||
self.metadata = model.metadata
|
||||
self.input_shapes: ShapeDict = {}
|
||||
self.output_slices: SliceDict = {}
|
||||
if self.metadata:
|
||||
self._load_metadata()
|
||||
|
||||
def _load_metadata(self) -> None:
|
||||
"""Loads input shapes and output slices from the model's metadata pickle file."""
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{self.metadata.fileName}"
|
||||
with open(metadata_path, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
self.input_shapes = model_metadata.get('input_shapes', {})
|
||||
self.output_slices = model_metadata.get('output_slices', {})
|
||||
|
||||
|
||||
class ModularRunner(ABC):
|
||||
"""
|
||||
Represents a modular runner for handling and slicing model outputs.
|
||||
|
||||
This abstract base class is designed to provide an interface for modular
|
||||
parsing and processing of model outputs. Classes inheriting from it must
|
||||
implement the specified abstract methods, defining how model outputs
|
||||
should be handled and stored. The primary goal is to enable structured
|
||||
parsing of outputs through a dictionary-based method mapping.
|
||||
|
||||
:ivar parser_method_dict: Mapping dictionary containing parser methods
|
||||
for handling specific types of outputs.
|
||||
:type parser_method_dict: dict
|
||||
"""
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def parser_method_dict(self) -> dict:
|
||||
pass
|
||||
|
||||
@parser_method_dict.setter
|
||||
@abstractmethod
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
pass
|
||||
|
||||
|
||||
class ModelRunner(ModularRunner):
|
||||
"""
|
||||
Abstract base class for managing and executing machine learning models.
|
||||
|
||||
Provides a common interface for loading models, preparing inputs, running
|
||||
inference, and slicing/parsing outputs based on model metadata. Derived
|
||||
classes implement the specifics of input preparation and model execution
|
||||
for different frameworks (e.g., Tinygrad, ONNX).
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Initializes the model runner, loading the active model bundle."""
|
||||
self.is_20hz: bool | None = None
|
||||
self.is_20hz_3d: bool | None = None
|
||||
self.models: dict[int, ModelData] = {}
|
||||
self._model_data: ModelData | None = None # Active model data for current operation
|
||||
self._parser_method_dict: dict = {}
|
||||
self.inputs: dict = {}
|
||||
self._parser = None
|
||||
self._load_models()
|
||||
self._constants = None
|
||||
|
||||
@property
|
||||
def constants(self):
|
||||
return self._constants
|
||||
|
||||
@property
|
||||
def parser_method_dict(self) -> dict:
|
||||
"""Returns the dictionary mapping model types to their respective parsing methods."""
|
||||
return self._parser_method_dict
|
||||
|
||||
@parser_method_dict.setter
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
"""Sets the dictionary mapping model types to their respective parsing methods."""
|
||||
self._parser_method_dict = value
|
||||
|
||||
def _load_models(self) -> None:
|
||||
"""Loads the active model bundle configuration and sets up ModelData."""
|
||||
bundle = get_active_bundle()
|
||||
if not bundle:
|
||||
raise ValueError("No active model bundle found, why are we being executed?")
|
||||
|
||||
self.models = {model.type.raw: ModelData(model) for model in bundle.models}
|
||||
self.is_20hz = bundle.is20hz
|
||||
self.is_20hz_3d = False
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the input shapes for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.input_shapes
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the output slices for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.output_slices
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@abstractmethod
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
"""
|
||||
Abstract method to prepare inputs for model inference.
|
||||
|
||||
:param imgs_cl: Dictionary of OpenCL memory objects for image inputs.
|
||||
:param numpy_inputs: Dictionary of numpy arrays for non-image inputs.
|
||||
:param frames: Dictionary of DrivingModelFrame objects for context.
|
||||
:return: Dictionary of prepared inputs ready for the model.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Abstract method to execute model inference with prepared inputs.
|
||||
|
||||
:return: Dictionary containing the model's raw output arrays.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""
|
||||
Slices the raw model output array based on the output_slices metadata.
|
||||
|
||||
:param model_outputs: The raw numpy array output from the model.
|
||||
:return: A dictionary where keys are output names and values are sliced numpy arrays.
|
||||
"""
|
||||
if not self._model_data:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
sliced_outputs = {k: model_outputs[np.newaxis, v] for k, v in self._model_data.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
sliced_outputs['raw_pred'] = model_outputs.copy() # Optionally include the full raw output
|
||||
return sliced_outputs
|
||||
|
||||
def run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Executes the model inference pipeline: runs the model and parses outputs.
|
||||
|
||||
:return: Dictionary containing the final parsed model outputs.
|
||||
"""
|
||||
return self._run_model() # Parsing is handled within specific runner implementations
|
||||
@@ -0,0 +1,62 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2 import MODEL_PATH
|
||||
from openpilot.sunnypilot.modeld_v2.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType, ShapeDict, CLMemDict, NumpyDict, FrameDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
|
||||
class ONNXRunner(ModelRunner):
|
||||
"""
|
||||
A ModelRunner implementation for executing ONNX models using ONNX Runtime CPU.
|
||||
|
||||
Handles loading the ONNX model, preparing inputs as numpy arrays, running
|
||||
inference, and parsing outputs. This runner is typically used on non-TICI platforms.
|
||||
"""
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
# Initialize ONNX Runtime session for the model at MODEL_PATH
|
||||
self.runner = make_onnx_cpu_runner(MODEL_PATH)
|
||||
# Map expected input names to numpy dtypes
|
||||
self.input_to_nptype = {
|
||||
model_input.name: ORT_TYPES_TO_NP_TYPES[model_input.type]
|
||||
for model_input in self.runner.get_inputs()
|
||||
}
|
||||
# For ONNX, _model_data isn't strictly necessary as shapes/types come from the runner
|
||||
# However, we might still need output_slices if custom models define them.
|
||||
# We assume supercombo type for potentially loading output_slices metadata if available.
|
||||
self._model_data = self.models.get(ModelType.supercombo)
|
||||
self._constants = ModelConstants # Constants for ONNX models, if needed
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the input shapes defined in the ONNX model."""
|
||||
# ONNX shapes are derived directly from the model definition via the runner
|
||||
return {runner_input.name: runner_input.shape for runner_input in self.runner.get_inputs()}
|
||||
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
"""Prepares inputs for the ONNX model as numpy arrays."""
|
||||
self.inputs = numpy_inputs # Start with non-image numpy inputs
|
||||
# Convert image inputs from OpenCL buffers to numpy arrays
|
||||
for key in imgs_cl:
|
||||
buffer = frames[key].buffer_from_cl(imgs_cl[key])
|
||||
reshaped_buffer = buffer.reshape(self.input_shapes[key])
|
||||
self.inputs[key] = reshaped_buffer.astype(dtype=self.input_to_nptype[key])
|
||||
return self.inputs
|
||||
|
||||
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses the raw ONNX model outputs using the standard Parser."""
|
||||
# Use slicing if metadata is available, otherwise pass raw outputs
|
||||
if self._model_data is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
outputs_to_parse = self._slice_outputs(model_outputs) if self._model_data else {'raw_pred': model_outputs}
|
||||
result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](outputs_to_parse)
|
||||
return result
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs the ONNX model inference and parses the outputs."""
|
||||
# Execute the ONNX Runtime session
|
||||
outputs = self.runner.run(None, self.inputs)[0].flatten()
|
||||
return self._parse_outputs(outputs)
|
||||
@@ -0,0 +1,59 @@
|
||||
import os
|
||||
from abc import ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType, NumpyDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModularRunner
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
class PolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for policy-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.policy] = self._parse_policy_outputs
|
||||
|
||||
def _parse_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class VisionTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._vision_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.vision] = self._parse_vision_outputs
|
||||
|
||||
def _parse_vision_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._vision_parser.parse_vision_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class SupercomboTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._supercombo_parser = CombinedParser()
|
||||
self.parser_method_dict[ModelType.supercombo] = self._parse_supercombo_outputs
|
||||
|
||||
def _parse_supercombo_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._supercombo_parser.parse_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
@@ -0,0 +1,129 @@
|
||||
import pickle
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
from openpilot.sunnypilot.models.runners.constants import CLMemDict, FrameDict, NumpyDict, ModelType, ShapeDict, CUSTOM_MODEL_PATH, SliceDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.model_types import PolicyTinygrad, VisionTinygrad, SupercomboTinygrad
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
|
||||
class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad):
|
||||
"""
|
||||
A ModelRunner implementation for executing Tinygrad models.
|
||||
|
||||
Handles loading Tinygrad model artifacts (.pkl), preparing inputs as Tinygrad
|
||||
Tensors (potentially using QCOM extensions on TICI), running inference,
|
||||
and parsing the outputs.
|
||||
|
||||
:param model_type: The type of model (e.g., supercombo) to load and run.
|
||||
"""
|
||||
def __init__(self, model_type: int = ModelType.supercombo):
|
||||
ModelRunner.__init__(self)
|
||||
SupercomboTinygrad.__init__(self)
|
||||
PolicyTinygrad.__init__(self)
|
||||
VisionTinygrad.__init__(self)
|
||||
self._constants = ModelConstants
|
||||
self._model_data = self.models.get(model_type)
|
||||
if not self._model_data or not self._model_data.model:
|
||||
raise ValueError(f"Model data for type {model_type} not available.")
|
||||
|
||||
artifact_filename = self._model_data.model.artifact.fileName
|
||||
assert artifact_filename.endswith('_tinygrad.pkl'), \
|
||||
f"Invalid model file {artifact_filename} for TinygradRunner"
|
||||
|
||||
model_pkl_path = f"{CUSTOM_MODEL_PATH}/{artifact_filename}"
|
||||
with open(model_pkl_path, "rb") as f:
|
||||
try:
|
||||
# Load the compiled Tinygrad model runner function
|
||||
self.model_run = pickle.load(f)
|
||||
except FileNotFoundError as e:
|
||||
# Provide a helpful error message if the model was built for a different platform
|
||||
assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC"
|
||||
raise
|
||||
|
||||
# Map input names to their required dtype and device from the loaded model
|
||||
self.input_to_dtype = {}
|
||||
self.input_to_device = {}
|
||||
for idx, name in enumerate(self.model_run.captured.expected_names):
|
||||
info = self.model_run.captured.expected_st_vars_dtype_device[idx]
|
||||
self.input_to_dtype[name] = info[2] # dtype
|
||||
self.input_to_device[name] = info[3] # device
|
||||
|
||||
def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict):
|
||||
"""Prepares vision (image) inputs as Tinygrad Tensors."""
|
||||
for key in imgs_cl:
|
||||
if TICI and key not in self.inputs:
|
||||
# On TICI, directly use OpenCL memory address for efficiency via QCOM extensions
|
||||
self.inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=self.input_to_dtype[key])
|
||||
elif not TICI:
|
||||
# On other platforms, copy data from CL buffer to a numpy array first
|
||||
shape = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key])
|
||||
self.inputs[key] = Tensor(shape, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
|
||||
|
||||
def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
|
||||
"""Prepares non-image (policy) inputs as Tinygrad Tensors."""
|
||||
for key, value in numpy_inputs.items():
|
||||
self.inputs[key] = Tensor(value, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
|
||||
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
"""Prepares all vision and policy inputs for the model."""
|
||||
self.prepare_vision_inputs(imgs_cl, frames)
|
||||
self.prepare_policy_inputs(numpy_inputs)
|
||||
return self.inputs
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs the Tinygrad model inference and parses the outputs."""
|
||||
outputs = self.model_run(**self.inputs).numpy().flatten()
|
||||
return self._parse_outputs(outputs)
|
||||
|
||||
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses the raw model outputs using the standard Parser."""
|
||||
if self._model_data is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
|
||||
return result
|
||||
|
||||
|
||||
class TinygradSplitRunner(ModelRunner):
|
||||
"""
|
||||
A ModelRunner that coordinates separate TinygradVisionRunner and TinygradPolicyRunner instances.
|
||||
|
||||
Manages the execution of split vision and policy models, combining their inputs and outputs.
|
||||
"""
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.is_20hz_3d = True
|
||||
self.vision_runner = TinygradRunner(ModelType.vision)
|
||||
self.policy_runner = TinygradRunner(ModelType.policy)
|
||||
self._constants = SplitModelConstants
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs both vision and policy models and merges their parsed outputs."""
|
||||
policy_output = self.policy_runner.run_model()
|
||||
vision_output = self.vision_runner.run_model()
|
||||
return {**policy_output, **vision_output} # Combine results
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the combined input shapes from both vision and policy models."""
|
||||
return {**self.policy_runner.input_shapes, **self.vision_runner.input_shapes}
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the combined output slices from both vision and policy models."""
|
||||
return {**self.policy_runner.output_slices, **self.vision_runner.output_slices}
|
||||
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
"""Prepares inputs for both vision and policy models."""
|
||||
# Policy inputs only depend on numpy_inputs
|
||||
self.policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
# Vision inputs depend on imgs_cl and frames
|
||||
self.vision_runner.prepare_vision_inputs(imgs_cl, frames)
|
||||
# Return combined inputs (though they are stored within respective runners)
|
||||
return {**self.policy_runner.inputs, **self.vision_runner.inputs}
|
||||
@@ -0,0 +1,94 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def index_function(idx, max_val=192, max_idx=32):
|
||||
return max_val * ((idx/max_idx)**2)
|
||||
|
||||
|
||||
class SplitModelConstants:
|
||||
# time and distance indices
|
||||
IDX_N = 33
|
||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
|
||||
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
|
||||
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
|
||||
LEAD_T_OFFSETS = [0., 2., 4.]
|
||||
META_T_IDXS = [2., 4., 6., 8., 10.]
|
||||
|
||||
# model inputs constants
|
||||
MODEL_FREQ = 20
|
||||
HISTORY_FREQ = 5
|
||||
HISTORY_LEN_SECONDS = 5
|
||||
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
|
||||
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
|
||||
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
|
||||
|
||||
FEATURE_LEN = 512
|
||||
|
||||
DESIRE_LEN = 8
|
||||
TRAFFIC_CONVENTION_LEN = 2
|
||||
LAT_PLANNER_STATE_LEN = 4
|
||||
LATERAL_CONTROL_PARAMS_LEN = 2
|
||||
PREV_DESIRED_CURV_LEN = 1
|
||||
|
||||
# model outputs constants
|
||||
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
|
||||
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
|
||||
FCW_5MS2_PROBS_WIDTH = 5
|
||||
FCW_3MS2_PROBS_WIDTH = 2
|
||||
|
||||
DISENGAGE_WIDTH = 5
|
||||
POSE_WIDTH = 6
|
||||
WIDE_FROM_DEVICE_WIDTH = 3
|
||||
LEAD_WIDTH = 4
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
|
||||
NUM_LANE_LINES = 4
|
||||
NUM_ROAD_EDGES = 2
|
||||
|
||||
LEAD_TRAJ_LEN = 6
|
||||
DESIRE_PRED_LEN = 4
|
||||
|
||||
PLAN_MHP_N = 5
|
||||
LEAD_MHP_N = 2
|
||||
PLAN_MHP_SELECTION = 1
|
||||
LEAD_MHP_SELECTION = 3
|
||||
|
||||
FCW_THRESHOLD_5MS2_HIGH = 0.15
|
||||
FCW_THRESHOLD_5MS2_LOW = 0.05
|
||||
FCW_THRESHOLD_3MS2 = 0.7
|
||||
|
||||
CONFIDENCE_BUFFER_LEN = 5
|
||||
RYG_GREEN = 0.01165
|
||||
RYG_YELLOW = 0.06157
|
||||
|
||||
POLY_PATH_DEGREE = 4
|
||||
|
||||
|
||||
# model outputs slices
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
VELOCITY = slice(3, 6)
|
||||
ACCELERATION = slice(6, 9)
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
|
||||
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)
|
||||
# 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)
|
||||
@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
from cereal import messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.sunnypilot.models.helpers import get_active_model_runner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
@@ -16,7 +15,6 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, mpc):
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.is_stock = get_active_model_runner() == custom.ModelManagerSP.Runner.stock
|
||||
|
||||
def get_mpc_mode(self) -> str | None:
|
||||
if not self.dec.active():
|
||||
|
||||
Reference in New Issue
Block a user