From 448b3737be767cbc0a34623a2af8f4addb70d565 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 19:40:04 +0900 Subject: [PATCH 1/2] feat(imu_corrector): add gyro_bias estimation in diag Signed-off-by: kminoda --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 581eccb0097d4..e6ab2753bced1 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,6 +83,10 @@ 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", gyro_bias_.value().x); + stat.add("gyro_bias_y", gyro_bias_.value().y); + stat.add("gyro_bias_z", gyro_bias_.value().z); + // Validation const bool is_bias_small_enough = std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && From 523735617250d97f40c5e9d591c47cd742e05edb Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 20 Sep 2023 20:11:58 +0900 Subject: [PATCH 2/2] update Signed-off-by: kminoda --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index e6ab2753bced1..5f41a9089b6ee 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,9 +83,13 @@ 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", gyro_bias_.value().x); - stat.add("gyro_bias_y", gyro_bias_.value().y); - stat.add("gyro_bias_z", gyro_bias_.value().z); + 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 =