diff --git a/cereal/log.capnp b/cereal/log.capnp index 29e462e1c6..077dddd897 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2331,7 +2331,6 @@ struct Event { onroadEvents @68: List(Car.CarEvent); carParams @69: Car.CarParams; driverMonitoringState @71: DriverMonitoringState; - liveLocationKalman @72 :LiveLocationKalman; livePose @129 :LivePose; modelV2 @75 :ModelDataV2; drivingModelData @128 :DrivingModelData; @@ -2404,7 +2403,7 @@ struct Event { model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated liveMpcDEPRECATED @36 :LiveMpcData; liveLongitudinalMpcDEPRECATED @37 :LiveLongitudinalMpcData; - liveLocationKalmanDEPRECATED @51 :Legacy.LiveLocationData; + liveLocationKalmanLegacyDEPRECATED @51 :Legacy.LiveLocationData; orbslamCorrectionDEPRECATED @45 :Legacy.OrbslamCorrection; liveUIDEPRECATED @14 :Legacy.LiveUI; sensorEventDEPRECATED @4 :SensorEventData; @@ -2441,5 +2440,6 @@ struct Event { lateralPlanDEPRECATED @64 :LateralPlan; navModelDEPRECATED @104 :NavModelData; uiPlanDEPRECATED @106 :UiPlan; + liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; } } diff --git a/cereal/services.py b/cereal/services.py index f7aaa377a9..7d0a7d380a 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -48,7 +48,6 @@ _services: dict[str, tuple] = { "clocks": (True, 0.1, 1), "ubloxRaw": (True, 20.), "livePose": (True, 20., 4), - "liveLocationKalman": (True, 20.), "liveParameters": (True, 20., 5), "cameraOdometry": (True, 20., 10), "thumbnail": (True, 0.2, 1), diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 5607871f5f..e1e5cd1bb2 100644 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -17,9 +17,6 @@ const double TRANS_SANITY_CHECK = 200.0; // m/s const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) const double ALTITUDE_SANITY_CHECK = 10000; // m const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad -const double VALID_TIME_SINCE_RESET = 1.0; // s -const double VALID_POS_STD = 50.0; // m -const double MAX_RESET_TRACKER = 5.0; const double SANE_GPS_UNCERTAINTY = 1500.0; // m const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker const double RESET_TRACKER_DECAY = 0.99995; @@ -68,13 +65,7 @@ static Quaterniond vector2quat(const VectorXd& vec) { return Quaterniond(vec(0), vec(1), vec(2), vec(3)); } -static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { - meas.setValue(kj::arrayPtr(val.data(), val.size())); - meas.setStd(kj::arrayPtr(std.data(), std.size())); - meas.setValid(valid); -} - -static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXf& val, const VectorXf& std, bool valid) { +static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]); meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]); meas.setValid(valid); @@ -107,31 +98,7 @@ Localizer::Localizer(LocalizerGnssSource gnss_source) { this->configure_gnss_source(gnss_source); } -void Localizer::build_live_pose(cereal::LivePose::Builder& livePose, cereal::LiveLocationKalman::Reader& liveLocation) { - // Just copy the values from liveLocation to livePose for now - VectorXf orientation_ned = float32list2vector(liveLocation.getOrientationNED().getValue()), orientation_ned_std = float32list2vector(liveLocation.getOrientationNED().getStd()); - init_xyz_measurement(livePose.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); - VectorXf velocity_device = float32list2vector(liveLocation.getVelocityDevice().getValue()), velocity_device_std = float32list2vector(liveLocation.getVelocityDevice().getStd()); - init_xyz_measurement(livePose.initVelocityDevice(), velocity_device, velocity_device_std, true); - VectorXf acceleration_device = float32list2vector(liveLocation.getAccelerationDevice().getValue()), acceleration_device_std = float32list2vector(liveLocation.getAccelerationDevice().getStd()); - init_xyz_measurement(livePose.initAccelerationDevice(), acceleration_device, acceleration_device_std, true); - VectorXf ang_velocity_device = float32list2vector(liveLocation.getAngularVelocityDevice().getValue()), ang_velocity_device_std = float32list2vector(liveLocation.getAngularVelocityDevice().getStd()); - init_xyz_measurement(livePose.initAngularVelocityDevice(), ang_velocity_device, ang_velocity_device_std, true); - - if (DEBUG) { - VectorXd filter_state = float64list2vector(liveLocation.getFilterState().getValue()), filter_state_std = float64list2vector(liveLocation.getFilterState().getStd()); - cereal::LivePose::FilterState::Builder filter_state_builder = livePose.initFilterState(); - filter_state_builder.setValue(kj::arrayPtr(filter_state.data(), filter_state.size())); - filter_state_builder.setStd(kj::arrayPtr(filter_state_std.data(), filter_state_std.size())); - filter_state_builder.setValid(true); - } - - livePose.setInputsOK(liveLocation.getInputsOK()); - livePose.setPosenetOK(liveLocation.getPosenetOK()); - livePose.setSensorsOK(liveLocation.getSensorsOK()); -} - -void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { +void Localizer::build_live_pose(cereal::LivePose::Builder& fix) { VectorXd predicted_state = this->kf->get_x(); MatrixXdr predicted_cov = this->kf->get_P(); VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); @@ -141,20 +108,12 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); - VectorXd fix_pos_geo_vec = this->get_position_geodetic(); VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); - VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose()); - - VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); - MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); - VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); - VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); - VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); VectorXd vel_device = device_from_ecef * vel_ecef; VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); @@ -173,12 +132,8 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); - VectorXd vel_calib = this->calib_from_device * vel_device; - VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); - VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); - VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef); VectorXd nextfix_ecef = fix_ecef + vel_ecef; VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); @@ -188,26 +143,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); - Vector3d nans = Vector3d(NAN, NAN, NAN); - - // TODO fill in NED and Calibrated stds // write measurements to msg - init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); - init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); - init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode); - init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode); - init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); - init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); - init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); - init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); - init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); - init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); - init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); - init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); - init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); - init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); + init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); if (DEBUG) { - init_measurement(fix.initFilterState(), predicted_state, predicted_std, true); + cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState(); + filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size())); + filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size())); + filter_state_builder.setValid(true); } double old_mean = 0.0, new_mean = 0.0; @@ -224,26 +169,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { new_mean /= POSENET_STD_HIST_HALF; // experimentally found these values, no false positives in 20k minutes of driving bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); - fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); - fix.setDeviceStable(!this->device_fell); - fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); - fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); - this->device_fell = false; - - //fix.setGpsWeek(this->time.week); - //fix.setGpsTimeOfWeek(this->time.tow); - fix.setUnixTimestampMillis(this->unix_timestamp_millis); - - double time_since_reset = this->kf->get_filter_time() - this->last_reset_time; - fix.setTimeSinceReset(time_since_reset); - if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) { - fix.setStatus(cereal::LiveLocationKalman::Status::VALID); - } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) { - fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); - } else { - fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); - } } VectorXd Localizer::get_position_geodetic() { @@ -651,26 +577,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->update_reset_tracker(); } -void Localizer::build_location_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid) { - cereal::Event::Builder evt = msg_builder.initEvent(); - evt.setValid(msgValid); - cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); - this->build_live_location(liveLoc); - liveLoc.setSensorsOK(sensorsOK); - liveLoc.setGpsOK(gpsOK); - liveLoc.setInputsOK(inputsOK); -} - void Localizer::build_pose_message( - MessageBuilder& msg_builder, MessageBuilder& location_msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) { + MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) { cereal::Event::Builder evt = msg_builder.initEvent(); evt.setValid(msgValid); cereal::LivePose::Builder livePose = evt.initLivePose(); - - cereal::LiveLocationKalman::Reader location_msg = location_msg_builder.getRoot().getLiveLocationKalman().asReader(); - this->build_live_pose(livePose, location_msg); - + this->build_live_pose(livePose); livePose.setSensorsOK(sensorsOK); livePose.setInputsOK(inputsOK); } @@ -744,7 +656,7 @@ int Localizer::locationd_thread() { "carState", "accelerometer", "gyroscope"}; SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); - PubMaster pm({"liveLocationKalman", "livePose"}); + PubMaster pm({"livePose"}); uint64_t cnt = 0; bool filterInitialized = false; @@ -778,12 +690,8 @@ int Localizer::locationd_thread() { this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); } - MessageBuilder location_msg_builder, pose_msg_builder; - this->build_location_message(location_msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); - this->build_pose_message(pose_msg_builder, location_msg_builder, inputsOK, sensorsOK, filterInitialized); - - kj::ArrayPtr location_bytes = location_msg_builder.toBytes(); - pm.send("liveLocationKalman", location_bytes.begin(), location_bytes.size()); + MessageBuilder pose_msg_builder; + this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized); kj::ArrayPtr pose_bytes = pose_msg_builder.toBytes(); pm.send("livePose", pose_bytes.begin(), pose_bytes.size()); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 615f31c9d0..f59b7d4bda 100644 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -45,12 +45,9 @@ public: bool are_inputs_ok(); void observation_timings_invalid_reset(); - void build_location_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); void build_pose_message( - MessageBuilder& msg_builder, MessageBuilder& location_msg_builder, bool inputsOK, bool sensorsOK, bool msgValid); - void build_live_location(cereal::LiveLocationKalman::Builder& fix); - void build_live_pose(cereal::LivePose::Builder& livePose, cereal::LiveLocationKalman::Reader& liveLocation); + MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid); + void build_live_pose(cereal::LivePose::Builder& fix); Eigen::VectorXd get_position_geodetic(); Eigen::VectorXd get_state(); @@ -83,7 +80,6 @@ private: int64_t unix_timestamp_millis = 0; double reset_tracker = 0.0; - bool device_fell = false; bool gps_mode = false; double first_valid_log_time = NAN; double ttff = NAN; diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index ca52bffeea..bacb2a10a7 100644 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -7,12 +7,12 @@ from openpilot.tools.lib.logreader import LogReader from openpilot.selfdrive.test.process_replay.migration import migrate_all from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name -TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4" +# TODO find a new segment to test +TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5" GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation'] SELECT_COMPARE_FIELDS = { - 'yaw_rate': ['angularVelocityCalibrated', 'value', 2], - 'roll': ['orientationNED', 'value', 0], - 'gps_flag': ['gpsOK'], + 'yaw_rate': ['angularVelocityDevice', 'z'], + 'roll': ['orientationNED', 'x'], 'inputs_flag': ['inputsOK'], 'sensors_flag': ['sensorsOK'], } @@ -37,9 +37,9 @@ def get_select_fields_data(logs): for key in keys: val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key] return val - llk = [x.liveLocationKalman for x in logs if x.which() == 'liveLocationKalman'] + lp = [x.livePose for x in logs if x.which() == 'livePose'] data = defaultdict(list) - for msg in llk: + for msg in lp: for key, fields in SELECT_COMPARE_FIELDS.items(): data[key].append(get_nested_keys(msg, fields)) for key in data: @@ -116,8 +116,8 @@ class TestLocationdScenarios: - roll: unchanged """ orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) def test_gps_off(self): """ @@ -128,9 +128,8 @@ class TestLocationdScenarios: - gpsOK: False """ orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) - assert np.all(replayed_data['gps_flag'] == 0.0) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) def test_gps_off_midway(self): """ @@ -138,12 +137,10 @@ class TestLocationdScenarios: Expected Result: - yaw_rate: unchanged - roll: - - gpsOK: True for the first half, False for the second half """ orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) - assert np.diff(replayed_data['gps_flag'])[512] == -1.0 + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) def test_gps_on_midway(self): """ @@ -151,12 +148,10 @@ class TestLocationdScenarios: Expected Result: - yaw_rate: unchanged - roll: - - gpsOK: False for the first half, True for the second half """ orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5)) - assert np.diff(replayed_data['gps_flag'])[505] == 1.0 def test_gps_tunnel(self): """ @@ -164,13 +159,10 @@ class TestLocationdScenarios: Expected Result: - yaw_rate: unchanged - roll: - - gpsOK: False for the middle section, True for the rest """ orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) - assert np.diff(replayed_data['gps_flag'])[213] == -1.0 - assert np.diff(replayed_data['gps_flag'])[805] == 1.0 + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) def test_gyro_off(self): """ @@ -194,10 +186,10 @@ class TestLocationdScenarios: - inputsOK: False for some time after the spike, True for the rest """ orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) - assert np.diff(replayed_data['inputs_flag'])[500] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[694] == 1.0 + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) + assert np.diff(replayed_data['inputs_flag'])[499] == -1.0 + assert np.diff(replayed_data['inputs_flag'])[696] == 1.0 def test_accel_off(self): """ @@ -219,5 +211,5 @@ class TestLocationdScenarios: Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. """ orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) - assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) - assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index da0197b256..7997612e25 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -547,7 +547,7 @@ CONFIGS = [ "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", "liveCalibration", "carState", "gpsLocation" ], - subs=["liveLocationKalman", "livePose"], + subs=["livePose"], ignore=["logMonoTime"], config_callback=locationd_config_pubsub_callback, tolerance=NUMPY_TOLERANCE, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index e580430a36..bc635df7dd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fa4965a27dee4449ad8b255f9f7674d69327b6f7 \ No newline at end of file +51a1a4716fdd6d58fc69c586aaa62c5c25a3d93e \ 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 faad05c0f2..702de4f7d1 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -40,23 +40,23 @@ source_segments = [ ] segments = [ - ("BODY", "regen34ECCE11CA1|2024-07-29--22-55-10--0"), - ("HYUNDAI", "regenC713CE6FA82|2024-07-29--22-56-31--0"), - ("HYUNDAI2", "regenD81F3A374A7|2024-07-29--22-58-45--0"), - ("TOYOTA", "regenE6D76723DC2|2024-07-29--23-00-08--0"), - ("TOYOTA2", "regen198859A572C|2024-07-29--23-01-31--0"), - ("TOYOTA3", "regenDF1EB621A66|2024-07-29--23-03-49--0"), - ("HONDA", "regen0FE7C4758B5|2024-07-29--23-05-14--0"), - ("HONDA2", "regen510A1F60E60|2024-07-29--23-06-39--0"), - ("CHRYSLER", "regenDACF082E83B|2024-07-29--23-08-01--0"), - ("RAM", "regen8BFB7E62F52|2024-07-29--23-10-14--0"), - ("SUBARU", "regen4EE2D45369E|2024-07-29--23-12-31--0"), - ("GM", "regenB38D92E6A4D|2024-07-29--23-13-54--0"), - ("GM2", "regenC5488470F1A|2024-07-29--23-16-09--0"), - ("NISSAN", "regenE5400EB4689|2024-07-29--23-17-30--0"), - ("VOLKSWAGEN", "regenD0B5635A8B9|2024-07-29--23-18-54--0"), - ("MAZDA", "regen57F8511F082|2024-07-29--23-21-09--0"), - ("FORD", "regen5708620AA2E|2024-07-29--23-23-20--0"), + ("BODY", "regenA67A128BCD8|2024-08-30--02-36-22--0"), + ("HYUNDAI", "regen9CBD921E93E|2024-08-30--02-38-51--0"), + ("HYUNDAI2", "regen12E0C4EA1A7|2024-08-30--02-42-40--0"), + ("TOYOTA", "regen1CA7A48E6F7|2024-08-30--02-45-08--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"), + ("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"), + ("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", "regen6ECC59A6307|2024-08-30--03-25-42--0"), ] # dashcamOnly makes don't need to be tested until a full port is done @@ -86,7 +86,7 @@ def run_test_process(data): def get_log_data(segment): r, n = segment.rsplit("--", 1) - with FileReader(get_url(r, n, "rlog.bz2")) as f: + with FileReader(get_url(r, n, "rlog.zst")) as f: return (segment, f.read()) @@ -199,7 +199,7 @@ if __name__ == "__main__": cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst") if args.update_refs: # reference logs will not exist if routes were just regenerated - ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.bz2") + ref_log_path = get_url(*segment.rsplit("--", 1,), "rlog.zst") else: ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.zst") ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 61b0e28483..63ef2a7a4c 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -91,7 +91,7 @@ TIMINGS = { "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], "driverStateV2": [2.5, 0.40], - "liveLocationKalman": [2.5, 0.35], + "livePose": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], }