diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 7af82c1129aff..2df12c94b7d3b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -70,11 +70,11 @@ In the future, with careful implementation for pose errors, the IMU bias estimat ### Parameters +Note that this node also uses `angular_velocity_offset_x`, `angular_velocity_offset_y`, `angular_velocity_offset_z` parameters from `imu_corrector.param.yaml`. + | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | | `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | | `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `diagnostics_updater_interval_sec` | double | period of the diagnostics updater [sec] | | `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index e10329c920137..eac423f94fa78 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -2,4 +2,5 @@ ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 50e4a702ec949..e99667ed1c4a6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -30,6 +30,7 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + diagnostics_updater_interval_sec_(declare_parameter("diagnostics_updater_interval_sec")), straight_motion_ang_vel_upper_limit_( declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), @@ -37,6 +38,8 @@ GyroBiasEstimator::GyroBiasEstimator() { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + // diagnostic_updater is designed to be updated at the same rate as the timer + updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -57,6 +60,18 @@ GyroBiasEstimator::GyroBiasEstimator() this->get_node_timers_interface()->add_timer(timer_, nullptr); transform_listener_ = std::make_shared(this); + + // initialize diagnostics_info_ + { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Not initialized"; + diagnostics_info_.gyro_bias_x_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_y_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_z_for_imu_corrector = std::nan(""); + diagnostics_info_.estimated_gyro_bias_x = std::nan(""); + diagnostics_info_.estimated_gyro_bias_y = std::nan(""); + diagnostics_info_.estimated_gyro_bias_z = std::nan(""); + } } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) @@ -99,6 +114,7 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { if (pose_buf_.empty()) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is empty)"; return; } @@ -112,6 +128,7 @@ void GyroBiasEstimator::timer_callback() const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); if (t1_rclcpp_time <= t0_rclcpp_time) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is not in chronological order)"; return; } @@ -127,6 +144,7 @@ void GyroBiasEstimator::timer_callback() // Check gyro data size // Data size must be greater than or equal to 2 since the time difference will be taken later if (gyro_filtered.size() <= 1) { + diagnostics_info_.summary_message = "Skipped update (gyro_filtered size is less than 2)"; return; } @@ -140,6 +158,8 @@ void GyroBiasEstimator::timer_callback() const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); if (!is_straight) { + diagnostics_info_.summary_message = + "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -151,12 +171,45 @@ void GyroBiasEstimator::timer_callback() if (!tf_base2imu_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } + gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); + validate_gyro_bias(); updater_.force_update(); + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater +} + +void GyroBiasEstimator::validate_gyro_bias() +{ + // Calculate diagnostics key-values + diagnostics_info_.gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; + diagnostics_info_.gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; + diagnostics_info_.gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; + diagnostics_info_.estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; + diagnostics_info_.estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; + diagnostics_info_.estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; + + // Validation + const bool is_bias_small_enough = + std::abs(diagnostics_info_.estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_z) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Successfully updated"; + } else { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diagnostics_info_.summary_message = + "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."; + } } geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( @@ -172,36 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (gyro_bias_ == std::nullopt) { - 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_ && - std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; - - // Update diagnostics - if (is_bias_small_enough) { - stat.add("gyro_bias", "OK"); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); - } else { - stat.add( - "gyro_bias", - "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"); - } - } + auto f = [](const double & value) { + std::stringstream ss; + ss << std::fixed << std::setprecision(8) << value; + return ss.str(); + }; + + stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message); + stat.add("gyro_bias_x_for_imu_corrector", f(diagnostics_info_.gyro_bias_x_for_imu_corrector)); + stat.add("gyro_bias_y_for_imu_corrector", f(diagnostics_info_.gyro_bias_y_for_imu_corrector)); + stat.add("gyro_bias_z_for_imu_corrector", f(diagnostics_info_.gyro_bias_z_for_imu_corrector)); + + stat.add("estimated_gyro_bias_x", f(diagnostics_info_.estimated_gyro_bias_x)); + stat.add("estimated_gyro_bias_y", f(diagnostics_info_.estimated_gyro_bias_y)); + stat.add("estimated_gyro_bias_z", f(diagnostics_info_.estimated_gyro_bias_z)); + + stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 821cba0b213ff..3592ff1f7d3b4 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -49,6 +49,7 @@ class GyroBiasEstimator : public rclcpp::Node void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); void timer_callback(); + void validate_gyro_bias(); static geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, @@ -68,6 +69,7 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; const double timer_callback_interval_sec_; + const double diagnostics_updater_interval_sec_; const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; @@ -80,6 +82,20 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; + + struct DiagnosticsInfo + { + unsigned char level; + std::string summary_message; + double gyro_bias_x_for_imu_corrector; + double gyro_bias_y_for_imu_corrector; + double gyro_bias_z_for_imu_corrector; + double estimated_gyro_bias_x; + double estimated_gyro_bias_y; + double estimated_gyro_bias_z; + }; + + DiagnosticsInfo diagnostics_info_; }; } // namespace imu_corrector