From 2bc4949c9cdd2b4f31ecbcc9e68c10e839355ccf Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Fri, 19 Jan 2024 14:35:15 +0900 Subject: [PATCH 01/13] Let diagnostics be updated even if expections occur in timer_callback Signed-off-by: TaikiYamada4 --- .../imu_corrector/src/gyro_bias_estimator.cpp | 71 +++++++++++++++---- .../imu_corrector/src/gyro_bias_estimator.hpp | 3 + 2 files changed, 59 insertions(+), 15 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 50e4a702ec949..5e4921ec451cb 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -57,6 +57,9 @@ GyroBiasEstimator::GyroBiasEstimator() this->get_node_timers_interface()->add_timer(timer_, nullptr); transform_listener_ = std::make_shared(this); + + is_bias_updated_ = false; + supplement_ = "none"; } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) @@ -98,7 +101,13 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { + is_bias_updated_ = true; + supplement_ = "none"; + if (pose_buf_.empty()) { + is_bias_updated_ = false; + supplement_ = "pose_buf is empty"; + updater_.force_update(); return; } @@ -112,6 +121,9 @@ 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) { + is_bias_updated_ = false; + supplement_ = "pose_buf is not in chronological order"; + updater_.force_update(); return; } @@ -127,6 +139,9 @@ 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) { + is_bias_updated_ = false; + supplement_ = "gyro_filtered size is less than 2"; + updater_.force_update(); return; } @@ -140,6 +155,9 @@ 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) { + is_bias_updated_ = false; + supplement_ = "yaw angular velocity is greater than straight_motion_ang_vel_upper_limit"; + updater_.force_update(); return; } @@ -151,8 +169,13 @@ 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()); + + is_bias_updated_ = false; + supplement_ = "tf betweeen base and imu is not available"; + updater_.force_update(); return; } + gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); @@ -172,36 +195,54 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { + double gyro_bias_x_for_imu_corrector = std::nan(""); + double gyro_bias_y_for_imu_corrector = std::nan(""); + double gyro_bias_z_for_imu_corrector = std::nan(""); + double estimated_gyro_bias_x = std::nan(""); + double estimated_gyro_bias_y = std::nan(""); + double estimated_gyro_bias_z = std::nan(""); + 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"); + is_bias_updated_ = false; } 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_); + // Get gyro bias + gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; + gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; + gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; + + estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; + estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; + 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_; + std::abs(estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(estimated_gyro_bias_z) < gyro_bias_threshold_; // Update diagnostics if (is_bias_small_enough) { - stat.add("gyro_bias", "OK"); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Succeccefully updated"); } else { - stat.add( - "gyro_bias", + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "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"); } } + + stat.add("is_updated", is_bias_updated_); + + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_x_for_imu_corrector); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_y_for_imu_corrector); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_z_for_imu_corrector); + + stat.add("estimated_gyro_bias_x", estimated_gyro_bias_x); + stat.add("estimated_gyro_bias_y", estimated_gyro_bias_y); + stat.add("estimated_gyro_bias_z", estimated_gyro_bias_z); + + stat.add("supplement", supplement_); } } // 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..643c587910ac8 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -80,6 +80,9 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; + + bool is_bias_updated_; + std::string supplement_; }; } // namespace imu_corrector From 4d9627e6a86f6a83608e252bff12a57a8d8974b9 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Fri, 19 Jan 2024 14:58:44 +0900 Subject: [PATCH 02/13] Fixed the summary message to be "Not updated" when the gyro bias is not updated. Fixed typo. Signed-off-by: TaikiYamada4 --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 5e4921ec451cb..b6338b25ee677 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -223,8 +223,15 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW std::abs(estimated_gyro_bias_z) < gyro_bias_threshold_; // Update diagnostics + // The summary depends on which of the three states you are in: + // updated, not updated, or threshold exceeded. if (is_bias_small_enough) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Succeccefully updated"); + if (is_bias_updated_) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Successfully updated"); + } + else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not updated"); + } } else { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " From 818f636f2a14ee05adac509f2dba35cf9e9b0657 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Mon, 22 Jan 2024 17:27:31 +0900 Subject: [PATCH 03/13] Removed force_update() since updater_ originaly updates diagnostics automatically. Set the update peirod of diagnostics to be same to the timer_callback. Changed words of the diagnostics message a bit. Signed-off-by: TaikiYamada4 --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index b6338b25ee677..6ad2df0a540a8 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -37,6 +37,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(timer_callback_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -107,7 +109,6 @@ void GyroBiasEstimator::timer_callback() if (pose_buf_.empty()) { is_bias_updated_ = false; supplement_ = "pose_buf is empty"; - updater_.force_update(); return; } @@ -123,7 +124,6 @@ void GyroBiasEstimator::timer_callback() if (t1_rclcpp_time <= t0_rclcpp_time) { is_bias_updated_ = false; supplement_ = "pose_buf is not in chronological order"; - updater_.force_update(); return; } @@ -141,7 +141,6 @@ void GyroBiasEstimator::timer_callback() if (gyro_filtered.size() <= 1) { is_bias_updated_ = false; supplement_ = "gyro_filtered size is less than 2"; - updater_.force_update(); return; } @@ -157,7 +156,6 @@ void GyroBiasEstimator::timer_callback() if (!is_straight) { is_bias_updated_ = false; supplement_ = "yaw angular velocity is greater than straight_motion_ang_vel_upper_limit"; - updater_.force_update(); return; } @@ -172,14 +170,11 @@ void GyroBiasEstimator::timer_callback() is_bias_updated_ = false; supplement_ = "tf betweeen base and imu is not available"; - updater_.force_update(); return; } gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); - - updater_.force_update(); } geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( @@ -230,7 +225,7 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Successfully updated"); } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not updated"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Paused update"); } } else { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, From a1b84b58531826314e03e07d1574cc7bb71d4be7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 22 Jan 2024 09:38:08 +0000 Subject: [PATCH 04/13] style(pre-commit): autofix --- .../imu_corrector/src/gyro_bias_estimator.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 6ad2df0a540a8..f7b06210f6348 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -38,7 +38,7 @@ 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(timer_callback_interval_sec_); + updater_.setPeriod(timer_callback_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -201,7 +201,6 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); is_bias_updated_ = false; } else { - // Get gyro bias gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; @@ -212,23 +211,22 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; // Validation - const bool is_bias_small_enough = - std::abs(estimated_gyro_bias_x) < gyro_bias_threshold_ && - std::abs(estimated_gyro_bias_y) < gyro_bias_threshold_ && - std::abs(estimated_gyro_bias_z) < gyro_bias_threshold_; + const bool is_bias_small_enough = std::abs(estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(estimated_gyro_bias_z) < gyro_bias_threshold_; // Update diagnostics - // The summary depends on which of the three states you are in: + // The summary depends on which of the three states you are in: // updated, not updated, or threshold exceeded. if (is_bias_small_enough) { if (is_bias_updated_) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Successfully updated"); - } - else { + } else { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Paused update"); } } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "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."); } From 320047d1de1e04a88822d5846b05295a240ecd52 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Tue, 23 Jan 2024 23:24:22 +0900 Subject: [PATCH 05/13] Let the period of diagnostics publication independent to timer. Let update_diagnostics to be simple. Signed-off-by: TaikiYamada4 --- .../config/gyro_bias_estimator.param.yaml | 1 + .../imu_corrector/src/gyro_bias_estimator.cpp | 125 ++++++++---------- .../imu_corrector/src/gyro_bias_estimator.hpp | 17 ++- 3 files changed, 73 insertions(+), 70 deletions(-) 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 f7b06210f6348..93292013fe31c 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), @@ -38,7 +39,7 @@ 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(timer_callback_interval_sec_); + updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -60,8 +61,17 @@ GyroBiasEstimator::GyroBiasEstimator() transform_listener_ = std::make_shared(this); - is_bias_updated_ = false; - supplement_ = "none"; + // 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) @@ -103,12 +113,9 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { - is_bias_updated_ = true; - supplement_ = "none"; if (pose_buf_.empty()) { - is_bias_updated_ = false; - supplement_ = "pose_buf is empty"; + diagnostics_info_.summary_message = "Skipped update (pose_buf is empty)"; return; } @@ -122,8 +129,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) { - is_bias_updated_ = false; - supplement_ = "pose_buf is not in chronological order"; + diagnostics_info_.summary_message = "Skipped update (pose_buf is not in chronological order)"; return; } @@ -139,8 +145,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) { - is_bias_updated_ = false; - supplement_ = "gyro_filtered size is less than 2"; + diagnostics_info_.summary_message = "Skipped update (gyro_filtered size is less than 2)"; return; } @@ -154,8 +159,7 @@ 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) { - is_bias_updated_ = false; - supplement_ = "yaw angular velocity is greater than straight_motion_ang_vel_upper_limit"; + diagnostics_info_.summary_message = "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -168,13 +172,43 @@ void GyroBiasEstimator::timer_callback() RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); - is_bias_updated_ = false; - supplement_ = "tf betweeen base and imu is not available"; + diagnostics_info_.summary_message = "Skipped update (tf betweeen 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(); +} + +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( @@ -190,59 +224,14 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { - double gyro_bias_x_for_imu_corrector = std::nan(""); - double gyro_bias_y_for_imu_corrector = std::nan(""); - double gyro_bias_z_for_imu_corrector = std::nan(""); - double estimated_gyro_bias_x = std::nan(""); - double estimated_gyro_bias_y = std::nan(""); - double estimated_gyro_bias_z = std::nan(""); - - if (gyro_bias_ == std::nullopt) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); - is_bias_updated_ = false; - } else { - // Get gyro bias - gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; - gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; - gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; - - estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; - estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; - estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; - - // Validation - const bool is_bias_small_enough = std::abs(estimated_gyro_bias_x) < gyro_bias_threshold_ && - std::abs(estimated_gyro_bias_y) < gyro_bias_threshold_ && - std::abs(estimated_gyro_bias_z) < gyro_bias_threshold_; - - // Update diagnostics - // The summary depends on which of the three states you are in: - // updated, not updated, or threshold exceeded. - if (is_bias_small_enough) { - if (is_bias_updated_) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Successfully updated"); - } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Paused update"); - } - } else { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "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.add("is_updated", is_bias_updated_); - - stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_x_for_imu_corrector); - stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_y_for_imu_corrector); - stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_z_for_imu_corrector); - - stat.add("estimated_gyro_bias_x", estimated_gyro_bias_x); - stat.add("estimated_gyro_bias_y", estimated_gyro_bias_y); - stat.add("estimated_gyro_bias_z", estimated_gyro_bias_z); - - stat.add("supplement", supplement_); + stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message); + stat.add("gyro_bias_x_for_imu_corrector", diagnostics_info_.gyro_bias_x_for_imu_corrector); + stat.add("gyro_bias_y_for_imu_corrector", diagnostics_info_.gyro_bias_y_for_imu_corrector); + stat.add("gyro_bias_z_for_imu_corrector", diagnostics_info_.gyro_bias_z_for_imu_corrector); + + stat.add("estimated_gyro_bias_x", diagnostics_info_.estimated_gyro_bias_x); + stat.add("estimated_gyro_bias_y", diagnostics_info_.estimated_gyro_bias_y); + stat.add("estimated_gyro_bias_z", diagnostics_info_.estimated_gyro_bias_z); } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 643c587910ac8..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_; @@ -81,8 +83,19 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; - bool is_bias_updated_; - std::string supplement_; + 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 From 639c2c4dfde4934de332db7771312b28171f3ed2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 14:47:47 +0000 Subject: [PATCH 06/13] style(pre-commit): autofix --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 93292013fe31c..1f7e5059464a5 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -113,7 +113,6 @@ 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; @@ -159,7 +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)"; + diagnostics_info_.summary_message = + "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -172,7 +172,8 @@ void GyroBiasEstimator::timer_callback() 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 betweeen base and imu is not available)"; + diagnostics_info_.summary_message = + "Skipped update (tf betweeen base and imu is not available)"; return; } @@ -183,8 +184,8 @@ void GyroBiasEstimator::timer_callback() updater_.force_update(); } -void GyroBiasEstimator::validate_gyro_bias(){ - +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; @@ -194,7 +195,7 @@ void GyroBiasEstimator::validate_gyro_bias(){ diagnostics_info_.estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; // Validation - const bool is_bias_small_enough = + 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_; @@ -205,7 +206,7 @@ void GyroBiasEstimator::validate_gyro_bias(){ diagnostics_info_.summary_message = "Successfully updated"; } else { diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diagnostics_info_.summary_message = + 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."; } From 524c5ad7a232979da0edb8da226068c0d03860b1 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Tue, 23 Jan 2024 23:54:42 +0900 Subject: [PATCH 07/13] Fixed typo Signed-off-by: TaikiYamada4 --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 93292013fe31c..af4843d39261c 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -172,7 +172,7 @@ void GyroBiasEstimator::timer_callback() 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 betweeen base and imu is not available)"; + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } From 270c58ba02df6b6090da3d07b6634d841824a9c5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 14:59:28 +0000 Subject: [PATCH 08/13] style(pre-commit): autofix --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 431512398874b..6da04f68749dc 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -172,8 +172,7 @@ void GyroBiasEstimator::timer_callback() 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)"; + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } From 71eadc23a37d694f5b4ea1a79daaaf10c98a4d8a Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Fri, 26 Jan 2024 09:59:55 +0900 Subject: [PATCH 09/13] Added setPeriod after force_update to reset the timer of updater_. Then, there will be no duplicates of diagnostics. Signed-off-by: TaikiYamada4 --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 6da04f68749dc..a2de0f06b74d7 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -181,6 +181,7 @@ void GyroBiasEstimator::timer_callback() validate_gyro_bias(); updater_.force_update(); + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater } void GyroBiasEstimator::validate_gyro_bias() From a02f8970b1c6f1736eb8f1991e8371f3dcba9f08 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 26 Jan 2024 01:05:01 +0000 Subject: [PATCH 10/13] style(pre-commit): autofix --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index a2de0f06b74d7..19632f73437d2 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -181,7 +181,7 @@ void GyroBiasEstimator::timer_callback() validate_gyro_bias(); updater_.force_update(); - updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater } void GyroBiasEstimator::validate_gyro_bias() From 94ccc95c12a11c0e4cefa2957883fc577569dd00 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Fri, 26 Jan 2024 16:36:16 +0900 Subject: [PATCH 11/13] Set diagnostics values to have the same precision Added gyro_bias_threshold_ to the diagnostics Signed-off-by: TaikiYamada4 --- .../imu_corrector/src/gyro_bias_estimator.cpp | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 19632f73437d2..e99667ed1c4a6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -225,14 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { + 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", diagnostics_info_.gyro_bias_x_for_imu_corrector); - stat.add("gyro_bias_y_for_imu_corrector", diagnostics_info_.gyro_bias_y_for_imu_corrector); - stat.add("gyro_bias_z_for_imu_corrector", diagnostics_info_.gyro_bias_z_for_imu_corrector); + 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("estimated_gyro_bias_x", diagnostics_info_.estimated_gyro_bias_x); - stat.add("estimated_gyro_bias_y", diagnostics_info_.estimated_gyro_bias_y); - stat.add("estimated_gyro_bias_z", diagnostics_info_.estimated_gyro_bias_z); + stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } } // namespace imu_corrector From 714fece5d00e723d6efad6f832f20c78d3ec2902 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Mon, 29 Jan 2024 15:10:03 +0900 Subject: [PATCH 12/13] Updated README.md to match the paramters in gyro_bias_estimator Signed-off-by: TaikiYamada4 --- sensing/imu_corrector/README.md | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 7af82c1129aff..332b00757fb92 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -69,12 +69,11 @@ In the future, with careful implementation for pose errors, the IMU bias estimat | `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | ### 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] | From 0b92a8b0295f0ed72aed48dd5bb1fb80904a50d3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 29 Jan 2024 06:12:41 +0000 Subject: [PATCH 13/13] style(pre-commit): autofix --- sensing/imu_corrector/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 332b00757fb92..2df12c94b7d3b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -69,6 +69,7 @@ In the future, with careful implementation for pose errors, the IMU bias estimat | `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | ### 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 |