From 86fa4fbba85f8abca7de01256bff62c33708dfa1 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 28 Aug 2023 11:02:08 +0900 Subject: [PATCH 1/4] feat(imu_corrector): add gyro_bias_validator (backport #4729) * feat(imu_corrector): add gyro_bias_validator Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * revert launch Signed-off-by: kminoda * updat Signed-off-by: kminoda * style(pre-commit): autofix * add debug publisher Signed-off-by: kminoda * minor fix Signed-off-by: kminoda * style(pre-commit): autofix * style(pre-commit): autofix * add gtest Signed-off-by: kminoda * style(pre-commit): autofix * updat e readme Signed-off-by: kminoda * style(pre-commit): autofix * add diagnostics Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * style(pre-commit): autofix * validator -> estimator Signed-off-by: kminoda * fix build Signed-off-by: kminoda * update default parameter Signed-off-by: kminoda * update comment Signed-off-by: kminoda * update readme Signed-off-by: kminoda * style(pre-commit): autofix * updated Signed-off-by: kminoda * minor update in readme Signed-off-by: kminoda * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda * update readme Signed-off-by: kminoda * style(pre-commit): autofix * Fix NG -> WARN Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: asa-naki --- sensing/imu_corrector/CMakeLists.txt | 25 +++- sensing/imu_corrector/README.md | 39 +++++-- .../config/gyro_bias_estimator.param.yaml | 6 + .../launch/gyro_bias_estimator.launch.xml | 16 +++ sensing/imu_corrector/package.xml | 2 + .../src/gyro_bias_estimation_module.cpp | 71 ++++++++++++ .../src/gyro_bias_estimation_module.hpp | 44 +++++++ .../imu_corrector/src/gyro_bias_estimator.cpp | 109 ++++++++++++++++++ .../imu_corrector/src/gyro_bias_estimator.hpp | 66 +++++++++++ .../imu_corrector/src/imu_corrector_core.cpp | 4 +- .../imu_corrector_core.hpp | 6 +- .../test/test_gyro_bias_estimation_module.cpp | 65 +++++++++++ 12 files changed, 437 insertions(+), 16 deletions(-) create mode 100644 sensing/imu_corrector/config/gyro_bias_estimator.param.yaml create mode 100644 sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml create mode 100644 sensing/imu_corrector/src/gyro_bias_estimation_module.cpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimation_module.hpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimator.cpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimator.hpp rename sensing/imu_corrector/{include/imu_corrector => src}/imu_corrector_core.hpp (89%) create mode 100644 sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 903a6bcab07c5..79e23af53a373 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -14,19 +14,42 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +ament_auto_add_library(gyro_bias_estimation_module SHARED + src/gyro_bias_estimation_module.cpp +) + ament_auto_add_library(imu_corrector_node SHARED src/imu_corrector_core.cpp - include/imu_corrector/imu_corrector_core.hpp ) +ament_auto_add_library(gyro_bias_estimator_node SHARED + src/gyro_bias_estimator.cpp +) + +target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module) + rclcpp_components_register_node(imu_corrector_node PLUGIN "imu_corrector::ImuCorrector" EXECUTABLE imu_corrector ) +rclcpp_components_register_node(gyro_bias_estimator_node + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator +) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + add_testcase(test/test_gyro_bias_estimation_module.cpp) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index aedbcbc3c09cd..54b4f70abda5e 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -1,6 +1,6 @@ # imu_corrector -## Purpose +## imu_corrector `imu_corrector_node` is a node that correct imu data. @@ -10,8 +10,6 @@ -## Inputs / Outputs - ### Input | Name | Type | Description | @@ -24,9 +22,7 @@ | --------- | ----------------------- | ------------------ | | `~output` | `sensor_msgs::msg::Imu` | corrected imu data | -## Parameters - -### Core Parameters +### Parameters | Name | Type | Description | | ---------------------------- | ------ | ------------------------------------- | @@ -37,12 +33,33 @@ | `angular_velocity_stddev_yy` | double | pitch rate standard deviation [rad/s] | | `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] | -## Assumptions / Known limits +## gyro_bias_estimator + +`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range. -## (Optional) Error detection and handling +Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped. -## (Optional) Performance characterization +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | -## (Optional) References/External links +### Output -## (Optional) Future extensions / Unimplemented parts +| Name | Type | Description | +| -------------------- | ------------------------------------ | ----------------------------- | +| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | + +### Parameters + +| 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] | +| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | +| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | +| `data_num_threshold` | int | number of data used to calculate bias | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml new file mode 100644 index 0000000000000..d5868e1df416a --- /dev/null +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.0015 # [rad/s] + velocity_threshold: 0.0 # [m/s] + timestamp_threshold: 0.1 # [s] + data_num_threshold: 200 # [num] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml new file mode 100644 index 0000000000000..a25ce5f90ed27 --- /dev/null +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 71fece2622338..c188b3709d9f1 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -13,6 +13,8 @@ rclcpp_components sensor_msgs + ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..2deb6088f6542 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_bias_estimation_module.hpp" + +namespace imu_corrector +{ +GyroBiasEstimationModule::GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold) +: velocity_threshold_(velocity_threshold), + timestamp_threshold_(timestamp_threshold), + data_num_threshold_(data_num_threshold), + is_stopped_(false) +{ +} + +void GyroBiasEstimationModule::update_gyro( + const double time, const geometry_msgs::msg::Vector3 & gyro) +{ + if (time - last_velocity_time_ > timestamp_threshold_) { + return; + } + if (!is_stopped_) { + return; + } + gyro_buffer_.push_back(gyro); + if (gyro_buffer_.size() > data_num_threshold_) { + gyro_buffer_.pop_front(); + } +} + +void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +{ + is_stopped_ = velocity <= velocity_threshold_; + last_velocity_time_ = time; +} + +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +{ + if (gyro_buffer_.size() < data_num_threshold_) { + throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + } + + geometry_msgs::msg::Vector3 bias; + bias.x = 0.0; + bias.y = 0.0; + bias.z = 0.0; + for (const auto & gyro : gyro_buffer_) { + bias.x += gyro.x; + bias.y += gyro.y; + bias.z += gyro.z; + } + bias.x /= gyro_buffer_.size(); + bias.y /= gyro_buffer_.size(); + bias.z /= gyro_buffer_.size(); + return bias; +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp new file mode 100644 index 0000000000000..6ebae942fff5d --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef GYRO_BIAS_ESTIMATION_MODULE_HPP_ +#define GYRO_BIAS_ESTIMATION_MODULE_HPP_ + +#include + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModule +{ +public: + GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold); + geometry_msgs::msg::Vector3 get_bias() const; + void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); + void update_velocity(const double time, const double velocity); + +private: + const double velocity_threshold_; + const double timestamp_threshold_; + const size_t data_num_threshold_; + bool is_stopped_; + std::deque gyro_buffer_; + + double last_velocity_time_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp new file mode 100644 index 0000000000000..42795805f803e --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -0,0 +1,109 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_bias_estimator.hpp" + +namespace imu_corrector +{ +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & node_options) +: Node("gyro_bias_validator", node_options), + gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), + angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), + angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), + angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + updater_(this), + gyro_bias_(std::nullopt) +{ + updater_.setHardwareID(get_name()); + updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + + const double velocity_threshold = declare_parameter("velocity_threshold"); + const double timestamp_threshold = declare_parameter("timestamp_threshold"); + const size_t data_num_threshold = + static_cast(declare_parameter("data_num_threshold")); + gyro_bias_estimation_module_ = std::make_unique( + velocity_threshold, timestamp_threshold, data_num_threshold); + + imu_sub_ = create_subscription( + "~/input/imu_raw", rclcpp::SensorDataQoS(), + [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); + twist_sub_ = create_subscription( + "~/input/twist", rclcpp::SensorDataQoS(), + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); + + gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); +} + +void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) +{ + // Update gyro data + gyro_bias_estimation_module_->update_gyro( + rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); + + // Estimate gyro bias + try { + gyro_bias_ = gyro_bias_estimation_module_->get_bias(); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + } + + // Publish results for debugging + if (gyro_bias_ != std::nullopt) { + Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); + gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); + } + + // Update diagnostics + updater_.force_update(); +} + +void GyroBiasEstimator::callback_twist( + const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + gyro_bias_estimation_module_->update_velocity( + rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); +} + +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 { + // 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"); + } + } +} + +} // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp new file mode 100644 index 0000000000000..37ca1d81dc8fa --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef GYRO_BIAS_ESTIMATOR_HPP_ +#define GYRO_BIAS_ESTIMATOR_HPP_ + +#include "gyro_bias_estimation_module.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimator : public rclcpp::Node +{ +private: + using Imu = sensor_msgs::msg::Imu; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + using Vector3 = geometry_msgs::msg::Vector3; + +public: + explicit GyroBiasEstimator(const rclcpp::NodeOptions & node_options); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); + void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + + rclcpp::Publisher::SharedPtr gyro_bias_pub_; + + std::unique_ptr gyro_bias_estimation_module_; + + const double gyro_bias_threshold_; + const double angular_velocity_offset_x_; + const double angular_velocity_offset_y_; + const double angular_velocity_offset_z_; + + diagnostic_updater::Updater updater_; + + std::optional gyro_bias_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index c540ab828c32c..491f720db7dd0 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "imu_corrector/imu_corrector_core.hpp" +#include "imu_corrector_core.hpp" + +#include namespace imu_corrector { diff --git a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp similarity index 89% rename from sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp rename to sensing/imu_corrector/src/imu_corrector_core.hpp index 4e4d154d92aac..30e361fe95b34 100644 --- a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ -#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#ifndef IMU_CORRECTOR_CORE_HPP_ +#define IMU_CORRECTOR_CORE_HPP_ #include @@ -42,4 +42,4 @@ class ImuCorrector : public rclcpp::Node }; } // namespace imu_corrector -#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..a0da4d0e24e17 --- /dev/null +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -0,0 +1,65 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/gyro_bias_estimation_module.hpp" + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModuleTest : public ::testing::Test +{ +protected: + double velocity_threshold = 1.0; + double timestamp_threshold = 5.0; + size_t data_num_threshold = 10; + GyroBiasEstimationModule module = + GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); +}; + +TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); + ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); + ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) +{ + ASSERT_THROW(module.get_bias(), std::runtime_error); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_THROW(module.get_bias(), std::runtime_error); +} +} // namespace imu_corrector From a5f078a737d5e3a2e325e49b86e9d34434bdad6a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 13 Sep 2023 16:25:28 +0200 Subject: [PATCH 2/4] build(imu_corrector): add missing diagnostic_updater dependency (#4980) Signed-off-by: Esteve Fernandez --- sensing/imu_corrector/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index c188b3709d9f1..5fba088d858fa 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto + diagnostic_updater rclcpp rclcpp_components sensor_msgs From e1bdf486a3d39ae01f4ce339b2227cdbaad80036 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Tue, 26 Sep 2023 17:04:11 +0900 Subject: [PATCH 3/4] add gyro_bias estimation in diag ( #5054) Signed-off-by: asa-naki --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 42795805f803e..47c1014062714 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,6 +83,12 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW 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_ && From 2ff87ed88ae45c45a52b63c79994db6d966c8a57 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 26 Sep 2023 08:05:06 +0000 Subject: [PATCH 4/4] ci(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 47c1014062714..c114ba6caba5d 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -88,7 +88,7 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW 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_); + 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_ &&