From 2c162d9b75ae4e77bdf5bc3225e880d352ec0f5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 17 Apr 2025 23:21:25 -0700 Subject: [PATCH] Tomb raider 2 (#35029) * db56b8fb-9135-4ab6-af18-99b7df7b2245/400 * fixes * linter unhappy * 6dbe0991-baa1-49ad-836a-ab370d1f0d92/400 * This one is good: 19387087-1005-475e-9015-9458dd8e7c5f/400 * Better every day: 39ed911c-0937-417f-97d2-58a8bb3caa53/400 * Actually end-to-end * typo * smooooooth: 94e23541-eb84-4fef-9f51-6a2d82aff314/360 * Revert "smooooooth: 94e23541-eb84-4fef-9f51-6a2d82aff314/360" This reverts commit edd4f02386d83d82dd8a188985cde80ed1646b7f. * 11632ef7-f555-489c-8480-e3bf97d9285e/400 * 08712d27-f6bd-4536-a30e-c729e5f62356/400 * 0a92a35e-1f72-476a-8cb6-c9f103f36822/400 * ee6d2394-2072-420c-a664-b4c0d4ed0b61/400 * no prev curv * No double work * fix bug * smooth * update prev action * whitespace * add little accel * new ref * Update plant.py --- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++++----- selfdrive/modeld/fill_model_msg.py | 10 +++++----- selfdrive/modeld/modeld.py | 7 ++++--- selfdrive/modeld/models/driving_policy.onnx | 4 ++-- selfdrive/modeld/models/driving_vision.onnx | 4 ++-- selfdrive/modeld/parse_model_outputs.py | 13 ++++++------ .../test/longitudinal_maneuvers/plant.py | 1 + selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 36 insertions(+), 25 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8cbf0beee9..104c7c9d49 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -89,14 +89,15 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + self.mpc.mode = 'acc' + self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) else: accel_coast = ACCEL_MAX - v_ego = sm['carState'].vEgo + v_ego = sm['modelV2'].velocity.x[0] v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET @@ -112,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.mpc.mode == 'acc': + if self.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) @@ -128,7 +129,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], 0) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED @@ -159,8 +160,17 @@ 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, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, + output_a_target_mpc, output_should_stop_mpc = 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) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index a91c6395c7..36bd724d01 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -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) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f5007273ec..0ad629f519 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -43,8 +43,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.0 -LONG_SMOOTH_SECONDS = 0.0 +LAT_SMOOTH_SECONDS = 0.3 +LONG_SMOOTH_SECONDS = 0.3 def smooth_value(val, prev_val, tau): alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 @@ -174,7 +174,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'][:] = self.full_prev_desired_curv[0, self.temporal_idxs] + self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs] combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: @@ -338,6 +338,7 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + 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) diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 3601bbb5da..3bf2e01b5e 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36 -size 15966721 +oid sha256:fa00a07a4307b0f1638a10ec33b651d8a102e38371289b744f43215c89dfd342 +size 15578328 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index aee6d8f1bf..5e08b78472 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624 -size 34882971 +oid sha256:f222d2c528f1763828de01bb55e8979b1e4056e1dbb41350f521d2d2bb09d177 +size 46265585 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 810c44ccb9..783572d436 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -88,6 +88,12 @@ 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 @@ -95,17 +101,10 @@ 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 diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 989b84dee3..b8c6adb436 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -107,6 +107,7 @@ class Plant: position = log.XYZTData.new_message() position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] model.modelV2.position = position + model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1) velocity = log.XYZTData.new_message() velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] velocity.x[0] = float(self.speed) # always start at current speed diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 632922a8a4..5f89d7802d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3d3f875dc211ab0ca8a8e564ecf6dd3f52fcf7d9 \ No newline at end of file +0800e73b5d9c801f161b9559557c0559b0682fec \ No newline at end of file