diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1d0facd133..bcade327c6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -43,10 +43,11 @@ POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.p LAT_SMOOTH_SECONDS = 0.1 LONG_SMOOTH_SECONDS = 0.3 +STATIONARY_LAT_SMOOTH_SECONDS = 100.0 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float,) -> 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], @@ -55,7 +56,8 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) desired_curvature = model_output['desired_curvature'][0, 0] - desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) + low_speed_smooth_seconds = np.interp(v_ego, [0, MIN_SPEED], [STATIONARY_LAT_SMOOTH_SECONDS, 0.0]) + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS+low_speed_smooth_seconds) return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), @@ -331,7 +333,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + action = get_action_from_model(model_output, prev_action, lat_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,