mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 19:12:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Disable low speed steer in modeld (#35056)

Browse Source 
* disable low speed steer

* No steer under
This commit is contained in:
Harald Schäfer
2025-04-23 14:15:06 -07:00
committed by GitHub
parent 281a72780b
commit a38dcbb3fe
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.py
+7 -3
View File
@@ -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
MIN_LAT_CONTROL_SPEED = 0.3
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,10 @@ 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)
if v_ego > MIN_LAT_CONTROL_SPEED:
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
else:
desired_curvature = prev_action.desiredCurvature
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
@@ -331,7 +335,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,