diff --git a/.github/workflows/cereal_validation.yaml b/.github/workflows/cereal_validation.yaml index 9833f472fe..2290481c56 100644 --- a/.github/workflows/cereal_validation.yaml +++ b/.github/workflows/cereal_validation.yaml @@ -21,8 +21,8 @@ concurrency: env: PYTHONWARNINGS: error - BASE_IMAGE: sunnypilot-base - BUILD: release/ci/docker_build_sp.sh base + BASE_IMAGE: openpilot-base + BUILD: selfdrive/test/docker_build.sh base RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c jobs: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6f653a788c..360c1a4730 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -97,7 +97,7 @@ class Controls(ControlsExt, ModelStateBase): self.LaC.extension.update_model_v2(self.sm['modelV2']) - self.lat_delay = get_lat_delay(self.params, self.lat_delay, self.sm.updated["liveDelay"]) + self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) self.LaC.extension.update_lateral_lag(self.lat_delay) long_plan = self.sm['longitudinalPlan'] diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index ee12f2c36c..130a352c5b 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -180,7 +180,7 @@ class TorqueEstimator(ParameterEstimator): elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "liveDelay": - self.lag = get_lat_delay(self.params, self.lag, True) + self.lag = get_lat_delay(self.params, msg.lateralDelay) # calculate lateral accel from past steering torque elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 13f4b6e123..27bf4c6ebb 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -296,8 +296,10 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + LAT_SMOOTH_SECONDS - lateral_control_params = np.array([v_ego, model.lat_delay], dtype=np.float32) + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + LAT_SMOOTH_SECONDS + lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -343,7 +345,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, model.lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/sunnypilot/livedelay/helpers.py b/sunnypilot/livedelay/helpers.py index fe0760c1ca..0f7437ceea 100644 --- a/sunnypilot/livedelay/helpers.py +++ b/sunnypilot/livedelay/helpers.py @@ -7,8 +7,8 @@ See the LICENSE.md file in the root directory for more details. from openpilot.common.params import Params -def get_lat_delay(params: Params, cur_val: float, updated: bool) -> float: - if updated and params.get_bool("LagdToggle"): +def get_lat_delay(params: Params, stock_lat_delay: float) -> float: + if params.get_bool("LagdToggle"): return float(params.get("LagdValueCache", return_default=True)) - return cur_val + return stock_lat_delay diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 8e20e6b62a..b94e166bc6 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -248,8 +248,9 @@ def main(demo=False): v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + model.LAT_SMOOTH_SECONDS - + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -283,7 +284,7 @@ def main(demo=False): } if "lateral_control_params" in model.inputs.keys(): - inputs['lateral_control_params'] = np.array([max(v_ego, 0.), model.lat_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([max(v_ego, 0.), lat_delay], dtype=np.float32) if "driving_style" in model.inputs.keys(): inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) @@ -306,7 +307,7 @@ def main(demo=False): action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL) fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, - v_ego, model.lat_delay, model.meta) + v_ego, lat_delay, model.meta) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index cec54c0033..2dc1026c01 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -288,7 +288,9 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - model.lat_delay = get_lat_delay(params, model.lat_delay, sm.updated["liveDelay"]) + model.LAT_SMOOTH_SECONDS + if sm.frame % 60 == 0: + model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -325,7 +327,7 @@ def main(demo=False): } if "lateral_control_params" in model.numpy_inputs.keys(): - inputs['lateral_control_params'] = np.array([v_ego, model.lat_delay], dtype=np.float32) + inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) @@ -337,7 +339,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = model.get_action_from_model(model_output, prev_action, model.lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,