Handle posenet and sensor alerts in locationd (#1541)

* handle posenet and senor alerts in locationd

* defaults now set in capnp file

* Cleanup c++ version of params learner

* update ref commit

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)

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

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)
 
 

selfdrive/locationd/locationd_yawrate.cc

@@ -32,7 +32,6 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
 void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::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;
 }
 
 

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

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::CONTROLS_STATE){
-      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){
         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();

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"],

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-76e577b86d113139167275b4a7379f3591abfa02
+59bd32350139796784708f51c233d37c18f33e6e