livePose log (#32868)

* add livepose

* Formatting

* Add to sevices

* Update locationd to publish livePose

* Remove fields and increase decimation

* Fix field indices

* Remove the line

* Add livePose to pubmaster

* Fix llk decimation

* Update ref commit

* XYZ measurements instead of lists

* Update locationd

* Update ref commit

* Lower the qlog size in test_onroad

* Update lower and upper boundary

---------

Co-authored-by: Kacper Rączy <gfw.kra@gmail.com>

cereal/log.capnp

@@ -1252,6 +1252,38 @@ struct LiveLocationKalman {
   }
 }
 
+
+struct LivePose {
+  # More info on reference frames:
+  # https://github.com/commaai/openpilot/tree/master/common/transformations
+  orientationNED @0 :XYZMeasurement;
+  velocityDevice @1 :XYZMeasurement;
+  accelerationDevice @2 :XYZMeasurement;
+  angularVelocityDevice @3 :XYZMeasurement;
+
+  inputsOK @4 :Bool = false;
+  posenetOK @5 :Bool = false;
+  sensorsOK @6 :Bool = false;
+
+  filterState @7 :FilterState;
+
+  struct XYZMeasurement {
+    x @0 :Float32;
+    y @1 :Float32;
+    z @2 :Float32;
+    xStd @3 :Float32;
+    yStd @4 :Float32;
+    zStd @5 :Float32;
+    valid @6 :Bool;
+  }
+
+  struct FilterState {
+    value @0 : List(Float64);
+    std @1 : List(Float64);
+    valid @2 : Bool;
+  }
+}
+
 struct ProcLog {
   cpuTimes @0 :List(CPUTimes);
   mem @1 :Mem;
@@ -2293,6 +2325,7 @@ struct Event {
     carParams @69: Car.CarParams;
     driverMonitoringState @71: DriverMonitoringState;
     liveLocationKalman @72 :LiveLocationKalman;
+    livePose @129 :LivePose;
     modelV2 @75 :ModelDataV2;
     drivingModelData @128 :DrivingModelData;
     driverStateV2 @92 :DriverStateV2;

cereal/services.py

@@ -47,7 +47,8 @@ _services: dict[str, tuple] = {
   "gnssMeasurements": (True, 10., 10),
   "clocks": (True, 0.1, 1),
   "ubloxRaw": (True, 20.),
-  "liveLocationKalman": (True, 20., 5),
+  "livePose": (True, 20., 4),
+  "liveLocationKalman": (True, 20.),
   "liveParameters": (True, 20., 5),
   "cameraOdometry": (True, 20., 5),
   "thumbnail": (True, 0.2, 1),

selfdrive/locationd/locationd.cc

@@ -41,14 +41,25 @@ const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
 
 const bool   DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
 
-static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
+template <typename ListType, typename Vector>
-  VectorXd res(floatlist.size());
+static Vector floatlist2vector(const ListType& floatlist) {
+  Vector res(floatlist.size());
   for (int i = 0; i < floatlist.size(); i++) {
     res[i] = floatlist[i];
   }
   return res;
 }
 
+template <typename ListType>
+static VectorXd float64list2vector(const ListType& floatlist) {
+  return floatlist2vector<ListType, VectorXd>(floatlist);
+}
+
+template <typename ListType>
+static VectorXf float32list2vector(const ListType& floatlist) {
+  return floatlist2vector<ListType, VectorXf>(floatlist);
+}
+
 static Vector4d quat2vector(const Quaterniond& quat) {
   return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
 }
@@ -63,6 +74,11 @@ static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder me
   meas.setValid(valid);
 }
 
+static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXf& val, const VectorXf& 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);
+}
 
 static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
   // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
