From e1bdf486a3d39ae01f4ce339b2227cdbaad80036 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Tue, 26 Sep 2023 17:04:11 +0900 Subject: [PATCH] add gyro_bias estimation in diag ( #5054) Signed-off-by: asa-naki --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 42795805f803e..47c1014062714 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,6 +83,12 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); // Validation const bool is_bias_small_enough = std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ &&