sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Deprecate liveLocationKalman (#33405)

Browse Source 
* Remove usages of llk

* Deprecate it

* Add scenarios back

* Fix orientationNED valid

* Regenerate

* Increase tolerances and update segment

* Remove calibration
old-commit-hash: 282eae3c30
pull/33384/head
Kacper Rączy 8 months ago committed by GitHub
parent 786aff27da
commit 012162e816
9 changed files with 62 additions and 167 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      cereal/log.capnp
  2. 1
      cereal/services.py
  3. 122
      selfdrive/locationd/locationd.cc
  4. 8
      selfdrive/locationd/locationd.h
  5. 50
      selfdrive/locationd/test/test_locationd_scenarios.py
  6. 2
      selfdrive/test/process_replay/process_replay.py
  7. 2
      selfdrive/test/process_replay/ref_commit
  8. 38
      selfdrive/test/process_replay/test_processes.py
  9. 2
      selfdrive/test/test_onroad.py

4
cereal/log.capnp
Unescape View File

@ -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;
}
}

1
cereal/services.py
Unescape View File

@ -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),

122
selfdrive/locationd/locationd.cc
Unescape View File

@ -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_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(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_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(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_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(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_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(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_LEN>(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_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(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<cereal::Event>().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<capnp::byte> 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<capnp::byte> pose_bytes = pose_msg_builder.toBytes();
pm.send("livePose", pose_bytes.begin(), pose_bytes.size());

8
selfdrive/locationd/locationd.h
Unescape View File

@ -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;

50
selfdrive/locationd/test/test_locationd_scenarios.py
Unescape View File

@ -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))

2
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -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,

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
fa4965a27dee4449ad8b255f9f7674d69327b6f7
51a1a4716fdd6d58fc69c586aaa62c5c25a3d93e

38
selfdrive/test/process_replay/test_processes.py
Unescape View File

@ -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)

2
selfdrive/test/test_onroad.py
Unescape View File

@ -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],
}

Write Preview
Loading…
Cancel
Save