diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index b47ae1500f43b..7af82c1129aff 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -51,10 +51,16 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ----------------------------------------------- | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose | + +Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged. + +Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation. + +In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration. ### Output @@ -64,12 +70,11 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### 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 | +| 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] | +| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index d5868e1df416a..e10329c920137 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,5 @@ /**: 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] + timer_callback_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index a25ce5f90ed27..0d7cba9faa3a6 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -1,14 +1,14 @@ - + - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp index 2deb6088f6542..9b5719de69524 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,58 +14,106 @@ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + 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) +/** + * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY) + */ +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - 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(); + geometry_msgs::msg::Vector3 d_rpy{}; + double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds(); + for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) { + double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds(); + + d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x); + d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y); + d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z); + + t_prev = t_cur; } + return d_rpy; } -void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +/** + * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the + * ground-truth pose from "pose_list". + */ +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - is_stopped_ = velocity <= velocity_threshold_; - last_velocity_time_ = time; + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); + + geometry_msgs::msg::Vector3 error_rpy; + error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + return error_rpy; } -geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +/** + * @brief update gyroscope bias based on a given trajectory data + */ +void GyroBiasEstimationModule::update_bias( + const std::vector & pose_list, + const std::vector & gyro_list) { - if (gyro_buffer_.size() < data_num_threshold_) { - throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + const double dt_pose = + (rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = + (rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp)) + .seconds(); + if (dt_pose == 0 || dt_gyro == 0) { + throw std::runtime_error("dt_pose or dt_gyro is zero"); } - 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; + auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{}); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + gyro_bias_pair_.first.x += dt_pose * error_rpy.x; + gyro_bias_pair_.first.y += dt_pose * error_rpy.y; + gyro_bias_pair_.first.z += dt_pose * error_rpy.z; + gyro_bias_pair_.second.x += dt_pose * dt_pose; + gyro_bias_pair_.second.y += dt_pose * dt_pose; + gyro_bias_pair_.second.z += dt_pose * dt_pose; + + geometry_msgs::msg::Vector3 gyro_bias; + gyro_bias.x = error_rpy.x / dt_pose; + gyro_bias.y = error_rpy.y / dt_pose; + gyro_bias.z = error_rpy.z / dt_pose; +} + +/** + * @brief getter function for current estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const +{ + geometry_msgs::msg::Vector3 gyro_bias_base; + if ( + gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 || + gyro_bias_pair_.second.z == 0) { + throw std::runtime_error("gyro_bias_pair_.second is zero"); } - bias.x /= gyro_buffer_.size(); - bias.y /= gyro_buffer_.size(); - bias.z /= gyro_buffer_.size(); - return bias; + gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x; + gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y; + gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z; + return gyro_bias_base; } } // 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 index 6ebae942fff5d..dfa152d32c0d9 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -11,33 +11,30 @@ // 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 +#include #include +#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); + GyroBiasEstimationModule() = default; + void update_bias( + const std::vector & pose_list, + const std::vector & gyro_list); + geometry_msgs::msg::Vector3 get_bias_base_link() const; 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_; + std::pair gyro_bias_pair_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 5f41a9089b6ee..21bb51dc5f1f1 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -14,6 +14,13 @@ #include "gyro_bias_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include + namespace imu_corrector { GyroBiasEstimator::GyroBiasEstimator() @@ -22,59 +29,146 @@ GyroBiasEstimator::GyroBiasEstimator() 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")), + timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + straight_motion_ang_vel_upper_limit_( + declare_parameter("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); - 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); + gyro_bias_estimation_module_ = std::make_unique(); 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); }); - + pose_sub_ = create_subscription( + "~/input/pose", rclcpp::SensorDataQoS(), + [this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); + + auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto period_control = std::chrono::duration_cast( + std::chrono::duration(timer_callback_interval_sec_)); + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + transform_listener_ = std::make_shared(this); } 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()); + imu_frame_ = imu_msg_ptr->header.frame_id; + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_frame_, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_frame_).c_str()); + return; } + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = imu_msg_ptr->header.stamp; + gyro.vector = transform_vector3(imu_msg_ptr->angular_velocity, *tf_imu2base_ptr); + + gyro_all_.push_back(gyro); + // 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); } +} + +void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr) +{ + // push pose_msg to queue + geometry_msgs::msg::PoseStamped pose; + pose.header = pose_msg_ptr->header; + pose.pose = pose_msg_ptr->pose.pose; + pose_buf_.push_back(pose); +} + +void GyroBiasEstimator::timer_callback() +{ + if (pose_buf_.empty()) { + return; + } + + // Copy data + const std::vector pose_buf = pose_buf_; + const std::vector gyro_all = gyro_all_; + pose_buf_.clear(); + gyro_all_.clear(); + + // Check time + 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) { + return; + } + + // Filter gyro data + std::vector gyro_filtered; + for (const auto & gyro : gyro_all) { + const rclcpp::Time t = rclcpp::Time(gyro.header.stamp); + if (t0_rclcpp_time <= t && t < t1_rclcpp_time) { + gyro_filtered.push_back(gyro); + } + } + + // 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) { + return; + } + + // Check if the vehicle is moving straight + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_buf.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); + const double yaw_vel = yaw_diff / time_diff; + const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); + if (!is_straight) { + return; + } + + // Calculate gyro bias + gyro_bias_estimation_module_->update_bias(pose_buf, gyro_filtered); + + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = + transform_listener_->getLatestTransform(output_frame_, imu_frame_); + if (!tf_base2imu_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + return; + } + gyro_bias_ = + transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); - // Update diagnostics updater_.force_update(); } -void GyroBiasEstimator::callback_twist( - const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) { - gyro_bias_estimation_module_->update_velocity( - rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); + geometry_msgs::msg::Vector3Stamped vec_stamped; + vec_stamped.vector = vec; + + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + tf2::doTransform(vec_stamped, vec_stamped_transformed, transform); + return vec_stamped_transformed.vector; } void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index e74a0390af3aa..7eb06bcdcb365 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -15,17 +15,19 @@ #define GYRO_BIAS_ESTIMATOR_HPP_ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include -#include +#include #include #include #include #include #include +#include namespace imu_corrector { @@ -33,7 +35,7 @@ class GyroBiasEstimator : public rclcpp::Node { private: using Imu = sensor_msgs::msg::Imu; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; using Vector3 = geometry_msgs::msg::Vector3; @@ -43,12 +45,19 @@ class GyroBiasEstimator : public rclcpp::Node 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); + void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr); + void timer_callback(); - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; + static geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, + const geometry_msgs::msg::TransformStamped & transform); + + const std::string output_frame_ = "base_link"; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr gyro_bias_pub_; + rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr gyro_bias_estimation_module_; @@ -56,10 +65,19 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_x_; const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; + const double timer_callback_interval_sec_; + const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; std::optional gyro_bias_; + + std::shared_ptr transform_listener_; + + std::string imu_frame_; + + std::vector gyro_all_; + std::vector pose_buf_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp index a0da4d0e24e17..78558feeb7936 100644 --- a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -14,6 +14,8 @@ #include "../src/gyro_bias_estimation_module.hpp" +#include + #include namespace imu_corrector @@ -21,45 +23,67 @@ 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); + GyroBiasEstimationModule module; }; 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); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + geometry_msgs::msg::PoseStamped pose2; + pose2.header.stamp = rclcpp::Time(1e9); + pose2.pose.orientation.w = 1.0; + pose_list.push_back(pose2); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0.25 * 1e9); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + geometry_msgs::msg::Vector3Stamped gyro2; + gyro2.header.stamp = rclcpp::Time(0.5 * 1e9); + gyro2.vector.x = 0.1; + gyro2.vector.y = 0.2; + gyro2.vector.z = 0.3; + gyro_list.push_back(gyro2); + + for (size_t i = 0; i < 10; ++i) { + module.update_bias(pose_list, gyro_list); } - 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); + const geometry_msgs::msg::Vector3 result = module.get_bias_base_link(); + ASSERT_DOUBLE_EQ(result.x, 0.1); + ASSERT_DOUBLE_EQ(result.y, 0.2); + ASSERT_DOUBLE_EQ(result.z, 0.3); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) { - ASSERT_THROW(module.get_bias(), std::runtime_error); + ASSERT_THROW(module.get_bias_base_link(), 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); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + + for (size_t i = 0; i < 10; ++i) { + ASSERT_THROW(module.update_bias(pose_list, gyro_list), std::runtime_error); } - ASSERT_THROW(module.get_bias(), std::runtime_error); } } // namespace imu_corrector