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

Revert TR (#35110)

Browse Source 
* Revert "Tomb raider 2 (#35029)"

This reverts commit 2c162d9b75.

* bugfix

* fix policy

* min control speed
This commit is contained in:
Harald Schäfer
2025-05-02 20:54:42 -07:00
committed by GitHub
parent a1ec8c6bfe
commit df4f2955dc
7 changed files with 23 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+3 -12
View File
@@ -90,7 +90,7 @@ class LongitudinalPlanner:
return x, v, a, j, throttle_prob
def update(self, sm):
self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -113,7 +113,7 @@ class LongitudinalPlanner:
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if self.mode == 'acc':
if self.mpc.mode == 'acc':
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
@@ -160,17 +160,8 @@ class LongitudinalPlanner:
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
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_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
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
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)
selfdrive/modeld/fill_model_msg.py
+5 -5
View File
@@ -90,11 +90,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# temporal pose
#temporal_pose = modelV2.temporalPose
#temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
#temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
#temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
#temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
temporal_pose = modelV2.temporalPose
temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
# poly path
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
selfdrive/modeld/modeld.py
+3 -3
View File
@@ -41,8 +41,8 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
LAT_SMOOTH_SECONDS = 0.3
LONG_SMOOTH_SECONDS = 0.3
LAT_SMOOTH_SECONDS = 0.0
LONG_SMOOTH_SECONDS = 0.0
MIN_LAT_CONTROL_SPEED = 0.3
@@ -172,7 +172,7 @@ class ModelState:
# TODO model only uses last value now
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
if SEND_RAW_PRED:
selfdrive/modeld/models/driving_policy.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b86d130c5db2d772da1b139c136ed86976f37137129a19a6b881fdf641bca198
size 15578328
oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36
size 15966721
selfdrive/modeld/models/driving_vision.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f222d2c528f1763828de01bb55e8979b1e4056e1dbb41350f521d2d2bb09d177
size 46265585
oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624
size 34882971
selfdrive/modeld/parse_model_outputs.py
+7 -6
View File
@@ -88,12 +88,6 @@ class Parser:
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
self.parse_binary_crossentropy('meta', outs)
return outs
@@ -101,10 +95,17 @@ class Parser:
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
if 'lat_planner_solution' in outs:
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
if 'desired_curvature' in outs:
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
return outs
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
7bf4ae5b92a3ad1f073f675e24e28babad0f2aa0
7bf4ae5b92a3ad1f073f675e24e28babad0f2aa0