Skip to content

Commit

Permalink
Merge pull request #1122 from tier4/hotfix/v0.20.1/gyro_bias_estimato…
Browse files Browse the repository at this point in the history 
…r_diag

feat(imu_corrector): cherry pick diagnostics modification in gyro_bias_estimatior from awf
  • Loading branch information
@shmpwk
shmpwk authored Jan 30, 2024
2 parents 0645ad4 + 7ef351c commit 7021f0a
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 33 deletions.
6 changes: 3 additions & 3 deletions sensing/imu_corrector/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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] |
1 change: 1 addition & 0 deletions sensing/imu_corrector/config/gyro_bias_estimator.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
99 changes: 69 additions & 30 deletions sensing/imu_corrector/src/gyro_bias_estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>();

Expand All @@ -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)
Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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(
Expand All @@ -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
16 changes: 16 additions & 0 deletions sensing/imu_corrector/src/gyro_bias_estimator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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_;
Expand All @@ -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

Expand Down

0 comments on commit 7021f0a

Please sign in to comment.