diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 93353d0690..36de2fc6d7 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -50,7 +50,7 @@ class Controls: self.sm = sm if self.sm is None: self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \ - 'dMonitoringState', 'plan', 'pathPlan']) + 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) self.can_sock = can_sock if can_sock is None: @@ -123,8 +123,6 @@ class Controls: self.current_alert_types = [] self.sm['liveCalibration'].calStatus = Calibration.INVALID - self.sm['pathPlan'].sensorValid = True - self.sm['pathPlan'].posenetValid = True self.sm['thermal'].freeSpace = 1. self.sm['dMonitoringState'].events = [] self.sm['dMonitoringState'].awarenessStatus = 1. @@ -201,11 +199,11 @@ class Controls: self.events.add(EventName.commIssue) if not self.sm['pathPlan'].mpcSolutionValid: self.events.add(EventName.plannerError) - if not self.sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None: + if not self.sm['liveLocationKalman'].inputsOK and os.getenv("NOSENSOR") is None: self.events.add(EventName.sensorDataInvalid) if not self.sm['pathPlan'].paramsValid: self.events.add(EventName.vehicleModelInvalid) - if not self.sm['pathPlan'].posenetValid: + if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['plan'].radarValid: self.events.add(EventName.radarFault) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 413d71d3a2..a8de48013a 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -207,8 +207,6 @@ class PathPlanner(): plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) - plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) - plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index b8359a49b0..e6a3e38c13 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -57,6 +57,10 @@ class Localizer(): self.calibrated = 0 self.H = get_H() + self.posenet_invalid_count = 0 + self.posenet_speed = 0 + self.car_speed = 0 + @staticmethod def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov): predicted_std = np.sqrt(np.diagonal(predicted_cov)) @@ -141,6 +145,12 @@ class Localizer(): def liveLocationMsg(self, time): fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P) + if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0): + self.posenet_invalid_count += 1 + else: + self.posenet_invalid_count = 0 + fix.posenetOK = self.posenet_invalid_count < 4 + #fix.gpsWeek = self.time.week #fix.gpsTimeOfWeek = self.time.tow fix.unixTimestampMillis = self.unix_timestamp_millis @@ -198,12 +208,12 @@ class Localizer(): self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R) self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R) - def handle_car_state(self, current_time, log): self.speed_counter += 1 if self.speed_counter % SENSOR_DECIMATION == 0: self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo]) + self.car_speed = abs(log.vEgo) if log.vEgo == 0: self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) @@ -218,6 +228,7 @@ class Localizer(): np.concatenate([rot_device, 10*rot_device_std])) trans_device = self.device_from_calib.dot(log.trans) trans_device_std = self.device_from_calib.dot(log.transStd) + self.posenet_speed = np.linalg.norm(trans_device) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([trans_device, 10*trans_device_std])) @@ -263,7 +274,7 @@ class Localizer(): def locationd_thread(sm, pm, disabled_logs=[]): if sm is None: - socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration'] + socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState'] sm = messaging.SubMaster(socks) if pm is None: pm = messaging.PubMaster(['liveLocationKalman']) @@ -293,6 +304,8 @@ def locationd_thread(sm, pm, disabled_logs=[]): msg.logMonoTime = t msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) + msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() + pm.send('liveLocationKalman', msg) diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index 4b93b68014..e682ec3909 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -32,7 +32,6 @@ void Localizer::update_state(const Eigen::Matrix &C, const double void Localizer::handle_sensor_events(capnp::List::Reader sensor_events, double current_time) { for (cereal::SensorEventData::Reader sensor_event : sensor_events){ if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) { - sensor_data_time = current_time; double meas = -sensor_event.getGyroUncalibrated().getV()[0]; update_state(C_gyro, R_gyro, current_time, meas); } @@ -43,16 +42,11 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo double R = pow(5 * camera_odometry.getRotStd()[2], 2); double meas = camera_odometry.getRot()[2]; update_state(C_posenet, R, current_time, meas); - - auto trans = camera_odometry.getTrans(); - posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]); - camera_odometry_time = current_time; } void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS; car_speed = controls_state.getVEgo(); - controls_state_time = current_time; } diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h index c59734aa68..58999e5f37 100644 --- a/selfdrive/locationd/locationd_yawrate.h +++ b/selfdrive/locationd/locationd_yawrate.h @@ -25,11 +25,7 @@ public: Eigen::Matrix2d P; double steering_angle = 0; double car_speed = 0; - double posenet_speed = 0; double prev_update_time = -1; - double controls_state_time = -1; - double sensor_data_time = -1; - double camera_odometry_time = -1; Localizer(); void handle_log(cereal::Event::Reader event); diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 8c60bc12e4..bae43ba367 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -116,23 +116,12 @@ int main(int argc, char *argv[]) { localizer.handle_log(event); auto which = event.which(); - // Throw vision failure if posenet and odometric speed too different - if (which == cereal::Event::CAMERA_ODOMETRY){ - if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) { - posenet_invalid_count++; - } else { - posenet_invalid_count = 0; - } - } else if (which == cereal::Event::CONTROLS_STATE){ + if (which == cereal::Event::CONTROLS_STATE){ save_counter++; double yaw_rate = -localizer.x[0]; bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); - // TODO: Fix in replay - double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time; - double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time; - double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; @@ -143,13 +132,10 @@ int main(int argc, char *argv[]) { live_params.setValid(valid); live_params.setYawRate(localizer.x[0]); live_params.setGyroBias(localizer.x[1]); - live_params.setSensorValid(sensor_data_age < 5.0); live_params.setAngleOffset(angle_offset_degrees); live_params.setAngleOffsetAverage(angle_offset_average_degrees); live_params.setStiffnessFactor(learner.x); live_params.setSteerRatio(learner.sR); - live_params.setPosenetSpeed(localizer.posenet_speed); - live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0)); auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 106d871e5f..2687b24210 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -202,7 +202,7 @@ CONFIGS = [ proc_name="controlsd", pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], - "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], + "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [], "model": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 880f969541..baaafb0f8d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -76e577b86d113139167275b4a7379f3591abfa02 \ No newline at end of file +59bd32350139796784708f51c233d37c18f33e6e \ No newline at end of file