forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(imu_corrector): add gyro_bias_validator (backport autowarefounda…
…tion#4729) * feat(imu_corrector): add gyro_bias_validator Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update Signed-off-by: kminoda <[email protected]> * revert launch Signed-off-by: kminoda <[email protected]> * updat Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * add debug publisher Signed-off-by: kminoda <[email protected]> * minor fix Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * style(pre-commit): autofix * add gtest Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * updat e readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * add diagnostics Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * validator -> estimator Signed-off-by: kminoda <[email protected]> * fix build Signed-off-by: kminoda <[email protected]> * update default parameter Signed-off-by: kminoda <[email protected]> * update comment Signed-off-by: kminoda <[email protected]> * update readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * updated Signed-off-by: kminoda <[email protected]> * minor update in readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda <[email protected]> * update readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * Fix NG -> WARN Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: asa-naki <[email protected]>
- Loading branch information
Showing
12 changed files
with
437 additions
and
16 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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] |
16 changes: 16 additions & 0 deletions
16
sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<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 { | ||
| // 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) |
Oops, something went wrong.