feat(imu_corrector): changed publication algorithm and content of /diagnostics in gyro_bias_estimator (#6139)

* Let diagnostics be updated even if expections occur in timer_callback

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Fixed the summary message to be "Not updated" when the gyro bias is not updated.
Fixed typo.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* 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 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Let the period of diagnostics publication independent to timer.
Let update_diagnostics to be simple.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Fixed typo

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Added setPeriod after force_update to reset the timer of updater_.
Then, there will be no duplicates of diagnostics.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Set diagnostics values to have the same precision
Added gyro_bias_threshold_ to the diagnostics

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Updated README.md to match the paramters in gyro_bias_estimator

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: TaikiYamada4

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] |

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]

sensing/imu_corrector/src/gyro_bias_estimator.cpp

@@ -30,13 +30,16 @@ GyroBiasEstimator::GyroBiasEstimator()
   angular_velocity_offset_y_(declare_parameter<double>("angular_velocity_offset_y")),
   angular_velocity_offset_z_(declare_parameter<double>("angular_velocity_offset_z")),
   timer_callback_interval_sec_(declare_parameter<double>("timer_callback_interval_sec")),
+  diagnostics_updater_interval_sec_(declare_parameter<double>("diagnostics_updater_interval_sec")),
   straight_motion_ang_vel_upper_limit_(
     declare_parameter<double>("straight_motion_ang_vel_upper_limit")),
   updater_(this),
   gyro_bias_(std::nullopt)
 {
   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<GyroBiasEstimationModule>();
 
@@ -57,6 +60,18 @@ GyroBiasEstimator::GyroBiasEstimator()
   this->get_node_timers_interface()->add_timer(timer_, nullptr);
 
   transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(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

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<geometry_msgs::msg::Vector3Stamped> gyro_all_;
   std::vector<geometry_msgs::msg::PoseStamped> 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