feat(imu_corrector): add gyro bias log (#918)

add gyro_bias log Signed-off-by: asa-naki <[email protected]>
tier4 · Aug 14, 2024 · 7f7616a · 7f7616a
1 parent 02a31d3
commit 7f7616a
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
@@ -105,6 +105,11 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW
         "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in "
         "imu_corrector. You may also use the output of gyro_bias_estimator.");
       stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN");
+      RCLCPP_WARN(
+        get_logger(), "gyro_bias_x: %lf, gyro_bias_y: %lf, gyro_bias_z: %lf",
+        (gyro_bias_.value().x - angular_velocity_offset_x_),
+        (gyro_bias_.value().y - angular_velocity_offset_y_),
+        (gyro_bias_.value().z - angular_velocity_offset_z_));
     }
   }
 }