diff --git a/common/model.h b/common/model.h index 551c0794b9..6a3286acce 100644 --- a/common/model.h +++ b/common/model.h @@ -1 +1 @@ -#define DEFAULT_MODEL "Tomb_Raider_6 (Default)" +#define DEFAULT_MODEL "Filet o Fish (Default)" diff --git a/opendbc_repo b/opendbc_repo index 10248728cf..ab8640fde2 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 10248728cf3e104a2e3584a8dcd6e3e257823b5a +Subproject commit ab8640fde2806d140ac774fd1de9c324ae0e1583 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 787452b32a..ae5f57db9d 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -338,6 +338,7 @@ class TestCarModelBase(unittest.TestCase): prev_panda_gas = self.safety.get_gas_pressed_prev() prev_panda_brake = self.safety.get_brake_pressed_prev() prev_panda_regen_braking = self.safety.get_regen_braking_prev() + prev_panda_steering_disengage = self.safety.get_steering_disengage_prev() prev_panda_vehicle_moving = self.safety.get_vehicle_moving() prev_panda_vehicle_speed_min = self.safety.get_vehicle_speed_min() prev_panda_vehicle_speed_max = self.safety.get_vehicle_speed_max() @@ -365,6 +366,9 @@ class TestCarModelBase(unittest.TestCase): if self.safety.get_regen_braking_prev() != prev_panda_regen_braking: self.assertEqual(CS.regenBraking, self.safety.get_regen_braking_prev()) + if self.safety.get_steering_disengage_prev() != prev_panda_steering_disengage: + self.assertEqual(CS.steeringDisengage, self.safety.get_steering_disengage_prev()) + if self.safety.get_vehicle_moving() != prev_panda_vehicle_moving: self.assertEqual(not CS.standstill, self.safety.get_vehicle_moving()) @@ -440,6 +444,7 @@ class TestCarModelBase(unittest.TestCase): brake_pressed = False checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev() + checks['steeringDisengage'] += CS.steeringDisengage != self.safety.get_steering_disengage_prev() if self.CP.pcmCruise: # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state. diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 41dcde52eb..8d13e98ad3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -94,9 +94,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP): def update(self, sm): LongitudinalPlannerSP.update(self, sm) - self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if dec_mpc_mode := self.get_mpc_mode(): - self.mode = dec_mpc_mode + self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -119,7 +119,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): # 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) @@ -166,17 +166,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): 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) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index e09d37fad0..086f36a797 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -102,7 +102,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist() temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].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 dde742f159..5e1acdea0e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -26,7 +26,7 @@ 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, get_curvature_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -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 @@ -54,9 +54,8 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. ModelConstants.T_IDXS, action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], - plan[:, Plan.ORIENTATION_RATE][:, 2], - ModelConstants.T_IDXS, v_ego, lat_action_t) + + desired_curvature = model_output['desired_curvature'][0, 0] if v_ego > MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) else: @@ -173,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: diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 5b9b686829..3601bbb5da 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:8a478376723ba79e2393ca95e2d3e497571ba6fed113e5f13a36f0e4b4d4a7c5 -size 15588463 +oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36 +size 15966721 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index 0a7b4a803d..aee6d8f1bf 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:dad289ae367cefcb862ef1d707fb4919d008f0eeaa1ebaf18df58d8de5a7d96e -size 46265585 +oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624 +size 34882971 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 783572d436..810c44ccb9 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3dd3eab25f..1ae4c4a1a8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7bf4ae5b92a3ad1f073f675e24e28babad0f2aa0 \ No newline at end of file +7bf4ae5b92a3ad1f073f675e24e28babad0f2aa0 diff --git a/sunnypilot/models/tests/model_hash b/sunnypilot/models/tests/model_hash index 4504dca4b2..2f50b2085c 100644 --- a/sunnypilot/models/tests/model_hash +++ b/sunnypilot/models/tests/model_hash @@ -1 +1 @@ -82ead1a4ecffbde961382329a6cf71bc97a6b112fbfb1f175a51c28ee2472bec \ No newline at end of file +dec34c57c4131f6fca5d1035201d1afbf43e5250cede7bfdc798371af008afad \ No newline at end of file