From 8a3e5bb25f08e8495d895648f2a5461053e90c11 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Tue, 9 Jul 2024 14:06:30 +0200 Subject: [PATCH] Added ability to reset IMU filters when ROS time jumps back (#165) --- .../complementary_filter.h | 3 ++ .../complementary_filter_ros.h | 8 ++++ .../src/complementary_filter.cpp | 10 ++++ .../src/complementary_filter_ros.cpp | 45 ++++++++++++++++++ .../include/imu_filter_madgwick/imu_filter.h | 3 ++ .../imu_filter_madgwick/imu_filter_ros.h | 8 ++++ imu_filter_madgwick/src/imu_filter.cpp | 5 ++ imu_filter_madgwick/src/imu_filter_ros.cpp | 46 +++++++++++++++++++ 8 files changed, 128 insertions(+) diff --git a/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h b/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h index 4758cad2..9a0530a7 100644 --- a/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h +++ b/imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h @@ -87,6 +87,9 @@ class ComplementaryFilter void update(double ax, double ay, double az, double wx, double wy, double wz, double mx, double my, double mz, double dt); + // Reset the filter to the initial state. + void reset(); + private: static const double kGravity; static const double gamma_; diff --git a/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h b/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h index 9c9a44f5..1111b903 100644 --- a/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h +++ b/imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h @@ -57,6 +57,9 @@ class ComplementaryFilterROS : public rclcpp::Node ComplementaryFilterROS(); ~ComplementaryFilterROS() override; + // Reset the filter to the initial state. + void reset(); + private: // Convenience typedefs typedef sensor_msgs::msg::Imu ImuMsg; @@ -86,10 +89,12 @@ class ComplementaryFilterROS : public rclcpp::Node bool publish_debug_topics_{}; std::string fixed_frame_; double orientation_variance_{}; + rclcpp::Duration time_jump_threshold_duration_{0, 0}; // State variables: ComplementaryFilter filter_; rclcpp::Time time_prev_; + rclcpp::Time last_ros_time_; bool initialized_filter_; void initializeParams(); @@ -100,6 +105,9 @@ class ComplementaryFilterROS : public rclcpp::Node tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2, double q3) const; + + // Check whether ROS time has jumped back. If so, reset the filter. + void checkTimeJump(); }; } // namespace imu_tools diff --git a/imu_complementary_filter/src/complementary_filter.cpp b/imu_complementary_filter/src/complementary_filter.cpp index 1581ed91..e44525ea 100644 --- a/imu_complementary_filter/src/complementary_filter.cpp +++ b/imu_complementary_filter/src/complementary_filter.cpp @@ -454,6 +454,16 @@ double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, return factor * alpha; } +void ComplementaryFilter::reset() +{ + initialized_ = false; + steady_state_ = false; + q0_ = 1.0; + q1_ = q2_ = q3_ = 0.0; + wx_bias_ = wy_bias_ = wz_bias_ = 0.0; + wx_prev_ = wy_prev_ = wz_prev_ = 0.0; +} + void normalizeVector(double& x, double& y, double& z) { double norm = sqrt(x * x + y * y + z * z); diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index 96f1bdaa..59dff3f1 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -49,6 +49,8 @@ ComplementaryFilterROS::ComplementaryFilterROS() RCLCPP_INFO(this->get_logger(), "Starting ComplementaryFilterROS"); initializeParams(); + last_ros_time_ = this->get_clock()->now(); + int queue_size = 5; // Register publishers: @@ -99,6 +101,7 @@ void ComplementaryFilterROS::initializeParams() double bias_alpha; bool do_adaptive_gain; double orientation_stddev; + double time_jump_threshold; // set "Not Dynamically Reconfigurable Parameters" auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); @@ -128,6 +131,11 @@ void ComplementaryFilterROS::initializeParams() this->declare_parameter("orientation_stddev", 0.0); orientation_variance_ = orientation_stddev * orientation_stddev; + time_jump_threshold = + this->declare_parameter("time_jump_threshold", 0.0); + time_jump_threshold_duration_ = + rclcpp::Duration::from_seconds(time_jump_threshold); + filter_.setDoBiasEstimation(do_bias_estimation); filter_.setDoAdaptiveGain(do_adaptive_gain); @@ -162,6 +170,8 @@ void ComplementaryFilterROS::initializeParams() void ComplementaryFilterROS::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) { + checkTimeJump(); + const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; const rclcpp::Time &time = imu_msg_raw->header.stamp; @@ -193,6 +203,8 @@ void ComplementaryFilterROS::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) void ComplementaryFilterROS::imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, MagMsg::ConstSharedPtr mag_msg) { + checkTimeJump(); + const geometry_msgs::msg::Vector3 &a = imu_msg_raw->linear_acceleration; const geometry_msgs::msg::Vector3 &w = imu_msg_raw->angular_velocity; const geometry_msgs::msg::Vector3 &m = mag_msg->magnetic_field; @@ -313,4 +325,37 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) } } +void ComplementaryFilterROS::checkTimeJump() +{ + const auto now = this->get_clock()->now(); + if (last_ros_time_ == rclcpp::Time(0, 0, last_ros_time_.get_clock_type()) || + last_ros_time_ <= now + time_jump_threshold_duration_) + { + last_ros_time_ = now; + return; + } + + RCLCPP_WARN(this->get_logger(), + "Detected jump back in time of %.1f s. Resetting IMU filter.", + (last_ros_time_ - now).seconds()); + + if (time_jump_threshold_duration_ == rclcpp::Duration(0, 0) && + this->get_clock()->get_clock_type() == RCL_SYSTEM_TIME) + { + RCLCPP_INFO(this->get_logger(), + "To ignore short time jumps back, use ~time_jump_threshold " + "parameter of the filter."); + } + + reset(); +} + +void ComplementaryFilterROS::reset() +{ + filter_.reset(); + time_prev_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type()); + last_ros_time_ = this->get_clock()->now(); + initialized_filter_ = false; +} + } // namespace imu_tools diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h index 05d0754b..f4cc9edc 100644 --- a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h +++ b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h @@ -97,6 +97,9 @@ class ImuFilter float az, float dt); void getGravity(float& rx, float& ry, float& rz, float gravity = 9.80665); + + // Reset the filter to the initial state. + void reset(); }; #endif // IMU_FILTER_MADWICK_IMU_FILTER_H diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h index 8ecc02ee..a0851f7b 100644 --- a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h +++ b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h @@ -59,6 +59,9 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode IMU_FILTER_MADGWICK_CPP_PUBLIC explicit ImuFilterMadgwickRos(const rclcpp::NodeOptions& options); + // Reset the filter to the initial state. + void reset(); + // Callbacks are public so they can be called when used as a library void imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw); void imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, @@ -98,11 +101,13 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode geometry_msgs::msg::Vector3 mag_bias_; double orientation_variance_; double yaw_offset_total_; + rclcpp::Duration time_jump_threshold_duration_{0, 0}; // **** state variables std::mutex mutex_; bool initialized_; rclcpp::Time last_time_; + rclcpp::Time last_ros_time_; tf2::Quaternion yaw_offsets_; // **** filter implementation @@ -120,4 +125,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode void checkTopicsTimerCallback(); void applyYawOffset(double& q0, double& q1, double& q2, double& q3); + + // Check whether ROS time has jumped back. If so, reset the filter. + void checkTimeJump(); }; diff --git a/imu_filter_madgwick/src/imu_filter.cpp b/imu_filter_madgwick/src/imu_filter.cpp index e2a948a1..9a83edd3 100644 --- a/imu_filter_madgwick/src/imu_filter.cpp +++ b/imu_filter_madgwick/src/imu_filter.cpp @@ -342,3 +342,8 @@ void ImuFilter::getGravity(float& rx, float& ry, float& rz, float gravity) break; } } + +void ImuFilter::reset() +{ + setOrientation(1, 0, 0, 0); +} diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 63421f55..84ef3eb8 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -63,6 +63,12 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) declare_parameter("publish_debug_topics", false, descriptor); get_parameter("publish_debug_topics", publish_debug_topics_); + double time_jump_threshold = 0.0; + declare_parameter("time_jump_threshold", 0.0, descriptor); + get_parameter("time_jump_threshold", time_jump_threshold); + time_jump_threshold_duration_ = + rclcpp::Duration::from_seconds(time_jump_threshold); + double yaw_offset = 0.0; declare_parameter("yaw_offset", 0.0, descriptor); get_parameter("yaw_offset", yaw_offset); @@ -130,6 +136,8 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) "The gravity vector is kept in the IMU message."); } + last_ros_time_ = this->get_clock()->now(); + // **** define reconfigurable parameters. double gain; floating_point_range float_range = {0.0, 1.0, 0}; @@ -229,6 +237,8 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) void ImuFilterMadgwickRos::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) { + checkTimeJump(); + std::lock_guard lock(mutex_); const geometry_msgs::msg::Vector3 &ang_vel = imu_msg_raw->angular_velocity; @@ -294,6 +304,8 @@ void ImuFilterMadgwickRos::imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw) void ImuFilterMadgwickRos::imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw, MagMsg::ConstSharedPtr mag_msg) { + checkTimeJump(); + std::lock_guard lock(mutex_); const geometry_msgs::msg::Vector3 &ang_vel = imu_msg_raw->angular_velocity; @@ -590,6 +602,40 @@ void ImuFilterMadgwickRos::checkTopicsTimerCallback() << "..."); } +void ImuFilterMadgwickRos::checkTimeJump() +{ + const auto now = this->get_clock()->now(); + if (last_ros_time_ == rclcpp::Time(0, 0, last_ros_time_.get_clock_type()) || + last_ros_time_ <= now + time_jump_threshold_duration_) + { + last_ros_time_ = now; + return; + } + + RCLCPP_WARN(this->get_logger(), + "Detected jump back in time of %.1f s. Resetting IMU filter.", + (last_ros_time_ - now).seconds()); + + if (time_jump_threshold_duration_ == rclcpp::Duration(0, 0) && + this->get_clock()->get_clock_type() == RCL_SYSTEM_TIME) + { + RCLCPP_INFO(this->get_logger(), + "To ignore short time jumps back, use ~time_jump_threshold " + "parameter of the filter."); + } + + reset(); +} + +void ImuFilterMadgwickRos::reset() +{ + std::lock_guard lock(mutex_); + filter_.reset(); + initialized_ = false; + last_time_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type()); + last_ros_time_ = this->get_clock()->now(); +} + #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ImuFilterMadgwickRos)