From 996bced674e6943976f1d6dba37075e113bf9288 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Wed, 17 Jul 2024 16:17:23 -0700 Subject: [PATCH] livePose log (#32868) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 --- cereal/log.capnp | 33 +++++++ cereal/services.py | 3 +- selfdrive/locationd/locationd.cc | 90 +++++++++++++++---- selfdrive/locationd/locationd.h | 7 +- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/test_onroad.py | 2 +- 7 files changed, 117 insertions(+), 22 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 3a53db9cc4..d94d9632fe 100644 --- a/cereal/log.capnp +++ b/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; diff --git a/cereal/services.py b/cereal/services.py index f1e1935c54..f2d01b9220 100755 --- a/cereal/services.py +++ b/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), diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 2ac392a778..5607871f5f 100644 --- a/selfdrive/locationd/locationd.cc +++ b/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::Reader& floatlist) { - VectorXd res(floatlist.size()); +template +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 +static VectorXd float64list2vector(const ListType& floatlist) { + return floatlist2vector(floatlist); +} + +template +static VectorXf float32list2vector(const ListType& floatlist) { + return floatlist2vector(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 trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + VectorXd rot_device = this->device_from_calib * float64list2vector(log.getRot()); + 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 trans_calib_std = floatlist2vector(log.getTransStd()); + VectorXd rot_calib_std = float64list2vector(log.getRotStd()); + 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 Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, - bool sensorsOK, bool gpsOK, bool msgValid) { +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(); @@ -620,7 +660,19 @@ kj::ArrayPtr 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().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; - kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); - pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + 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()); + + kj::ArrayPtr 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(); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 47c8bf5627..615f31c9d0 100644 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -45,9 +45,12 @@ public: bool are_inputs_ok(); void observation_timings_invalid_reset(); - kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, - bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); + 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); Eigen::VectorXd get_position_geodetic(); Eigen::VectorXd get_state(); diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index c7f57fd9bc..eae6abfaad 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 13b427b894..02e4b997aa 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0adff03d45c99dcfb330c48b2aa9d2093ce674a2 +3942de8e75caa7a99828e65c3396abd94d6b3da7 diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 78486a176d..bbb99d13b9 100644 --- a/selfdrive/test/test_onroad.py +++ b/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'):