From e7db17980b3667de1399cac97ef92041f5d59eff Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 30 Jun 2025 07:07:45 -0700 Subject: [PATCH] Try this for ModelDataV2.Action --- .../controls/lib/longitudinal_planner.py | 17 +++++++-------- sunnypilot/modeld/fill_model_msg.py | 6 +----- sunnypilot/modeld/modeld.py | 21 +++---------------- sunnypilot/modeld_v2/modeld.py | 7 +++++-- 4 files changed, 17 insertions(+), 34 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8efd34cceb..6990e76c01 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -171,16 +171,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if self.generation == 11: - if self.mode == 'acc': - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc - else: - 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 self.mode == 'acc': + output_a_target = output_a_target_mpc + self.output_should_stop = output_should_stop_mpc else: - output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + 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.generation == 11): + 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) diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index 608f24424d..27f8e1829a 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -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], action: log.ModelDataV2.Action, publish_state: PublishState, + net_output_data: dict[str, np.ndarray], 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: @@ -78,8 +78,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # 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 @@ -115,8 +113,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # lateral planning 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 diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index d4bd96ff66..e2c4f8adf7 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -18,12 +18,11 @@ 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, Plan +from openpilot.sunnypilot.modeld.constants import ModelConstants from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext @@ -62,8 +61,7 @@ class ModelState: 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")) + self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) model_paths = get_model_path() self.model_metadata = load_metadata() @@ -127,14 +125,6 @@ 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): @@ -204,10 +194,6 @@ def main(demo=False): modeld_lagd = ModeldLagd() - # Enable lagd support for sunnypilot modeld - long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS - prev_action = log.ModelDataV2.Action() - DH = DesireHelper() while True: @@ -303,8 +289,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - 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, + fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, v_ego, steer_delay, model.meta) diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index fe322f462e..60d0db2646 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -57,7 +57,7 @@ class ModelState: 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.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) self.MIN_LAT_CONTROL_SPEED = 0.3 @@ -172,7 +172,10 @@ class ModelState: else: desired_curvature = prev_action.desiredCurvature - return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) + if self.generation == 11: + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) + else: + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature)) def main(demo=False):