Skip to content

Commit 94ccc95

Browse files
committed
Set diagnostics values to have the same precision
Added gyro_bias_threshold_ to the diagnostics Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
1 parent a02f897 commit 94ccc95

File tree

1 file changed

+14
-6
lines changed

1 file changed

+14
-6
lines changed

sensing/imu_corrector/src/gyro_bias_estimator.cpp

+14-6
Original file line numberDiff line numberDiff line change
@@ -225,14 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3(
225225

226226
void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
227227
{
228+
auto f = [](const double & value) {
229+
std::stringstream ss;
230+
ss << std::fixed << std::setprecision(8) << value;
231+
return ss.str();
232+
};
233+
228234
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));
232242

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_));
236244
}
237245

238246
} // namespace imu_corrector

0 commit comments

Comments
 (0)