@@ -225,14 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3(
225
225
226
226
void GyroBiasEstimator::update_diagnostics (diagnostic_updater::DiagnosticStatusWrapper & stat)
227
227
{
228
+ auto f = [](const double & value) {
229
+ std::stringstream ss;
230
+ ss << std::fixed << std::setprecision (8 ) << value;
231
+ return ss.str ();
232
+ };
233
+
228
234
stat.summary (diagnostics_info_.level , diagnostics_info_.summary_message );
229
- stat.add (" gyro_bias_x_for_imu_corrector" , diagnostics_info_.gyro_bias_x_for_imu_corrector );
230
- stat.add (" gyro_bias_y_for_imu_corrector" , diagnostics_info_.gyro_bias_y_for_imu_corrector );
231
- stat.add (" gyro_bias_z_for_imu_corrector" , diagnostics_info_.gyro_bias_z_for_imu_corrector );
235
+ stat.add (" gyro_bias_x_for_imu_corrector" , f (diagnostics_info_.gyro_bias_x_for_imu_corrector ));
236
+ stat.add (" gyro_bias_y_for_imu_corrector" , f (diagnostics_info_.gyro_bias_y_for_imu_corrector ));
237
+ stat.add (" gyro_bias_z_for_imu_corrector" , f (diagnostics_info_.gyro_bias_z_for_imu_corrector ));
238
+
239
+ stat.add (" estimated_gyro_bias_x" , f (diagnostics_info_.estimated_gyro_bias_x ));
240
+ stat.add (" estimated_gyro_bias_y" , f (diagnostics_info_.estimated_gyro_bias_y ));
241
+ stat.add (" estimated_gyro_bias_z" , f (diagnostics_info_.estimated_gyro_bias_z ));
232
242
233
- stat.add (" estimated_gyro_bias_x" , diagnostics_info_.estimated_gyro_bias_x );
234
- stat.add (" estimated_gyro_bias_y" , diagnostics_info_.estimated_gyro_bias_y );
235
- stat.add (" estimated_gyro_bias_z" , diagnostics_info_.estimated_gyro_bias_z );
243
+ stat.add (" gyro_bias_threshold" , f (gyro_bias_threshold_));
236
244
}
237
245
238
246
} // namespace imu_corrector
0 commit comments