From e39e4b9a610ccfda1986002e0bbdae2372a79466 Mon Sep 17 00:00:00 2001 From: commaci-public <60409688+commaci-public@users.noreply.github.com> Date: Mon, 3 Mar 2025 14:47:18 -0800 Subject: [PATCH 01/17] [bot] Update Python packages (#34761) Update Python packages Co-authored-by: Vehicle Researcher --- msgq_repo | 2 +- opendbc_repo | 2 +- panda | 2 +- uv.lock | 12 ++++++------ 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/msgq_repo b/msgq_repo index 095f1e2361..ad9020c430 160000 --- a/msgq_repo +++ b/msgq_repo @@ -1 +1 @@ -Subproject commit 095f1e23613d47031a9b23e3f9ed5dd3fe01a06e +Subproject commit ad9020c430362d17c0edf97747d344389234be4d diff --git a/opendbc_repo b/opendbc_repo index bc839875bd..ca4f77b010 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit bc839875bd0ca476be71dea2cbebc92a4544c40c +Subproject commit ca4f77b0101d85a5e8d292e2e61f7a8c7492b9e2 diff --git a/panda b/panda index 2c802449fd..154b66782d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2c802449fd51d9904ca265d06b7a5f9969057ae3 +Subproject commit 154b66782ddf4b9293c4594bb5ee8bd9cd88f332 diff --git a/uv.lock b/uv.lock index fa933d5491..022e30508d 100644 --- a/uv.lock +++ b/uv.lock @@ -4279,7 +4279,7 @@ wheels = [ [[package]] name = "pytest" -version = "8.3.4" +version = "8.3.5" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "colorama", marker = "sys_platform == 'win32'" }, @@ -4287,9 +4287,9 @@ dependencies = [ { name = "packaging" }, { name = "pluggy" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/05/35/30e0d83068951d90a01852cb1cef56e5d8a09d20c7f511634cc2f7e0372a/pytest-8.3.4.tar.gz", hash = "sha256:965370d062bce11e73868e0335abac31b4d3de0e82f4007408d242b4f8610761", size = 1445919 } +sdist = { url = "https://files.pythonhosted.org/packages/ae/3c/c9d525a414d506893f0cd8a8d0de7706446213181570cdbd766691164e40/pytest-8.3.5.tar.gz", hash = "sha256:f4efe70cc14e511565ac476b57c279e12a855b11f48f212af1080ef2263d3845", size = 1450891 } wheels = [ - { url = "https://files.pythonhosted.org/packages/11/92/76a1c94d3afee238333bc0a42b82935dd8f9cf8ce9e336ff87ee14d9e1cf/pytest-8.3.4-py3-none-any.whl", hash = "sha256:50e16d954148559c9a74109af1eaf0c945ba2d8f30f0a3d3335edde19788b6f6", size = 343083 }, + { url = "https://files.pythonhosted.org/packages/30/3d/64ad57c803f1fa1e963a7946b6e0fea4a70df53c1a7fed304586539c2bac/pytest-8.3.5-py3-none-any.whl", hash = "sha256:c69214aa47deac29fad6c2a4f590b9c4a9fdb16a403176fe154b79c0b4d4d820", size = 343634 }, ] [[package]] @@ -4703,11 +4703,11 @@ wheels = [ [[package]] name = "scons" -version = "4.8.1" +version = "4.9.0" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/b9/76/a2c1293642f9a448f2d012cabf525be69ca5abf4af289bc0935ac1554ee8/scons-4.8.1.tar.gz", hash = "sha256:5b641357904d2f56f7bfdbb37e165ab996b6143c948b9df0efc7305f54949daa", size = 3244174 } +sdist = { url = "https://files.pythonhosted.org/packages/61/7e/79e07dc2eb8874580934cd0c834a8a78f15d5b0d6155a424f6c7b35441c3/scons-4.9.0.tar.gz", hash = "sha256:f1a5e161bf3d1411d780d65d7919654b9405555994621d3d68e42d62114b592a", size = 3251763 } wheels = [ - { url = "https://files.pythonhosted.org/packages/b8/a7/823091100c88d7d992c8c608d82a88ed59e227d13f8ccb33ea7a96d43d51/SCons-4.8.1-py3-none-any.whl", hash = "sha256:a4c3b434330e2d7d975002fd6783284ba348bf394db94c8f83fdc5bf69cdb8d7", size = 4123564 }, + { url = "https://files.pythonhosted.org/packages/18/f5/77635f6c68ed742a23e1f52977267e191ea7c2aec89b9604a86487405da1/scons-4.9.0-py3-none-any.whl", hash = "sha256:c8cc309db0f91cffdc27c1374fa3d31e9421bcb31d210de071b0b11adc84739b", size = 4130637 }, ] [[package]] From 159b1c9eb48551e6ac1d5f9281ac2218f320f499 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Mon, 3 Mar 2025 15:15:36 -0800 Subject: [PATCH 02/17] Filet o Fish model (#34637) * 690b01c3 seems ok * correct temporal * push * inplace * bs * what thw * is this wrong * frames are skipped * new models * simplify decimation * clean up * clean up modelframe * need attr * lint * 0 --------- Co-authored-by: Comma Device --- selfdrive/modeld/constants.py | 8 +++++- selfdrive/modeld/modeld.py | 31 ++++++++++++++------- selfdrive/modeld/models/commonmodel.cc | 9 +++--- selfdrive/modeld/models/commonmodel.h | 5 ++-- selfdrive/modeld/models/commonmodel.pxd | 2 +- selfdrive/modeld/models/commonmodel_pyx.pyx | 4 +-- selfdrive/modeld/models/driving_policy.onnx | 4 +-- selfdrive/modeld/models/driving_vision.onnx | 2 +- 8 files changed, 42 insertions(+), 23 deletions(-) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 2bb7b8100c..5ca0a86bc8 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -14,8 +14,14 @@ class ModelConstants: # model inputs constants MODEL_FREQ = 20 + HISTORY_FREQ = 5 + HISTORY_LEN_SECONDS = 5 + TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ + FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS + INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS + FEATURE_LEN = 512 - FULL_HISTORY_BUFFER_LEN = 100 + DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 LAT_PLANNER_STATE_LEN = 4 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ee825d158f..ae1e75e3e9 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -56,16 +56,24 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): - self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)} + self.frames = { + 'input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP), + 'big_input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) + } self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) + self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32) + self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) + self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP) + # policy inputs self.numpy_inputs = { - 'desire': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32), + 'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32), 'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32), 'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32), - 'prev_desired_curv': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), - 'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), + 'prev_desired_curv': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), + 'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), } with open(VISION_METADATA_PATH, 'rb') as f: @@ -104,8 +112,9 @@ class ModelState: new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:] - self.numpy_inputs['desire'][0,-1] = new_desire + self.full_desire[0,:-1] = self.full_desire[0,1:] + self.full_desire[0,-1] = new_desire + self.numpy_inputs['desire'][:] = self.full_desire[0, self.temporal_idxs] self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] @@ -128,15 +137,17 @@ class ModelState: self.vision_output = self.vision_run(**self.vision_inputs).numpy().flatten() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) - self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:] - self.numpy_inputs['features_buffer'][0,-1] = vision_outputs_dict['hidden_state'][0, :] + self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] + self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :] + self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] self.policy_output = self.policy_run(**self.policy_inputs).numpy().flatten() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) # TODO model only uses last value now - self.numpy_inputs['prev_desired_curv'][0,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:] - self.numpy_inputs['prev_desired_curv'][0,-1,:] = policy_outputs_dict['desired_curvature'][0, :] + 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] combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 9973d18588..99155d9f1e 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -5,11 +5,12 @@ #include "common/clutil.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) { +DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip) : ModelFrame(device_id, context) { input_frames = std::make_unique(buf_size); + temporal_skip = _temporal_skip; input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); - img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 2*frame_size_bytes, NULL, &err)); - region.origin = 1 * frame_size_bytes; + img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (temporal_skip+1)*frame_size_bytes, NULL, &err)); + region.origin = temporal_skip * frame_size_bytes; region.size = frame_size_bytes; last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err)); @@ -20,7 +21,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - for (int i = 0; i < 1; i++) { + for (int i = 0; i < temporal_skip; i++) { CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr)); } loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 14409943e4..176d7eb6dc 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -64,20 +64,21 @@ protected: class DrivingModelFrame : public ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context); + DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip); ~DrivingModelFrame(); cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; - const int buf_size = MODEL_FRAME_SIZE * 2; + const int buf_size = MODEL_FRAME_SIZE * 2; // 2 frames are temporal_skip frames apart const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); private: LoadYUVState loadyuv; cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl; cl_buffer_region region; + int temporal_skip; }; class MonitoringModelFrame : public ModelFrame { diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index b4f08b12aa..4ac64d9172 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -20,7 +20,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h": cppclass DrivingModelFrame: int buf_size - DrivingModelFrame(cl_device_id, cl_context) + DrivingModelFrame(cl_device_id, cl_context, int) cppclass MonitoringModelFrame: int buf_size diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index 7b3a5bb342..5b7d11bc71 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -59,8 +59,8 @@ cdef class ModelFrame: cdef class DrivingModelFrame(ModelFrame): cdef cppDrivingModelFrame * _frame - def __cinit__(self, CLContext context): - self._frame = new cppDrivingModelFrame(context.device_id, context.context) + def __cinit__(self, CLContext context, int temporal_skip): + self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip) self.frame = (self._frame) self.buf_size = self._frame.buf_size diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index f804b4ec31..cc2148c74b 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:5cae3285c876804e649b14adadcfb8be79a9bd5a1b928113e37f1f08e25e9688 -size 16581121 +oid sha256:5fd38c3b4d465d14a8ed128f8b1b265bff1589d42f398faa678b93f25b3385dc +size 15966721 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index 06c87d8755..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:29bbf79f9dfd7048c0013bb81e86d9b2979275b95ea1ed8a86d1a86a88695240 +oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624 size 34882971 From 68d22b960b85871bf61dfdb0e2d955c28fde647b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 18:28:49 -0600 Subject: [PATCH 03/17] rename steer_limited (#34763) rename --- selfdrive/controls/controlsd.py | 10 +++++----- selfdrive/controls/lib/latcontrol.py | 6 +++--- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 4 ++-- selfdrive/controls/lib/latcontrol_torque.py | 6 +++--- tools/tuning/measure_steering_accuracy.py | 4 ++-- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fb7dd3944c..14fd37040e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -40,7 +40,7 @@ class Controls: 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) - self.steer_limited = False + self.steer_limited_by_controls = False self.desired_curvature = 0.0 self.pose_calibrator = PoseCalibrator() @@ -112,7 +112,7 @@ class Controls: self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) actuators.curvature = float(self.desired_curvature) steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited, self.desired_curvature, + self.steer_limited_by_controls, self.desired_curvature, self.calibrated_pose) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) @@ -161,10 +161,10 @@ class Controls: if self.sm['selfdriveState'].active: CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ - STEER_ANGLE_SATURATION_THRESHOLD + self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ + STEER_ANGLE_SATURATION_THRESHOLD else: - self.steer_limited = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 + self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 # TODO: both controlsState and carControl valids should be set by # sm.all_checks(), but this creates a circular dependency diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index f84f70be4f..f0121f73bc 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,14 +17,14 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited): - if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited and not CS.steeringPressed: + def _check_saturation(self, saturated, CS, steer_limited_by_controls): + if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 4162bd62dc..de07423fd4 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index dedc97a964..cf54125a77 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -43,6 +43,6 @@ class LatControlPID(LatControl): pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_steer) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls)) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f677b7f897..7b00cce56b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,7 +37,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -73,7 +73,7 @@ class LatControlTorque(LatControl): desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=True) - freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 + freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, feedforward=ff, speed=CS.vEgo, @@ -87,7 +87,7 @@ class LatControlTorque(LatControl): pid_log.output = float(-output_torque) pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls)) # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index 5294ffcc49..a2f437819f 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -49,7 +49,7 @@ class SteeringAccuracyTool: active = sm['controlsState'].active steer = sm['carOutput'].actuatorsOutput.torque standstill = sm['carState'].standstill - steer_limited = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 + steer_limited_by_controls = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 overriding = sm['carState'].steeringPressed changing_lanes = sm['modelV2'].meta.laneChangeState != 0 model_points = sm['modelV2'].position.y @@ -77,7 +77,7 @@ class SteeringAccuracyTool: self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) if len(model_points): self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0]) - if steer_limited: + if steer_limited_by_controls: self.speed_group_stats[group][angle_abs]["limited"] += 1 if control_state.saturated: self.speed_group_stats[group][angle_abs]["saturated"] += 1 From 41346b2cc72f6a699b4497b07363f78df7c3786d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Mar 2025 16:40:35 -0800 Subject: [PATCH 04/17] agnos 11.10 (#34764) --- launch_env.sh | 2 +- system/hardware/tici/agnos.json | 12 ++++---- system/hardware/tici/all-partitions.json | 36 ++++++++++++------------ 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/launch_env.sh b/launch_env.sh index 88c6550443..049b9da279 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="11.9" + export AGNOS_VERSION="11.10" fi export STAGING_ROOT="/data/safe_staging" diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 6538560363..d42c8151ab 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -67,17 +67,17 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75.img.xz", - "hash": "affcc08700026f1726ef16337e031fffc5556435b2b0facea81c7ffb0b66560c", - "hash_raw": "061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75", + "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img.xz", + "hash": "e4872f4132111b7b28586d978dd01bb48ffa031e103d029ebede7613c1bc2aa6", + "hash_raw": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9", "size": 4404019200, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "cc86836c6fc1c54f24dd0395e57e0b1ac6efe44d3ece833ab3ed456fe46a55cf", + "ondevice_hash": "4e5e680b4ac387ddc974b32dd3d5ec1d76282511eab974866b3b72399034985e", "alt": { - "hash": "061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75", - "url": "https://commadist.azureedge.net/agnosupdate/system-061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75.img", + "hash": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9", + "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img", "size": 4404019200 } } diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json index 5b3478f746..e90e8c2ec4 100644 --- a/system/hardware/tici/all-partitions.json +++ b/system/hardware/tici/all-partitions.json @@ -350,51 +350,51 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75.img.xz", - "hash": "affcc08700026f1726ef16337e031fffc5556435b2b0facea81c7ffb0b66560c", - "hash_raw": "061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75", + "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img.xz", + "hash": "e4872f4132111b7b28586d978dd01bb48ffa031e103d029ebede7613c1bc2aa6", + "hash_raw": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9", "size": 4404019200, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "cc86836c6fc1c54f24dd0395e57e0b1ac6efe44d3ece833ab3ed456fe46a55cf", + "ondevice_hash": "4e5e680b4ac387ddc974b32dd3d5ec1d76282511eab974866b3b72399034985e", "alt": { - "hash": "061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75", - "url": "https://commadist.azureedge.net/agnosupdate/system-061783c4826369c7aa711266e8e1d9eecfd0a6d6c8afc6042e4045617512be75.img", + "hash": "5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9", + "url": "https://commadist.azureedge.net/agnosupdate/system-5612484e7f255659c0845de620e7c733afd2e1b939f9464f5ef039721bb7cba9.img", "size": 4404019200 } }, { "name": "userdata_90", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-fd9d27a3ef5c1f63b85721798ba4ea10f2a4c71d0c8d9b59362a99f24dfff54a.img.xz", - "hash": "b77b66ae8178519dbd72443ff7bdd21b97d0d4d76425472bedc7d369958de37b", - "hash_raw": "fd9d27a3ef5c1f63b85721798ba4ea10f2a4c71d0c8d9b59362a99f24dfff54a", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-16c037fa42ee99bc6ec92909efc8a8075a0e8a0232a7d90e39e7d40a7bd0ee8e.img.xz", + "hash": "c6fb215f2b297f7ff5b8f133bc5d687772b37f2fee42a44aa730e37a84a14e52", + "hash_raw": "16c037fa42ee99bc6ec92909efc8a8075a0e8a0232a7d90e39e7d40a7bd0ee8e", "size": 96636764160, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "52d2b8e2486e7fa0cdeb2a6512a8058db43544c905367a2e1b81a5ad4636d4b7" + "ondevice_hash": "7199262f209abbb07be5eece505ac4d4c7ba8957f2d9ff7b1ac1ef2063461665" }, { "name": "userdata_89", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-1042711c5f69146ff7680f09eab1b2eb6fe9da0411318e9ff913c27168cb0307.img.xz", - "hash": "cc24b1c78234f92cd8ef66541f3fb8924719fc4b7f42c26e26eb71ec21cd2e93", - "hash_raw": "1042711c5f69146ff7680f09eab1b2eb6fe9da0411318e9ff913c27168cb0307", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-62c2c41470282b581ec1bbbe0375fb3b6c66df2f4bc3dc6c6fdf796f1797f136.img.xz", + "hash": "d66f894436fa11d4ff00f8a84e54d9e23a6492b0087f69bb958d2ab0bdc6dfba", + "hash_raw": "62c2c41470282b581ec1bbbe0375fb3b6c66df2f4bc3dc6c6fdf796f1797f136", "size": 95563022336, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "80bf38b8b60ea1ef1fc0849af4e18bf114761f98f5371476e2f762d0f8463204" + "ondevice_hash": "f1d3685618f6d1bde24ce6109284c5d30ece2f4fd015be67e8b52ef7e06067a4" }, { "name": "userdata_30", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-6d429550c42616e3f252034305f238ce0e90926762992ff3f92322f226caa5b6.img.xz", - "hash": "657b052cf6434ef19ff73bdeaf5b2511e5c42f64594ae37bac467a12b4cd673f", - "hash_raw": "6d429550c42616e3f252034305f238ce0e90926762992ff3f92322f226caa5b6", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-3e71d8804c90a6dff5048edb976149f9ea177efa139dea47cb585cef76b26f6e.img.xz", + "hash": "1b201ecbd0e1573777811bf18fa90cb080bfbccb34a6dcfd39b412632e7ca699", + "hash_raw": "3e71d8804c90a6dff5048edb976149f9ea177efa139dea47cb585cef76b26f6e", "size": 32212254720, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "6437e5a5b7144103952950f2f59f8e70b98cd26ee5bfb06cbd0a13b2e2f343b7" + "ondevice_hash": "47c28e63209556442cd9d2fd06a6c0e7fcf35c8a4e4fbcc550b9c089429ad0e0" } ] \ No newline at end of file From 701868d5cddb1fa2a4e84289b0e45c4232322c72 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Mar 2025 18:21:02 -0800 Subject: [PATCH 05/17] firehose auto start + stats (#34747) * don't need that * stats * lil more * lil more * cleanup * tweaks --------- Co-authored-by: Comma Device --- common/params_keys.h | 2 +- selfdrive/ui/qt/offroad/firehose.cc | 99 +++++++++++++++-------------- selfdrive/ui/qt/offroad/firehose.h | 19 +++--- system/athena/athenad.py | 4 -- 4 files changed, 62 insertions(+), 62 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 40a69ebd88..2b540b744c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -8,6 +8,7 @@ inline static std::unordered_map keys = { {"AdbEnabled", PERSISTENT}, {"AlwaysOnDM", PERSISTENT}, {"ApiCache_Device", PERSISTENT}, + {"ApiCache_FirehoseStats", PERSISTENT}, {"AssistNowToken", PERSISTENT}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, @@ -36,7 +37,6 @@ inline static std::unordered_map keys = { {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, - {"FirehoseMode", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ForcePowerDown", PERSISTENT}, {"GitBranch", PERSISTENT}, diff --git a/selfdrive/ui/qt/offroad/firehose.cc b/selfdrive/ui/qt/offroad/firehose.cc index 7b48b0fd9a..77474d00c2 100644 --- a/selfdrive/ui/qt/offroad/firehose.cc +++ b/selfdrive/ui/qt/offroad/firehose.cc @@ -10,6 +10,9 @@ #include #include #include +#include +#include +#include FirehosePanel::FirehosePanel(SettingsWindow *parent) : QWidget((QWidget*)parent) { layout = new QVBoxLayout(this); @@ -28,7 +31,7 @@ FirehosePanel::FirehosePanel(SettingsWindow *parent) : QWidget((QWidget*)parent) content_layout->setSpacing(20); // Top description - QLabel *description = new QLabel(tr("openpilot learns to drive by watching humans, like you, drive.\n\nFirehose Mode allows you to maximize your training data uploads to improve openpilot's driving models. More data means bigger models with better Experimental Mode.")); + QLabel *description = new QLabel(tr("openpilot learns to drive by watching humans, like you, drive.\n\nFirehose Mode allows you to maximize your training data uploads to improve openpilot's driving models. More data means bigger models, which means better Experimental Mode.")); description->setStyleSheet("font-size: 45px; padding-bottom: 20px;"); description->setWordWrap(true); content_layout->addWidget(description); @@ -40,41 +43,16 @@ FirehosePanel::FirehosePanel(SettingsWindow *parent) : QWidget((QWidget*)parent) line->setStyleSheet("background-color: #444444; margin-top: 5px; margin-bottom: 5px;"); content_layout->addWidget(line); - enable_firehose = new ParamControl("FirehoseMode", tr("Enable Firehose Mode"), "", ""); + toggle_label = new QLabel(tr("Firehose Mode: ACTIVE")); + toggle_label->setStyleSheet("font-size: 60px; font-weight: bold; color: white;"); + content_layout->addWidget(toggle_label); - content_layout->addWidget(enable_firehose); - - // Create progress bar container - progress_container = new QFrame(); - progress_container->hide(); - QHBoxLayout *progress_layout = new QHBoxLayout(progress_container); - progress_layout->setContentsMargins(10, 0, 10, 10); - progress_layout->setSpacing(20); - - progress_bar = new QProgressBar(); - progress_bar->setRange(0, 100); - progress_bar->setValue(0); - progress_bar->setTextVisible(false); - progress_bar->setStyleSheet(R"( - QProgressBar { - background-color: #444444; - border-radius: 10px; - height: 20px; - } - QProgressBar::chunk { - background-color: #3498db; - border-radius: 10px; - } - )"); - progress_bar->setFixedHeight(40); - - // Progress text - progress_text = new QLabel(tr("0%")); - progress_text->setStyleSheet("font-size: 40px; font-weight: bold; color: white;"); - - progress_layout->addWidget(progress_text); - - content_layout->addWidget(progress_container); + // Add contribution label + contribution_label = new QLabel("0 minutes"); + contribution_label->setStyleSheet("font-size: 52px; margin-top: 10px; margin-bottom: 10px;"); + contribution_label->setWordWrap(true); + contribution_label->hide(); + content_layout->addWidget(contribution_label); // Add a separator before detailed instructions QFrame *line2 = new QFrame(); @@ -85,22 +63,49 @@ FirehosePanel::FirehosePanel(SettingsWindow *parent) : QWidget((QWidget*)parent) // Detailed instructions at the bottom detailed_instructions = new QLabel(tr( - "Follow these steps to get your device ready:
" - "\t1. Bring your device inside and connect to a good USB-C adapter
" - "\t2. Connect to Wi-Fi
" - "\t3. Enable the toggle
" - "\t4. Leave it connected for at least 30 minutes
" + "For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.
" "
" - "The toggle turns off once you restart your device. Repeat at least once a week for maximum effectiveness." - "

FAQ
" - "Does it matter how or where I drive? Nope, just drive as you normally would.
" - "What's a good USB-C adapter? Any fast phone or laptop charger should be fine.
" - "Do I need to be on Wi-Fi? Yes.
" - "Do I need to bring the device inside? No, you can enable once you're parked, however your uploads will be limited by your car's battery.
" + "Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.
" + "

" + "Frequently Asked Questions

" + "Does it matter how or where I drive? Nope, just drive as you normally would.

" + "What's a good USB-C adapter? Any fast phone or laptop charger should be fine.

" + "Does it matter which software I run? Yes, only upstream openpilot (and particular forks) are able to be used for training." )); - detailed_instructions->setStyleSheet("font-size: 40px; padding: 20px; color: #E4E4E4;"); + detailed_instructions->setStyleSheet("font-size: 40px; color: #E4E4E4;"); detailed_instructions->setWordWrap(true); content_layout->addWidget(detailed_instructions); layout->addWidget(content, 1); + + // Set up the API request for firehose stats + const QString dongle_id = QString::fromStdString(Params().get("DongleId")); + firehose_stats = new RequestRepeater(this, CommaApi::BASE_URL + "/v1/devices/" + dongle_id + "/firehose_stats", + "ApiCache_FirehoseStats", 30, true); + QObject::connect(firehose_stats, &RequestRepeater::requestDone, [=](const QString &response, bool success) { + if (success) { + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + QJsonObject json = doc.object(); + int count = json["firehose"].toInt(); + contribution_label->setText(tr("%1 %2 of your driving are in the training dataset so far.").arg(count).arg(count == 1 ? "segment" : "segments")); + contribution_label->show(); + } + }); + + QObject::connect(uiState(), &UIState::uiUpdate, this, &FirehosePanel::refresh); +} + +void FirehosePanel::refresh() { + auto deviceState = (*uiState()->sm)["deviceState"].getDeviceState(); + auto networkType = deviceState.getNetworkType(); + bool networkMetered = deviceState.getNetworkMetered(); + + bool is_active = !networkMetered && (networkType != cereal::DeviceState::NetworkType::NONE); + if (is_active) { + toggle_label->setText(tr("ACTIVE")); + toggle_label->setStyleSheet("font-size: 60px; font-weight: bold; color: #2ecc71;"); + } else { + toggle_label->setText(tr("INACTIVE: connect to unmetered network")); + toggle_label->setStyleSheet("font-size: 60px;"); + } } diff --git a/selfdrive/ui/qt/offroad/firehose.h b/selfdrive/ui/qt/offroad/firehose.h index 7f5899f9f0..9082cb0f99 100644 --- a/selfdrive/ui/qt/offroad/firehose.h +++ b/selfdrive/ui/qt/offroad/firehose.h @@ -2,10 +2,8 @@ #include #include -#include #include -#include "selfdrive/ui/qt/widgets/controls.h" -#include "common/params.h" +#include "selfdrive/ui/qt/request_repeater.h" // Forward declarations class SettingsWindow; @@ -17,12 +15,13 @@ public: private: QVBoxLayout *layout; - - ParamControl *enable_firehose; - QFrame *progress_container; - QProgressBar *progress_bar; - QLabel *progress_text; + QLabel *detailed_instructions; - - void updateFirehoseState(bool enabled); + QLabel *contribution_label; + QLabel *toggle_label; + + RequestRepeater *firehose_stats; + +private slots: + void refresh(); }; diff --git a/system/athena/athenad.py b/system/athena/athenad.py index 27440e84a6..b36bdb103d 100755 --- a/system/athena/athenad.py +++ b/system/athena/athenad.py @@ -508,10 +508,6 @@ def getSshAuthorizedKeys() -> str: def getGithubUsername() -> str: return Params().get("GithubUsername", encoding='utf8') or '' -@dispatcher.add_method -def getFirehoseMode() -> bool: - return Params().get_bool("FirehoseMode") or False - @dispatcher.add_method def getSimInfo(): return HARDWARE.get_sim_info() From 34a072890cc3c963ea6cb2cdaa23c96f3d802a93 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Mar 2025 18:39:12 -0800 Subject: [PATCH 06/17] fix translations --- selfdrive/ui/qt/offroad/firehose.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/qt/offroad/firehose.cc b/selfdrive/ui/qt/offroad/firehose.cc index 77474d00c2..ab1d0198c7 100644 --- a/selfdrive/ui/qt/offroad/firehose.cc +++ b/selfdrive/ui/qt/offroad/firehose.cc @@ -48,7 +48,7 @@ FirehosePanel::FirehosePanel(SettingsWindow *parent) : QWidget((QWidget*)parent) content_layout->addWidget(toggle_label); // Add contribution label - contribution_label = new QLabel("0 minutes"); + contribution_label = new QLabel(); contribution_label->setStyleSheet("font-size: 52px; margin-top: 10px; margin-bottom: 10px;"); contribution_label->setWordWrap(true); contribution_label->hide(); From 338b7c915fd37345fe925088c7444ddfdbab9e3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 3 Mar 2025 22:10:48 -0500 Subject: [PATCH 07/17] regen: use ci bucket for files (#34766) * Remove route meta mode * use get_url * Remove useless import --- selfdrive/test/process_replay/regen.py | 52 +++++++--------------- selfdrive/test/process_replay/regen_all.py | 2 +- 2 files changed, 18 insertions(+), 36 deletions(-) diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index e87b8347e1..4d00613f0a 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -12,10 +12,9 @@ from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKE check_openpilot_enabled, check_most_messages_valid, get_custom_params_from_lr from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES from openpilot.selfdrive.test.update_ci_routes import upload_route -from openpilot.tools.lib.route import Route from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType from openpilot.tools.lib.logreader import LogReader, LogIterable, save_log - +from openpilot.tools.lib.openpilotci import get_url class DummyFrameReader(BaseFrameReader): def __init__(self, w: int, h: int, frame_count: int, pix_val: int): @@ -55,45 +54,28 @@ def regen_segment( def setup_data_readers( - route: str, sidx: int, use_route_meta: bool, - needs_driver_cam: bool = True, needs_road_cam: bool = True, dummy_driver_cam: bool = False + route: str, sidx: int, needs_driver_cam: bool = True, needs_road_cam: bool = True, dummy_driver_cam: bool = False ) -> tuple[LogReader, dict[str, Any]]: - if use_route_meta: - r = Route(route) - lr = LogReader(r.log_paths()[sidx]) - frs = {} - if needs_road_cam and len(r.camera_paths()) > sidx and r.camera_paths()[sidx] is not None: - frs['roadCameraState'] = FrameReader(r.camera_paths()[sidx]) - if needs_road_cam and len(r.ecamera_paths()) > sidx and r.ecamera_paths()[sidx] is not None: - frs['wideRoadCameraState'] = FrameReader(r.ecamera_paths()[sidx]) - if needs_driver_cam: - if dummy_driver_cam: - frs['driverCameraState'] = DummyFrameReader.zero_dcamera() - elif len(r.dcamera_paths()) > sidx and r.dcamera_paths()[sidx] is not None: - device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") - assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." - frs['driverCameraState'] = FrameReader(r.dcamera_paths()[sidx]) - else: - lr = LogReader(f"{route}/{sidx}/r") - frs = {} - if needs_road_cam: - frs['roadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") - if next((True for m in lr if m.which() == "wideRoadCameraState"), False): - frs['wideRoadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/ecamera.hevc") - if needs_driver_cam: - if dummy_driver_cam: - frs['driverCameraState'] = DummyFrameReader.zero_dcamera() - else: - device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") - assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." - frs['driverCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/dcamera.hevc") + lr = LogReader(f"{route}/{sidx}/r") + frs = {} + if needs_road_cam: + frs['roadCameraState'] = FrameReader(get_url(route, str(sidx), "fcamera.hevc")) + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs['wideRoadCameraState'] = FrameReader(get_url(route, str(sidx), "ecamera.hevc")) + if needs_driver_cam: + if dummy_driver_cam: + frs['driverCameraState'] = DummyFrameReader.zero_dcamera() + else: + device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") + assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." + frs['driverCameraState'] = FrameReader(get_url(route, str(sidx), "dcamera.hevc")) return lr, frs def regen_and_save( route: str, sidx: int, processes: str | Iterable[str] = "all", outdir: str = FAKEDATA, - upload: bool = False, use_route_meta: bool = False, disable_tqdm: bool = False, dummy_driver_cam: bool = False + upload: bool = False, disable_tqdm: bool = False, dummy_driver_cam: bool = False ) -> str: if not isinstance(processes, str) and not hasattr(processes, "__iter__"): raise ValueError("whitelist_proc must be a string or iterable") @@ -110,7 +92,7 @@ def regen_and_save( replayed_processes = CONFIGS all_vision_pubs = {pub for cfg in replayed_processes for pub in cfg.vision_pubs} - lr, frs = setup_data_readers(route, sidx, use_route_meta, + lr, frs = setup_data_readers(route, sidx, needs_driver_cam="driverCameraState" in all_vision_pubs, needs_road_cam="roadCameraState" in all_vision_pubs or "wideRoadCameraState" in all_vision_pubs, dummy_driver_cam=dummy_driver_cam) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py index 656a5b89e1..78a90b420c 100755 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -17,7 +17,7 @@ def regen_job(segment, upload, disable_tqdm): sn = SegmentName(segment[1]) fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11)) try: - relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False, + relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm, dummy_driver_cam=True) relr = '|'.join(relr.split('/')[-2:]) return f' ("{segment[0]}", "{relr}"), ' From 9c9a060365582eed91548a6c3d1fb368f73bc56e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 19:11:53 -0800 Subject: [PATCH 08/17] saturated warning: remove redundant speed check --- selfdrive/selfdrived/selfdrived.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 784925c966..68716c1f64 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -331,8 +331,8 @@ class SelfdriveD: desired_lateral_accel = controlstate.desiredCurvature * (clipped_speed**2) undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2 turning = abs(desired_lateral_accel) > 1.0 - good_speed = CS.vEgo > 5 - if undershooting and turning and good_speed and lac.saturated: + # TODO: lac.saturated includes speed and other checks, should be pulled out + if undershooting and turning and lac.saturated: self.events.add(EventName.steerSaturated) # Check for FCW From 949aaaba2c6ddc57c54bbd7ca938b476d7b15545 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 19:34:33 -0800 Subject: [PATCH 09/17] regenerate problem segments --- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/process_replay/test_processes.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3c63f00300..d0f0d671d7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d343af55506b14a4011bfe43b2a49c7e3c387d75 \ No newline at end of file +a9487c6a5c2f0bb83c9d629ff64c8f64a4e4ca13 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 0fff34917f..927f54c0c3 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -42,22 +42,22 @@ source_segments = [ segments = [ ("BODY", "regenA67A128BCD8|2024-08-30--02-36-22--0"), - ("HYUNDAI", "regen9CBD921E93E|2024-08-30--02-38-51--0"), + ("HYUNDAI", "regenCCD47FEBC0C|2025-03-04--03-21-48--0"), ("HYUNDAI2", "regen306779F6870|2024-10-03--04-03-23--0"), - ("TOYOTA", "regen1CA7A48E6F7|2024-08-30--02-45-08--0"), + ("TOYOTA", "regen4A5115B248D|2025-03-04--03-21-43--0"), ("TOYOTA2", "regen6E484EDAB96|2024-08-30--02-47-37--0"), ("TOYOTA3", "regen4CE950B0267|2024-08-30--02-51-30--0"), - ("HONDA", "regenC8F0D6ADC5C|2024-08-30--02-54-01--0"), + ("HONDA", "regenB8CABEC09CC|2025-03-04--03-32-55--0"), ("HONDA2", "regen4B38A7428CD|2024-08-30--02-56-31--0"), ("CHRYSLER", "regenF3DBBA9E8DF|2024-08-30--02-59-03--0"), ("RAM", "regenDB02684E00A|2024-08-30--03-02-54--0"), - ("SUBARU", "regenAA1FF48CF1F|2024-08-30--03-06-45--0"), + ("SUBARU", "regen5E3347D0A0F|2025-03-04--03-23-55--0"), ("GM", "regen720F2BA4CF6|2024-08-30--03-09-15--0"), ("GM2", "regen9ADBECBCD1C|2024-08-30--03-13-04--0"), ("NISSAN", "regen58464878D07|2024-08-30--03-15-31--0"), ("VOLKSWAGEN", "regenED976DEB757|2024-08-30--03-18-02--0"), ("MAZDA", "regenACF84CCF482|2024-08-30--03-21-55--0"), - ("FORD", "regen756F8230C21|2024-11-07--00-08-24--0"), + ("FORD", "regenA75209BD115|2025-03-04--03-23-53--0"), ("RIVIAN", "bc095dc92e101734|000000db--ee9fe46e57--1"), ] From 6891b795c49070a8916946454a85fae0fddb84a6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 21:47:52 -0600 Subject: [PATCH 10/17] controls: limit max curvature from lateral acceleration (#34651) * limit max curvature with lateral accel too * not a guideline * roll compensation in curv clip * improve clipping and alerting * typo * clean up * no float * get ready * good idea * good * redundant * TODO * test * do max curvature clip last * flip --------- Co-authored-by: Bruce Wayne --- selfdrive/controls/controlsd.py | 8 ++-- selfdrive/controls/lib/drive_helpers.py | 38 +++++++++++++------ selfdrive/controls/lib/latcontrol.py | 7 ++-- selfdrive/controls/lib/latcontrol_angle.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 4 +- selfdrive/controls/lib/latcontrol_torque.py | 4 +- .../controls/lib/tests/test_latcontrol.py | 10 ++++- selfdrive/selfdrived/selfdrived.py | 2 +- 8 files changed, 50 insertions(+), 27 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 14fd37040e..dcc1c74794 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -109,11 +109,11 @@ class Controls: actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) # Steering PID loop and lateral MPC - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) - actuators.curvature = float(self.desired_curvature) + self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, lp.roll) + actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, self.desired_curvature, - self.calibrated_pose) # TODO what if not available + self.calibrated_pose, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs @@ -180,7 +180,7 @@ class Controls: cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] - cs.desiredCurvature = float(self.desired_curvature) + cs.desiredCurvature = self.desired_curvature cs.longControlState = self.LoC.long_control_state cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index dcb0093a18..41384abae8 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,6 @@ import numpy as np from cereal import log +from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.realtime import DT_CTRL MIN_SPEED = 1.0 @@ -7,20 +8,33 @@ CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 # This is a turn radius smaller than most cars can achieve MAX_CURVATURE = 0.2 +MAX_VEL_ERR = 5.0 # m/s # EU guidelines -MAX_LATERAL_JERK = 5.0 -MAX_VEL_ERR = 5.0 - -def clip_curvature(v_ego, prev_curvature, new_curvature): - new_curvature = np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) - v_ego = max(MIN_SPEED, v_ego) - max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - safe_desired_curvature = np.clip(new_curvature, - prev_curvature - max_curvature_rate * DT_CTRL, - prev_curvature + max_curvature_rate * DT_CTRL) - - return safe_desired_curvature +MAX_LATERAL_JERK = 5.0 # m/s^3 +MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 + + +def clamp(val, min_val, max_val): + clamped_val = float(np.clip(val, min_val, max_val)) + return clamped_val, clamped_val != val + + +def clip_curvature(v_ego, prev_curvature, new_curvature, roll): + # This function respects ISO lateral jerk and acceleration limits + a max curvature + v_ego = max(v_ego, MIN_SPEED) + max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + new_curvature = np.clip(new_curvature, + prev_curvature - max_curvature_rate * DT_CTRL, + prev_curvature + max_curvature_rate * DT_CTRL) + + roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY + max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation + min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation + new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) + + new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) + return float(new_curvature), limited_accel or limited_max_curv def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index f0121f73bc..dcf0003430 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,14 +17,15 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_controls): - if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: + def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited): + # Saturated only if control output is not being limited by car torque/angle rate limits + if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index de07423fd4..1b249a3d1a 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: @@ -23,7 +23,7 @@ class LatControlAngle(LatControl): angle_steers_des += params.angleOffsetDeg angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False)) + angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index cf54125a77..1f1199565e 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -43,6 +43,6 @@ class LatControlPID(LatControl): pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_steer) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7b00cce56b..3aef57baca 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,7 +37,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -87,7 +87,7 @@ class LatControlTorque(LatControl): pid_log.output = float(-output_torque) pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index c7038dd581..564c93be0d 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -33,7 +33,15 @@ class TestLatControl: lp = generate_livePose() pose = Pose.from_live_pose(lp.livePose) + # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) + assert lac_log.saturated + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) + assert not lac_log.saturated + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) assert lac_log.saturated diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 68716c1f64..8b7db19c9a 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -328,7 +328,7 @@ class SelfdriveD: if lac.active and not recent_steer_pressed and not self.CP.notCar: clipped_speed = max(CS.vEgo, MIN_LATERAL_CONTROL_SPEED) actual_lateral_accel = controlstate.curvature * (clipped_speed**2) - desired_lateral_accel = controlstate.desiredCurvature * (clipped_speed**2) + desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2) undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2 turning = abs(desired_lateral_accel) > 1.0 # TODO: lac.saturated includes speed and other checks, should be pulled out From f818c5528f808351e5350e26c88bce4f84a786b5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Mar 2025 22:24:16 -0600 Subject: [PATCH 11/17] bump opendbc (#34768) bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index ca4f77b010..34a109cc79 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ca4f77b0101d85a5e8d292e2e61f7a8c7492b9e2 +Subproject commit 34a109cc7956dbf022f57b6bca2207f2719e25a7 From e5ba737db0f1991a02896480387dadca6ac46bf4 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Mar 2025 22:56:59 -0800 Subject: [PATCH 12/17] Revert "Filet o Fish model (#34637)" This reverts commit 159b1c9eb48551e6ac1d5f9281ac2218f320f499. --- selfdrive/modeld/constants.py | 8 +----- selfdrive/modeld/modeld.py | 31 +++++++-------------- selfdrive/modeld/models/commonmodel.cc | 9 +++--- selfdrive/modeld/models/commonmodel.h | 5 ++-- selfdrive/modeld/models/commonmodel.pxd | 2 +- selfdrive/modeld/models/commonmodel_pyx.pyx | 4 +-- selfdrive/modeld/models/driving_policy.onnx | 4 +-- selfdrive/modeld/models/driving_vision.onnx | 2 +- 8 files changed, 23 insertions(+), 42 deletions(-) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 5ca0a86bc8..2bb7b8100c 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -14,14 +14,8 @@ class ModelConstants: # model inputs constants MODEL_FREQ = 20 - HISTORY_FREQ = 5 - HISTORY_LEN_SECONDS = 5 - TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ - FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS - INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS - FEATURE_LEN = 512 - + FULL_HISTORY_BUFFER_LEN = 100 DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 LAT_PLANNER_STATE_LEN = 4 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ae1e75e3e9..ee825d158f 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -56,24 +56,16 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): - self.frames = { - 'input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP), - 'big_input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) - } + self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)} self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) - self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32) - self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) - self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP) - # policy inputs self.numpy_inputs = { - 'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32), + 'desire': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32), 'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32), 'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32), - 'prev_desired_curv': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), - 'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), + 'prev_desired_curv': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), + 'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), } with open(VISION_METADATA_PATH, 'rb') as f: @@ -112,9 +104,8 @@ class ModelState: new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - self.full_desire[0,:-1] = self.full_desire[0,1:] - self.full_desire[0,-1] = new_desire - self.numpy_inputs['desire'][:] = self.full_desire[0, self.temporal_idxs] + self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:] + self.numpy_inputs['desire'][0,-1] = new_desire self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] @@ -137,17 +128,15 @@ class ModelState: self.vision_output = self.vision_run(**self.vision_inputs).numpy().flatten() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) - self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] - self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :] - self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] + self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:] + self.numpy_inputs['features_buffer'][0,-1] = vision_outputs_dict['hidden_state'][0, :] self.policy_output = self.policy_run(**self.policy_inputs).numpy().flatten() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) # 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,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:] + self.numpy_inputs['prev_desired_curv'][0,-1,:] = policy_outputs_dict['desired_curvature'][0, :] combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 99155d9f1e..9973d18588 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -5,12 +5,11 @@ #include "common/clutil.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip) : ModelFrame(device_id, context) { +DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) { input_frames = std::make_unique(buf_size); - temporal_skip = _temporal_skip; input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); - img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (temporal_skip+1)*frame_size_bytes, NULL, &err)); - region.origin = temporal_skip * frame_size_bytes; + img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 2*frame_size_bytes, NULL, &err)); + region.origin = 1 * frame_size_bytes; region.size = frame_size_bytes; last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err)); @@ -21,7 +20,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - for (int i = 0; i < temporal_skip; i++) { + for (int i = 0; i < 1; i++) { CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr)); } loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 176d7eb6dc..14409943e4 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -64,21 +64,20 @@ protected: class DrivingModelFrame : public ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip); + DrivingModelFrame(cl_device_id device_id, cl_context context); ~DrivingModelFrame(); cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; - const int buf_size = MODEL_FRAME_SIZE * 2; // 2 frames are temporal_skip frames apart + const int buf_size = MODEL_FRAME_SIZE * 2; const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); private: LoadYUVState loadyuv; cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl; cl_buffer_region region; - int temporal_skip; }; class MonitoringModelFrame : public ModelFrame { diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index 4ac64d9172..b4f08b12aa 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -20,7 +20,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h": cppclass DrivingModelFrame: int buf_size - DrivingModelFrame(cl_device_id, cl_context, int) + DrivingModelFrame(cl_device_id, cl_context) cppclass MonitoringModelFrame: int buf_size diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index 5b7d11bc71..7b3a5bb342 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -59,8 +59,8 @@ cdef class ModelFrame: cdef class DrivingModelFrame(ModelFrame): cdef cppDrivingModelFrame * _frame - def __cinit__(self, CLContext context, int temporal_skip): - self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip) + def __cinit__(self, CLContext context): + self._frame = new cppDrivingModelFrame(context.device_id, context.context) self.frame = (self._frame) self.buf_size = self._frame.buf_size diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index cc2148c74b..f804b4ec31 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:5fd38c3b4d465d14a8ed128f8b1b265bff1589d42f398faa678b93f25b3385dc -size 15966721 +oid sha256:5cae3285c876804e649b14adadcfb8be79a9bd5a1b928113e37f1f08e25e9688 +size 16581121 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index aee6d8f1bf..06c87d8755 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 +oid sha256:29bbf79f9dfd7048c0013bb81e86d9b2979275b95ea1ed8a86d1a86a88695240 size 34882971 From fe4585ee884a9e694671a194a9382a1c5cef5736 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Tue, 4 Mar 2025 16:27:25 -0500 Subject: [PATCH 13/17] tools: notebook to search commaCarSegments for a CAN ID (#34771) --- .../examples/find_segments_with_message.ipynb | 279 ++++++++++++++++++ 1 file changed, 279 insertions(+) create mode 100644 tools/car_porting/examples/find_segments_with_message.ipynb diff --git a/tools/car_porting/examples/find_segments_with_message.ipynb b/tools/car_porting/examples/find_segments_with_message.ipynb new file mode 100644 index 0000000000..82b06c1f31 --- /dev/null +++ b/tools/car_porting/examples/find_segments_with_message.ipynb @@ -0,0 +1,279 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 30, + "id": "228a6736-de31-4255-9d72-a6ff391b968d", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Found 221 qualifying vehicles:\n" + ] + } + ], + "source": [ + "from opendbc.car import structs\n", + "from opendbc.car.values import PLATFORMS\n", + "\n", + "print(f\"Found {len(PLATFORMS)} qualifying vehicles:\")\n", + "# for platform in PLATFORMS:\n", + "# print(f\" {platform}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 31, + "id": "ed1c8aec-c274-4c61-b83d-711ea194bf86", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Collecting segments from commaCarSegments dataset:\n", + "No segments available for for DODGE_DURANGO\n", + "No segments available for for FORD_RANGER_MK2\n", + "No segments available for for HOLDEN_ASTRA\n", + "No segments available for for CADILLAC_ATS\n", + "No segments available for for CHEVROLET_MALIBU\n", + "No segments available for for CADILLAC_XT4\n", + "No segments available for for CHEVROLET_VOLT_2019\n", + "No segments available for for CHEVROLET_TRAVERSE\n", + "No segments available for for GMC_YUKON\n", + "No segments available for for HONDA_ODYSSEY_CHN\n", + "No segments available for for HYUNDAI_KONA_2022\n", + "No segments available for for HYUNDAI_NEXO_1ST_GEN\n", + "No segments available for for GENESIS_GV70_ELECTRIFIED_1ST_GEN\n", + "No segments available for for GENESIS_G80_2ND_GEN_FL\n", + "No segments available for for RIVIAN_R1_GEN1\n", + "No segments available for for SUBARU_FORESTER_HYBRID\n", + "No segments available for for TESLA_MODEL_3\n", + "No segments available for for TESLA_MODEL_Y\n", + "No segments available for for TOYOTA_RAV4_PRIME\n", + "No segments available for for TOYOTA_SIENNA_4TH_GEN\n", + "No segments available for for LEXUS_LC_TSS2\n", + "No segments available for for VOLKSWAGEN_CADDY_MK3\n", + "No segments available for for VOLKSWAGEN_CRAFTER_MK2\n", + "No segments available for for VOLKSWAGEN_JETTA_MK6\n", + "Collected 577 segments for analysis\n" + ] + } + ], + "source": [ + "import random\n", + "\n", + "from openpilot.tools.lib.logreader import LogReader\n", + "from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database\n", + "\n", + "database = get_comma_car_segments_database()\n", + "TEST_SEGMENTS = []\n", + "\n", + "MAX_SEGS_PER_PLATFORM = 3 # Increase this to search more segments\n", + "\n", + "print(\"Collecting segments from commaCarSegments dataset:\")\n", + "for platform in TEST_PLATFORMS:\n", + " if platform not in database:\n", + " print(f\"No segments available for for {platform}\")\n", + " continue\n", + " \n", + " all_segments = database[platform]\n", + "\n", + " NUM_SEGMENTS = min(len(all_segments), MAX_SEGS_PER_PLATFORM)\n", + "\n", + " # print(f\"Got {len(all_segments)} segments for platform {platform}, sampling {NUM_SEGMENTS} segments\")\n", + " TEST_SEGMENTS.extend(random.sample(all_segments, NUM_SEGMENTS))\n", + "\n", + "print(f\"Collected {len(TEST_SEGMENTS)} segments for analysis\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": 33, + "id": "0c75e8f2-4f5f-4f89-b8db-5223a6534a9f", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Match found in d753e5427226080e/2023-12-30--10-14-03/9/s for CHRYSLER PACIFICA HYBRID 2018\n", + "Match found in 810855dc1450c716/2023-12-26--10-03-20/3/s for CHRYSLER PACIFICA HYBRID 2018\n", + "Match found in 7f268b92fc672c4d/2023-11-02--20-54-34/103/s for CHRYSLER PACIFICA HYBRID 2018\n", + "Match found in 1a10807c60c127fd/2023-12-09--19-21-30/49/s for CHRYSLER PACIFICA HYBRID 2019\n", + "Match found in b70b56b76a6217f2/2023-12-26--12-29-24/4/s for CHRYSLER PACIFICA HYBRID 2019\n", + "Match found in 86deeb65f10c3129/2023-12-29--07-38-12/179/s for CHRYSLER PACIFICA HYBRID 2019\n", + "Match found in 0a3e89f78b1d0071/2023-11-30--10-33-13/13/s for CHRYSLER PACIFICA 2018\n", + "Match found in 8fc6a1b72c8b1357/2023-11-30--17-35-04/10/s for CHRYSLER PACIFICA 2018\n", + "Match found in d5de6f3e9371e89e/2023-11-16--17-19-54/48/s for CHRYSLER PACIFICA 2018\n", + "Match found in 5fc1717be83fc0da/2024-01-03--13-16-03/27/s for CHRYSLER PACIFICA 2020\n", + "Match found in c1feb10d20626c65/2023-12-16--17-46-24/9/s for CHRYSLER PACIFICA 2020\n", + "Match found in c1feb10d20626c65/2023-11-21--17-40-18/25/s for CHRYSLER PACIFICA 2020\n", + "Match found in a63a23c3e628f288/2023-11-05--18-36-20/22/s for JEEP GRAND CHEROKEE V6 2018\n", + "Match found in 46b21f1c5f7aa885/2023-11-14--15-22-04/21/s for JEEP GRAND CHEROKEE V6 2018\n", + "Match found in a63a23c3e628f288/2023-12-13--09-15-47/27/s for JEEP GRAND CHEROKEE V6 2018\n", + "Match found in 4ffe7a3fb831a625/2024-01-04--09-15-33/22/s for JEEP GRAND CHEROKEE 2019\n", + "Match found in c5b49919572d2c5a/2023-12-17--15-54-06/73/s for JEEP GRAND CHEROKEE 2019\n", + "Match found in 9c2b832d06f93621/2024-01-25--13-52-19/8/s for JEEP GRAND CHEROKEE 2019\n", + "Match found in 1618132d68afc876/2023-11-24--10-21-12/4/s for HYUNDAI KONA ELECTRIC 2ND GEN\n", + "Match found in 1618132d68afc876/2023-08-27--09-32-14/13/s for HYUNDAI KONA 2ND GEN\n", + "Match found in 1618132d68afc876/2023-11-24--10-21-12/3/s for HYUNDAI KONA ELECTRIC 2ND GEN\n", + "Match found in df7fdd56970d90fe/2023-12-29--07-48-56/53/s for HYUNDAI IONIQ 6 2023\n", + "Match found in 0ad7facc77922c3e/2023-12-29--17-12-45/14/s for HYUNDAI IONIQ 6 2023\n", + "Match found in 0ad7facc77922c3e/2023-12-16--08-49-13/39/s for HYUNDAI IONIQ 6 2023\n", + "Match found in 78ad5150de133637/2023-09-13--16-15-57/0/s for KIA K8 HYBRID 1ST GEN\n", + "Match found in 78ad5150de133637/2023-09-13--16-15-57/2/s for KIA K8 HYBRID 1ST GEN\n", + "Match found in 78ad5150de133637/2023-09-13--16-15-57/1/s for KIA K8 HYBRID 1ST GEN\n", + "Match found in b153671049a867b3/2023-12-15--20-40-16/0/s for KIA NIRO EV 2ND GEN\n", + "Match found in b153671049a867b3/2024-01-12--17-43-31/2/s for KIA NIRO EV 2ND GEN\n", + "Match found in b153671049a867b3/2023-12-03--21-08-30/3/s for KIA NIRO EV 2ND GEN\n", + "Match found in 7387ab2a564c6d9f/2023-12-02--07-10-34/52/s for KIA NIRO HYBRID 2ND GEN\n", + "Match found in 7387ab2a564c6d9f/2024-01-06--16-24-36/1/s for KIA NIRO HYBRID 2ND GEN\n", + "Match found in 7387ab2a564c6d9f/2024-01-06--16-24-36/9/s for KIA NIRO HYBRID 2ND GEN\n", + "Match found in 2efdac7dc41207f1/2023-12-27--07-24-22/89/s for KIA EV6 2022\n", + "Match found in 5ac586afbb236b5d/2023-12-05--18-23-10/7/s for VOLKSWAGEN ARTEON 1ST GEN\n", + "Match found in c34ef43981b6d888/2024-01-18--16-00-06/9/s for VOLKSWAGEN ARTEON 1ST GEN\n", + "Match found in 5ac586afbb236b5d/2024-01-04--20-01-19/4/s for VOLKSWAGEN ARTEON 1ST GEN\n", + "Match found in 16758e226aa2f747/2023-12-16--10-24-39/13/s for VOLKSWAGEN ATLAS 1ST GEN\n", + "Match found in 3174d252cec3bf6f/2023-12-12--18-39-06/6/s for VOLKSWAGEN ATLAS 1ST GEN\n", + "Match found in e80066d826c705a9/2024-01-06--17-28-45/211/s for VOLKSWAGEN ATLAS 1ST GEN\n", + "Match found in 2cdded3a5da75b6a/2024-01-06--13-37-16/2/s for VOLKSWAGEN GOLF 7TH GEN\n", + "Match found in abe0b7fe730c51a9/2023-10-30--14-24-49/4/s for VOLKSWAGEN GOLF 7TH GEN\n", + "Match found in 8fe9ced03c94e256/2023-12-14--16-31-55/8/s for VOLKSWAGEN GOLF 7TH GEN\n", + "Match found in a97d27ce2a78958a/2024-01-09--06-57-35/9/s for VOLKSWAGEN JETTA 7TH GEN\n", + "Match found in 0649d406184b7131/2024-01-20--14-43-41/3/s for VOLKSWAGEN JETTA 7TH GEN\n", + "Match found in 0649d406184b7131/2023-11-07--16-30-10/17/s for VOLKSWAGEN JETTA 7TH GEN\n", + "Match found in 5405cac0c9d58c3d/2023-11-05--17-13-17/6/s for VOLKSWAGEN PASSAT 8TH GEN\n", + "Match found in 2d7d87433a67c925/2024-01-05--16-31-16/38/s for VOLKSWAGEN PASSAT 8TH GEN\n", + "Match found in 2d7d87433a67c925/2023-12-26--08-44-17/2/s for VOLKSWAGEN PASSAT 8TH GEN\n", + "Match found in 0cd0b7f7e31a3853/2021-11-03--19-30-22/0/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 0bbe367c98fa1538/2023-07-01--19-42-26/7/s for VOLKSWAGEN POLO 6TH GEN\n", + "Match found in 0cd0b7f7e31a3853/2021-11-03--19-30-22/2/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 7d82b2f3a9115f1f/2021-10-21--15-39-42/1/s for VOLKSWAGEN TAOS 1ST GEN\n", + "Match found in 7d82b2f3a9115f1f/2021-10-21--15-39-42/0/s for VOLKSWAGEN TAOS 1ST GEN\n", + "Match found in 7d82b2f3a9115f1f/2021-10-21--15-39-42/2/s for VOLKSWAGEN TAOS 1ST GEN\n", + "Match found in 2744c89a8dda9a51/2021-07-24--21-28-06/0/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 2744c89a8dda9a51/2021-07-24--21-28-06/2/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 2744c89a8dda9a51/2021-07-24--21-28-06/1/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 4e144f64a2535890/2023-12-05--12-22-10/5/s for VOLKSWAGEN TIGUAN 2ND GEN\n", + "Match found in 1b5cf3b409037cc4/2023-12-11--10-23-35/5/s for VOLKSWAGEN TIGUAN 2ND GEN\n", + "Match found in 1b41980e9972d348/2023-10-29--15-13-08/3/s for VOLKSWAGEN TIGUAN 2ND GEN\n", + "Match found in a589dcc642fdb10a/2021-06-14--20-54-26/0/s for VOLKSWAGEN TOURAN 2ND GEN\n", + "Match found in a589dcc642fdb10a/2021-06-14--20-54-26/2/s for VOLKSWAGEN TOURAN 2ND GEN\n", + "Match found in a589dcc642fdb10a/2021-06-14--20-54-26/1/s for VOLKSWAGEN TOURAN 2ND GEN\n", + "Match found in 207b9d7ee3fb513a/2023-12-19--14-59-25/14/s for VOLKSWAGEN TRANSPORTER T6.1\n", + "Match found in a459f4556782eba1/2023-11-05--17-16-20/22/s for VOLKSWAGEN TRANSPORTER T6.1\n", + "Match found in a459f4556782eba1/2023-11-27--12-27-58/47/s for VOLKSWAGEN TRANSPORTER T6.1\n", + "Match found in 0cd0b7f7e31a3853/2021-11-18--00-38-32/0/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 0cd0b7f7e31a3853/2021-11-18--00-38-32/1/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 0cd0b7f7e31a3853/2021-11-18--00-38-32/2/s for SKODA KODIAQ 1ST GEN\n", + "Match found in a973e9182bc96a2c/2024-01-14--15-14-33/54/s for AUDI A3 3RD GEN\n", + "Match found in 23daff6c438612d2/2023-12-21--15-37-32/22/s for AUDI A3 3RD GEN\n", + "Match found in 23daff6c438612d2/2024-01-17--09-31-14/20/s for AUDI A3 3RD GEN\n", + "Match found in 6c6b466346192818/2021-06-06--14-17-47/0/s for AUDI Q2 1ST GEN\n", + "Match found in 6c6b466346192818/2021-06-06--14-17-47/1/s for AUDI Q2 1ST GEN\n", + "Match found in 6c6b466346192818/2021-06-06--14-17-47/2/s for AUDI Q2 1ST GEN\n", + "Match found in 839125529d3c3cdf/2023-11-29--15-49-50/56/s for AUDI Q3 2ND GEN\n", + "Match found in d7bfab70d5011e1d/2023-11-24--00-49-14/24/s for AUDI Q3 2ND GEN\n", + "Match found in 98a3015c4f16846d/2023-11-04--12-32-32/76/s for AUDI Q3 2ND GEN\n", + "Match found in fc6b6c9a3471c846/2021-05-27--13-39-56/2/s for SEAT LEON 3RD GEN\n", + "Match found in fc6b6c9a3471c846/2021-05-27--13-39-56/1/s for SEAT LEON 3RD GEN\n", + "Match found in aef9c04d6ec5cd57/2023-07-15--16-49-38/12/s for SEAT LEON 3RD GEN\n", + "Match found in 0bbe367c98fa1538/2023-03-04--17-46-11/2/s for VOLKSWAGEN POLO 6TH GEN\n", + "Match found in 0bbe367c98fa1538/2023-03-04--17-46-11/1/s for VOLKSWAGEN POLO 6TH GEN\n", + "Match found in 0bbe367c98fa1538/2023-03-04--17-46-11/0/s for VOLKSWAGEN POLO 6TH GEN\n", + "Match found in 026b6d18fba6417f/2021-03-26--09-17-04/0/s for SKODA SCALA 1ST GEN\n", + "Match found in 026b6d18fba6417f/2021-03-26--09-17-04/1/s for SKODA SCALA 1ST GEN\n", + "Match found in 026b6d18fba6417f/2021-03-26--09-17-04/2/s for SKODA SCALA 1ST GEN\n", + "Match found in 12d6ae3057c04b0d/2021-09-04--21-21-21/1/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 12d6ae3057c04b0d/2021-09-04--21-21-21/0/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 7d76baac7cf42e15/2024-01-13--03-42-46/2/s for SKODA KAROQ 1ST GEN\n", + "Match found in 8a5da7c1d1783730/2024-01-17--06-32-18/4/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 8a5da7c1d1783730/2023-12-23--16-48-07/42/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 2ca49a03c8084514/2023-12-27--08-54-31/4/s for SKODA KODIAQ 1ST GEN\n", + "Match found in 66e5edc3a16459c5/2021-05-25--19-00-29/1/s for SKODA OCTAVIA 3RD GEN\n", + "Match found in f5392228324cda8d/2023-10-29--14-05-24/120/s for SKODA OCTAVIA 3RD GEN\n", + "Match found in 3fa7570f2fcace3d/2023-11-11--07-57-34/1/s for SKODA OCTAVIA 3RD GEN\n", + "Match found in 54ada937747ef8ca/2023-11-09--16-13-28/12/s for SKODA SUPERB 3RD GEN\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Match found in 54ada937747ef8ca/2023-12-14--08-14-41/8/s for SKODA SUPERB 3RD GEN\n", + "Match found in 54ada937747ef8ca/2023-11-16--06-01-05/7/s for SKODA SUPERB 3RD GEN\n", + "Analysis finished\n" + ] + } + ], + "source": [ + "from openpilot.selfdrive.pandad import can_capnp_to_list\n", + "from openpilot.tools.lib.logreader import LogReader\n", + "\n", + "message_to_check = 0x3c0 # Volkswagen CAN ignition message\n", + "\n", + "segment_count = len(TEST_SEGMENTS)\n", + "for idx, segment in enumerate(TEST_SEGMENTS):\n", + " lr = LogReader(segment)\n", + " CP = lr.first(\"carParams\")\n", + " if CP is None:\n", + " continue\n", + "\n", + " can_packets = [msg for msg in lr if msg.which() == \"can\"]\n", + "\n", + " # print(f\"Evaluating segment {idx} of {segment_count} for {CP.carFingerprint}\")\n", + "\n", + " matched_segments = {}\n", + " for packet in can_packets:\n", + " for msg in packet.can:\n", + " if msg.address == message_to_check:\n", + " matched_segments.update({segment: CP.carFingerprint})\n", + "\n", + " if segment in matched_segments:\n", + " print(f\"Match found in {segment} for {CP.carFingerprint}\")\n", + "\n", + "print(f\"Analysis finished\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7724dd97-f62e-4fd3-9f64-63d49be669d2", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9f393e00-8efd-40fb-a41e-d312531a83e8", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "openpilot", + "language": "python", + "name": "openpilot" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.0" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From bc422a07384afed95b4c2037022c3517ef277cba Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 4 Mar 2025 15:54:43 -0600 Subject: [PATCH 14/17] juggle: use DBC dict generator (#34772) * juggle: support tesla DBC * better * fix * sort --- tools/cabana/dbc/generate_dbc_json.py | 6 +++--- tools/plotjuggler/juggle.py | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/tools/cabana/dbc/generate_dbc_json.py b/tools/cabana/dbc/generate_dbc_json.py index 5e928e6054..d002a50ef8 100755 --- a/tools/cabana/dbc/generate_dbc_json.py +++ b/tools/cabana/dbc/generate_dbc_json.py @@ -7,7 +7,7 @@ from opendbc.car.fingerprints import MIGRATION from opendbc.car.values import PLATFORMS -def generate_dbc_json() -> str: +def generate_dbc_dict() -> dict[str, str]: dbc_map = {} for platform in PLATFORMS.values(): if platform != "MOCK": @@ -24,7 +24,7 @@ def generate_dbc_json() -> str: if MIGRATION[m] in dbc_map: dbc_map[m] = dbc_map[MIGRATION[m]] - return json.dumps(dict(sorted(dbc_map.items())), indent=2) + return dbc_map if __name__ == "__main__": @@ -35,5 +35,5 @@ if __name__ == "__main__": args = parser.parse_args() with open(args.out, 'w') as f: - f.write(generate_dbc_json()) + f.write(json.dumps(dict(sorted(generate_dbc_dict().items())), indent=2)) print(f"Generated and written to {args.out}") diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py index 5b6bd0a418..f66782d6f3 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -13,6 +13,7 @@ from functools import partial from opendbc.car.fingerprints import MIGRATION from openpilot.common.basedir import BASEDIR from openpilot.common.swaglog import cloudlog +from openpilot.tools.cabana.dbc.generate_dbc_json import generate_dbc_dict from openpilot.tools.lib.logreader import LogReader, ReadMode, save_log from openpilot.selfdrive.test.process_replay.migration import migrate_all @@ -90,11 +91,10 @@ def juggle_route(route_or_segment_name, can, layout, dbc, should_migrate): if dbc is None: try: CP = lr.first('carParams') - platform = CP.carFingerprint - DBC = __import__(f"opendbc.car.{CP.brand}.values", fromlist=['DBC']).DBC - dbc = DBC[MIGRATION.get(CP.carFingerprint, CP.carFingerprint)]['pt'] + platform = MIGRATION.get(CP.carFingerprint, CP.carFingerprint) + dbc = generate_dbc_dict()[platform] except Exception: - cloudlog.error("Failed to get DBC name from logs!") + cloudlog.exception("Failed to get DBC name from logs!") with tempfile.NamedTemporaryFile(suffix='.rlog', dir=juggle_dir) as tmp: save_log(tmp.name, all_data, compress=False) From b8362fd7250d323fb4e200822d772a4735385b5c Mon Sep 17 00:00:00 2001 From: Alexandre Nobuharu Sato <66435071+AlexandreSato@users.noreply.github.com> Date: Tue, 4 Mar 2025 20:25:01 -0300 Subject: [PATCH 15/17] Multilang: update pt-BR translations (#34773) update pt-BR translations --- selfdrive/ui/translations/main_pt-BR.ts | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index f2aa00d322..06b2ef7b35 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -309,25 +309,27 @@ FirehosePanel 🔥 Firehose Mode 🔥 - + 🔥 Mode Firehose 🔥 Enable Firehose Mode - + Habilitar Modo Firehose openpilot learns to drive by watching humans, like you, drive. Firehose Mode allows you to maximize your training data uploads to improve openpilot's driving models. More data means bigger models with better Experimental Mode. - + O openpilot aprende a dirigir observando humanos, como você, dirigirem. + +O Modo Firehose permite maximizar o envio de seus dados de treinamento para melhorar os modelos de direção do openpilot. Mais dados significam modelos maiores e um Modo Experimental melhor. 0% - 5G {0%?} + 5G {0%?} Follow these steps to get your device ready:<br> 1. Bring your device inside and connect to a good USB-C adapter<br> 2. Connect to Wi-Fi<br> 3. Enable the toggle<br> 4. Leave it connected for at least 30 minutes<br><br>The toggle turns off once you restart your device. Repeat at least once a week for maximum effectiveness.<br><br><b>FAQ</b><br><i>Does it matter how or where I drive?</i> Nope, just drive as you normally would.<br><i>What's a good USB-C adapter?</i> Any fast phone or laptop charger should be fine.<br><i>Do I need to be on Wi-Fi?</i> Yes.<br><i>Do I need to bring the device inside?</i> No, you can enable once you're parked, however your uploads will be limited by your car's battery.<br> - + Siga estes passos para preparar seu dispositivo:<br> 1. Leve o dispositivo para dentro e conecte-o a um bom adaptador USB-C<br> 2. Conecte-se ao Wi-Fi<br> 3. Ative a opção<br> 4. Deixe-o conectado por pelo menos 30 minutos<br><br>A opção será desativada quando você reiniciar o dispositivo. Repita esse processo pelo menos uma vez por semana para obter a máxima eficácia.<br><br><b>FAQ</b><br><i>Importa como ou onde eu dirijo?</i> Não, basta dirigir normalmente.<br><i>O que é um bom adaptador USB-C?</i> Qualquer carregador rápido de celular ou laptop deve servir..<br><i>Preciso estar conectado ao Wi-Fi?</i> Sim.<br><i>Preciso levar o dispositivo para dentro de casa?</i> Não, você pode ativar a opção enquanto estiver estacionado, mas seus envios serão limitados pela bateria do carro.<br> @@ -668,7 +670,7 @@ Isso pode levar até um minuto. Firehose - + Firehose @@ -1146,15 +1148,15 @@ Isso pode levar até um minuto. WiFiPromptWidget Open - + Abrir Maximize your training data uploads to improve openpilot's driving models. - + Maximize seus envios de dados de treinamento para melhorar os modelos de direção do openpilot. <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span> - + <span style='font-family: "Noto Color Emoji";'>🔥</span> Mode Firehose <span style='font-family: Noto Color Emoji;'>🔥</span> From 7dfbf0b1e1860540a083a3511db98414733d15bb Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 4 Mar 2025 18:28:44 -0800 Subject: [PATCH 16/17] process_replay: set sensor in cameraState migration (#34776) set sensor in cameraState migration --- selfdrive/test/process_replay/migration.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 061da31de3..2d42d31619 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -358,6 +358,7 @@ def migrate_cameraStates(msgs): new_msg = messaging.new_message(msg.which()) new_camera_state = getattr(new_msg, new_msg.which()) + new_camera_state.sensor = camera_state.sensor new_camera_state.frameId = encode_id new_camera_state.encodeId = encode_id # timestampSof was added later so it might be missing on some old segments From 3e8e7e8e6df51d00e0008bc09c2a47acab7e3fc5 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 4 Mar 2025 18:36:30 -0800 Subject: [PATCH 17/17] update model_replay route (#34774) * update route * Update selfdrive/test/process_replay/model_replay.py Co-authored-by: Maxime Desroches --------- Co-authored-by: Maxime Desroches --- selfdrive/test/process_replay/model_replay.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 50027574ba..8cd8222ddc 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -19,8 +19,8 @@ from openpilot.tools.lib.framereader import FrameReader, NumpyFrameReader from openpilot.tools.lib.logreader import LogReader, save_log from openpilot.tools.lib.github_utils import GithubUtils -TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30" -SEGMENT = 6 +TEST_ROUTE = "8494c69d3c710e81|000001d4--2648a9a404" +SEGMENT = 4 MAX_FRAMES = 100 if PC else 400 NO_MODEL = "NO_MODEL" in os.environ