diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index 4fa3af3d5ef647..3792e605d66d0d 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -109,7 +109,7 @@ jobs: - name: Build openpilot run: scons - name: Run unit tests - timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 20 }} + timeout-minutes: ${{ contains(runner.name, 'nsc') && 10 || 20 }} run: | source openpilot/selfdrive/test/setup_xvfb.sh # Pre-compile Python bytecode so each pytest worker doesn't need to @@ -132,7 +132,7 @@ jobs: - name: Build openpilot run: scons - name: Run replay - timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 20 }} + timeout-minutes: ${{ contains(runner.name, 'nsc') && 10 || 20 }} continue-on-error: ${{ github.ref == 'refs/heads/master' }} run: openpilot/selfdrive/test/process_replay/test_processes.py -j$(nproc) - name: Print diff diff --git a/SConstruct b/SConstruct index 031984bf4c6826..9807ad1485c884 100644 --- a/SConstruct +++ b/SConstruct @@ -233,9 +233,6 @@ Export('messaging') # Build other submodules SConscript(['panda/SConscript']) -# Build rednose library -SConscript(['rednose/SConscript']) - # Build system services SConscript([ 'openpilot/system/loggerd/SConscript', diff --git a/openpilot/selfdrive/locationd/SConscript b/openpilot/selfdrive/locationd/SConscript index e8eeff7e0cc102..203e619729b38b 100644 --- a/openpilot/selfdrive/locationd/SConscript +++ b/openpilot/selfdrive/locationd/SConscript @@ -1,4 +1,4 @@ -Import('env', 'rednose') +Import('env') # build ekf models rednose_gen_dir = 'models/generated' diff --git a/openpilot/selfdrive/locationd/locationd.py b/openpilot/selfdrive/locationd/locationd.py index 8e03995d132c02..65fb01323b0b95 100755 --- a/openpilot/selfdrive/locationd/locationd.py +++ b/openpilot/selfdrive/locationd/locationd.py @@ -86,7 +86,7 @@ def _validate_sensor_time(self, sensor_time: float, t: float): def _validate_timestamp(self, t: float): kf_t = self.kf.t - invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME + invalid = kf_t is not None and not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME if invalid: cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter") return not invalid @@ -97,6 +97,31 @@ def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray): cloudlog.error("Non-finite values detected, kalman reset") self.reset(t) + def _predict_and_observe_camera_odometry(self, t: float, rot_device: np.ndarray, rot_device_noise: np.ndarray, + trans_device: np.ndarray, trans_device_noise: np.ndarray): + ekf = self.kf.filter + # Camera rotation and translation have the same delayed timestamp. Rewind once, apply both updates, + # then fast-forward once instead of replaying all newer IMU observations twice. + if ekf.filter_time is not None and t < ekf.filter_time: + if len(ekf.rewind_t) == 0 or t < ekf.rewind_t[0] or t < ekf.rewind_t[-1] - ekf.max_rewind_age: + ekf.logger.error(f"observation too old at {t:.3f} with filter at {ekf.filter_time:.3f}, ignoring") + return None, None + rewound = ekf.rewind(t) + else: + rewound = [] + + rot_R = np.array([np.diag(rot_device_noise)]) + trans_R = np.array([np.diag(trans_device_noise)]) + cam_odo_rot_res = ekf._predict_and_update_batch(t, ObservationKind.CAMERA_ODO_ROTATION, np.atleast_2d(rot_device), + rot_R, [[]]) + cam_odo_trans_res = ekf._predict_and_update_batch(t, ObservationKind.CAMERA_ODO_TRANSLATION, + np.atleast_2d(trans_device), trans_R, [[]]) + + for r in rewound: + ekf._predict_and_update_batch(*r) + + return cam_odo_rot_res, cam_odo_trans_res + def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult: new_x, new_P = None, None if which == "accelerometer" and msg.which() == "acceleration": @@ -190,8 +215,8 @@ def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> H rot_device_noise = rot_device_std ** 2 trans_device_noise = trans_device_std ** 2 - cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)])) - cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)])) + cam_odo_rot_res, cam_odo_trans_res = self._predict_and_observe_camera_odometry(t, rot_device, rot_device_noise, + trans_device, trans_device_noise) self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) if cam_odo_rot_res is not None: _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res @@ -209,7 +234,8 @@ def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> H def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_initialized: bool): state, cov = self.kf.x, self.kf.P std = np.sqrt(np.diag(cov)) - filter_time_valid = bool(np.isfinite(self.kf.t)) + kf_t = self.kf.t + filter_time_valid = kf_t is not None and bool(np.isfinite(kf_t)) filter_valid = filter_initialized and filter_time_valid orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION] @@ -241,7 +267,7 @@ def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_initialized: b livePose.inputsOK = inputs_valid livePose.posenetOK = not std_spike or self.car_speed <= 5.0 livePose.sensorsOK = sensors_valid - livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9) + livePose.timestamp = int(np.nan_to_num(kf_t) * 1e9) if kf_t is not None else 0 return msg diff --git a/openpilot/selfdrive/locationd/models/car_kf.py b/openpilot/selfdrive/locationd/models/car_kf.py index 27cc4ef9c93fc5..1394a8aaeed235 100755 --- a/openpilot/selfdrive/locationd/models/car_kf.py +++ b/openpilot/selfdrive/locationd/models/car_kf.py @@ -15,7 +15,7 @@ import sympy as sp from rednose.helpers.ekf_sym import gen_code else: - from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + from rednose.helpers.ekf_sym import EKF_sym i = 0 @@ -163,8 +163,8 @@ def generate_code(generated_dir): def __init__(self, generated_dir): dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0] - self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, - dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) + self.filter = EKF_sym(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, + dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): self.filter.set_global("mass", mass) diff --git a/openpilot/selfdrive/locationd/models/pose_kf.py b/openpilot/selfdrive/locationd/models/pose_kf.py index a8ff80c7130542..255de32dc22cee 100755 --- a/openpilot/selfdrive/locationd/models/pose_kf.py +++ b/openpilot/selfdrive/locationd/models/pose_kf.py @@ -12,7 +12,7 @@ from rednose.helpers.ekf_sym import gen_code from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler else: - from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + from rednose.helpers.ekf_sym import EKF_sym EARTH_G = 9.81 @@ -102,8 +102,8 @@ def generate_code(generated_dir): def __init__(self, generated_dir, max_rewind_age): dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0] - self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, - dim_state, dim_state_err, max_rewind_age=max_rewind_age) + self.filter = EKF_sym(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, + dim_state, dim_state_err, max_rewind_age=max_rewind_age) if __name__ == "__main__": diff --git a/openpilot/selfdrive/locationd/test/test_locationd.py b/openpilot/selfdrive/locationd/test/test_locationd.py new file mode 100644 index 00000000000000..adfe5cd2791227 --- /dev/null +++ b/openpilot/selfdrive/locationd/test/test_locationd.py @@ -0,0 +1,8 @@ +from openpilot.selfdrive.locationd.locationd import LocationEstimator + + +def test_get_msg_uninitialized_filter_time(): + msg = LocationEstimator(False).get_msg(sensors_valid=True, inputs_valid=True, filter_initialized=False) + + assert not msg.valid + assert msg.livePose.timestamp == 0 diff --git a/rednose_repo b/rednose_repo index 7ffefa3d8811a8..6c2abeb471c830 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 7ffefa3d8811a842f8ec97d311103ce3a45dfae0 +Subproject commit 6c2abeb471c830b38abb0f95d3aa348ca2b8dbc6