@@ -91,6 +107,30 @@ 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) {
   VectorXd predicted_state = this->kf->get_x();
   MatrixXdr predicted_cov = this->kf->get_P();
@@ -279,7 +319,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
     // TODO: reduce false positives and re-enable this check
     // check if device fell, estimate 10 for g
     // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
-    // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
+    // this->device_fell |= (float64list2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
 
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
     if (meas.norm() < ACCEL_SANITY_CHECK) {
@@ -311,7 +351,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
   bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
-  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
+  bool gps_vel_insane = (float64list2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
   if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
     //this->gps_valid = false;
@@ -450,8 +490,8 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re
 }
 
 void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
-  VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
+  VectorXd rot_device = this->device_from_calib * float64list2vector(log.getRot());
-  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
+  VectorXd trans_device = this->device_from_calib * float64list2vector(log.getTrans());
 
   if (!this->is_timestamp_valid(current_time)) {
     this->observation_timings_invalid = true;
@@ -463,8 +503,8 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
     return;
   }
 
-  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
+  VectorXd rot_calib_std = float64list2vector(log.getRotStd());
-  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
+  VectorXd trans_calib_std = float64list2vector(log.getTransStd());
 
   if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
     this->observation_values_invalid["cameraOdometry"] += 1.0;
@@ -499,7 +539,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
   }
 
   if (log.getRpyCalib().size() > 0) {
-    auto live_calib = floatlist2vector(log.getRpyCalib());
+    auto live_calib = float64list2vector(log.getRpyCalib());
     if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
       this->observation_values_invalid["liveCalibration"] += 1.0;
       return;
@@ -611,8 +651,8 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   this->update_reset_tracker();
 }
 
-kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
+void Localizer::build_location_message(
-                                                       bool sensorsOK, bool gpsOK, bool msgValid) {
+  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();
@@ -620,7 +660,19 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
   liveLoc.setSensorsOK(sensorsOK);
   liveLoc.setGpsOK(gpsOK);
   liveLoc.setInputsOK(inputsOK);
-  return msg_builder.toBytes();
+}
+
+void Localizer::build_pose_message(
+  MessageBuilder& msg_builder, MessageBuilder& location_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);
+
+  livePose.setSensorsOK(sensorsOK);
+  livePose.setInputsOK(inputsOK);
 }
 
 bool Localizer::is_gps_ok() {
@@ -692,7 +744,7 @@ int Localizer::locationd_thread() {
                                                           "carState", "accelerometer", "gyroscope"};
 
   SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
-  PubMaster pm({"liveLocationKalman"});
+  PubMaster pm({"liveLocationKalman", "livePose"});
 
   uint64_t cnt = 0;
   bool filterInitialized = false;
@@ -726,9 +778,15 @@ int Localizer::locationd_thread() {
         this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
       }
 
-      MessageBuilder msg_builder;
+      MessageBuilder location_msg_builder, pose_msg_builder;
-      kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
+      this->build_location_message(location_msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
-      pm.send("liveLocationKalman", bytes.begin(), bytes.size());
+      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());
+
+      kj::ArrayPtr<capnp::byte> pose_bytes = pose_msg_builder.toBytes();
+      pm.send("livePose", pose_bytes.begin(), pose_bytes.size());
 
       if (cnt % 1200 == 0 && gpsOK) {  // once a minute
         VectorXd posGeo = this->get_position_geodetic();

selfdrive/locationd/locationd.h

@@ -45,9 +45,12 @@ public:
   bool are_inputs_ok();
   void observation_timings_invalid_reset();
 
-  kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
+  void build_location_message(
-    bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
+    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);
 
   Eigen::VectorXd get_position_geodetic();
   Eigen::VectorXd get_state();

selfdrive/test/process_replay/process_replay.py

@@ -532,7 +532,7 @@ CONFIGS = [
       "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
       "liveCalibration", "carState", "gpsLocation"
     ],
-    subs=["liveLocationKalman"],
+    subs=["liveLocationKalman", "livePose"],
     ignore=["logMonoTime"],
     config_callback=locationd_config_pubsub_callback,
     tolerance=NUMPY_TOLERANCE,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-0adff03d45c99dcfb330c48b2aa9d2093ce674a2
+3942de8e75caa7a99828e65c3396abd94d6b3da7

selfdrive/test/test_onroad.py

@@ -206,7 +206,7 @@ class TestOnroad:
       if f.name == "qcamera.ts":
         assert 2.15 < sz < 2.35
       elif f.name == "qlog":
-        assert 0.6 < sz < 1.0
+        assert 0.4 < sz < 0.6
       elif f.name == "rlog":
         assert 5 < sz < 50
       elif f.name.endswith('.hevc'):