feat(imu_corrector): add gyro_bias_validator (backport autowarefounda…

…tion#4729) (#856) * feat(imu_corrector): add gyro_bias_validator (backport autowarefoundation#4729) * feat(imu_corrector): add gyro_bias_validator Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * revert launch Signed-off-by: kminoda <koji.minoda@tier4.jp> * updat Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add debug publisher Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * style(pre-commit): autofix * add gtest Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * updat e readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add diagnostics Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * validator -> estimator Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix build Signed-off-by: kminoda <koji.minoda@tier4.jp> * update default parameter Signed-off-by: kminoda <koji.minoda@tier4.jp> * update comment Signed-off-by: kminoda <koji.minoda@tier4.jp> * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * updated Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor update in readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda <koji.minoda@tier4.jp> * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * Fix NG -> WARN Signed-off-by: kminoda <koji.minoda@tier4.jp> --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * build(imu_corrector): add missing diagnostic_updater dependency (autowarefoundation#4980) Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> * add gyro_bias estimation in diag ( autowarefoundation#5054) Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> * ci(pre-commit): autofix --------- Signed-off-by: asa-naki <akihisa.nagata@tier4.jp> Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · kimurariku · Jul 14, 2023 · Aug 16, 2023 · Aug 25, 2023 · Aug 28, 2023
commit 56027b6c3b4b149782c72f68ae4ae9f6b45a960d
diff --git 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
@@ -1,6 +1,6 @@
 # imu_corrector
 
-## Purpose
+## imu_corrector
 
 `imu_corrector_node` is a node that correct imu data.
 
@@ -10,8 +10,6 @@
 <!-- TODO(TIER IV): Make this repository public or change the link. -->
 <!-- Use the value estimated by [deviation_estimator](https://github.com/tier4/calibration_tools/tree/main/localization/deviation_estimation_tools) as the parameters for this node. -->
 
-## 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
@@ -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
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_imu_raw" default="imu_raw"/>
+  <arg name="input_twist" default="twist"/>
+  <arg name="output_gyro_bias" default="gyro_bias"/>
+  <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
+  <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
+
+  <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
+    <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
+    <remap from="~/input/twist" to="$(var input_twist)"/>
+    <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
+    <param from="$(var gyro_bias_estimator_param_file)"/>
+    <param from="$(var imu_corrector_param_file)"/>
+  </node>
+</launch>
diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml
@@ -9,10 +9,13 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp 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
@@ -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 <geometry_msgs/msg/vector3.hpp>
+
+#include <deque>
+
+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<geometry_msgs::msg::Vector3> 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
@@ -0,0 +1,115 @@
+// 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<double>("gyro_bias_threshold")),
+  angular_velocity_offset_x_(declare_parameter<double>("angular_velocity_offset_x")),
+  angular_velocity_offset_y_(declare_parameter<double>("angular_velocity_offset_y")),
+  angular_velocity_offset_z_(declare_parameter<double>("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<double>("velocity_threshold");
+  const double timestamp_threshold = declare_parameter<double>("timestamp_threshold");
+  const size_t data_num_threshold =
+    static_cast<size_t>(declare_parameter<int>("data_num_threshold"));
+  gyro_bias_estimation_module_ = std::make_unique<GyroBiasEstimationModule>(
+    velocity_threshold, timestamp_threshold, data_num_threshold);
+
+  imu_sub_ = create_subscription<Imu>(
+    "~/input/imu_raw", rclcpp::SensorDataQoS(),
+    [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); });
+  twist_sub_ = create_subscription<TwistWithCovarianceStamped>(
+    "~/input/twist", rclcpp::SensorDataQoS(),
+    [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); });
+
+  gyro_bias_pub_ = create_publisher<Vector3Stamped>("~/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 {
+    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");
+    }
+  }
+}
+
+}  // namespace imu_corrector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator)