From b15c146c55523a450749c4046d9868ed29b3ee74 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sun, 31 Jul 2022 22:28:17 +0200 Subject: [PATCH] Added ability to reset IMU filters when ROS time jumps back. Signed-off-by: Martin Pecka --- .../complementary_filter.h | 3 ++ .../complementary_filter_ros.h | 7 ++++ .../src/complementary_filter.cpp | 10 +++++ .../src/complementary_filter_ros.cpp | 39 +++++++++++++++++++ .../include/imu_filter_madgwick/imu_filter.h | 3 ++ .../imu_filter_madgwick/imu_filter_ros.h | 7 ++++ imu_filter_madgwick/src/imu_filter.cpp | 5 +++ imu_filter_madgwick/src/imu_filter_ros.cpp | 39 +++++++++++++++++++ imu_filter_madgwick/test/madgwick_test.cpp | 12 ++++++ 9 files changed, 125 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 df7a5f21..89fa1969 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 @@ -55,6 +55,9 @@ class ComplementaryFilterROS const ros::NodeHandle& nh_private); virtual ~ComplementaryFilterROS(); + // Reset the filter to the initial state. + void reset(); + private: // Convenience typedefs typedef sensor_msgs::Imu ImuMsg; @@ -89,10 +92,12 @@ class ComplementaryFilterROS bool publish_debug_topics_; std::string fixed_frame_; double orientation_variance_; + ros::Duration time_jump_threshold_; // State variables: ComplementaryFilter filter_; ros::Time time_prev_; + ros::Time last_ros_time_; bool initialized_filter_; void initializeParams(); @@ -103,6 +108,8 @@ class ComplementaryFilterROS tf::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 933330e2..f512f08a 100644 --- a/imu_complementary_filter/src/complementary_filter.cpp +++ b/imu_complementary_filter/src/complementary_filter.cpp @@ -456,6 +456,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 59e963a0..87ac207d 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -45,6 +45,8 @@ ComplementaryFilterROS::ComplementaryFilterROS( ROS_INFO("Starting ComplementaryFilterROS"); initializeParams(); + last_ros_time_ = ros::Time::now(); + int queue_size = 5; // Register publishers: @@ -117,6 +119,11 @@ void ComplementaryFilterROS::initializeParams() if (!nh_private_.getParam("orientation_stddev", orientation_stddev)) orientation_stddev = 0.0; + double time_jump_threshold{0.0}; + nh_private_.param("time_jump_threshold", time_jump_threshold, + time_jump_threshold); + time_jump_threshold_ = ros::Duration(time_jump_threshold); + orientation_variance_ = orientation_stddev * orientation_stddev; filter_.setDoBiasEstimation(do_bias_estimation); @@ -148,6 +155,8 @@ void ComplementaryFilterROS::initializeParams() void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw) { + checkTimeJump(); + const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration; const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity; const ros::Time& time = imu_msg_raw->header.stamp; @@ -179,6 +188,8 @@ void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw) void ComplementaryFilterROS::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, const MagMsg::ConstPtr& mag_msg) { + checkTimeJump(); + const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration; const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity; const geometry_msgs::Vector3& m = mag_msg->magnetic_field; @@ -295,4 +306,32 @@ void ComplementaryFilterROS::publish( } } +void ComplementaryFilterROS::checkTimeJump() +{ + const auto now = ros::Time::now(); + if (last_ros_time_.isZero() || last_ros_time_ <= now + time_jump_threshold_) + { + last_ros_time_ = now; + return; + } + + ROS_WARN("Detected jump back in time of %.1f s. Resetting IMU filter.", + (last_ros_time_ - now).toSec()); + + if (time_jump_threshold_.isZero() && ros::Time::isSystemTime()) + ROS_INFO( + "To ignore short time jumps back, use ~time_jump_threshold " + "parameter of the filter."); + + reset(); +} + +void ComplementaryFilterROS::reset() +{ + filter_.reset(); + time_prev_ = {}; + last_ros_time_ = ros::Time::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 4b6c94ab..b2872131 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); + + //! \brief Reset the filter to the initial state. + void reset(); }; #endif // IMU_FILTER_IMU_MADWICK_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 5455602f..9276b4fe 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 @@ -57,6 +57,9 @@ class ImuFilterRos ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private); virtual ~ImuFilterRos(); + //! \brief Reset the filter to the initial state. + void reset(); + private: // **** ROS-related @@ -90,11 +93,13 @@ class ImuFilterRos bool remove_gravity_vector_; geometry_msgs::Vector3 mag_bias_; double orientation_variance_; + ros::Duration time_jump_threshold_; // **** state variables boost::mutex mutex_; bool initialized_; ros::Time last_time_; + ros::Time last_ros_time_; // **** filter implementation ImuFilter filter_; @@ -114,6 +119,8 @@ class ImuFilterRos void checkTopicsTimerCallback(const ros::TimerEvent&); void applyYawOffset(double& q0, double& q1, double& q2, double& q3); + //! \brief Check whether ROS time has jumped back. If so, reset the filter. + void checkTimeJump(); }; #endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H diff --git a/imu_filter_madgwick/src/imu_filter.cpp b/imu_filter_madgwick/src/imu_filter.cpp index 4a705b75..8d3014c7 100644 --- a/imu_filter_madgwick/src/imu_filter.cpp +++ b/imu_filter_madgwick/src/imu_filter.cpp @@ -352,3 +352,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 eb21d7db..a68f1a2f 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -45,6 +45,10 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private) remove_gravity_vector_ = false; if (!nh_private_.getParam("publish_debug_topics", publish_debug_topics_)) publish_debug_topics_ = false; + double time_jump_threshold{0.0}; + nh_private_.param("time_jump_threshold", time_jump_threshold, + time_jump_threshold); + time_jump_threshold_ = ros::Duration(time_jump_threshold); double yaw_offset = 0.0; if (!nh_private_.getParam("yaw_offset", yaw_offset)) yaw_offset = 0.0; @@ -97,6 +101,8 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private) else ROS_INFO("The gravity vector is kept in the IMU message."); + last_ros_time_ = ros::Time::now(); + // **** register dynamic reconfigure config_server_.reset(new FilterConfigServer(nh_private_)); FilterConfigServer::CallbackType f = @@ -153,6 +159,8 @@ ImuFilterRos::~ImuFilterRos() void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw) { + checkTimeJump(); + boost::mutex::scoped_lock lock(mutex_); const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity; @@ -213,6 +221,8 @@ void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw) void ImuFilterRos::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, const MagMsg::ConstPtr& mag_msg) { + checkTimeJump(); + boost::mutex::scoped_lock lock(mutex_); const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity; @@ -443,3 +453,32 @@ void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&) << ros::names::resolve("imu") << "/data_raw" << "..."); } + +void ImuFilterRos::reset() +{ + boost::mutex::scoped_lock lock(mutex_); + filter_.reset(); + initialized_ = false; + last_time_ = {}; + last_ros_time_ = ros::Time::now(); +} + +void ImuFilterRos::checkTimeJump() +{ + const auto now = ros::Time::now(); + if (last_ros_time_.isZero() || last_ros_time_ <= now + time_jump_threshold_) + { + last_ros_time_ = now; + return; + } + + ROS_WARN("Detected jump back in time of %.1f s. Resetting IMU filter.", + (last_ros_time_ - now).toSec()); + + if (time_jump_threshold_.isZero() && ros::Time::isSystemTime()) + ROS_INFO( + "To ignore short time jumps back, use ~time_jump_threshold " + "parameter of the filter."); + + reset(); +} diff --git a/imu_filter_madgwick/test/madgwick_test.cpp b/imu_filter_madgwick/test/madgwick_test.cpp index 052cc18a..da9815a8 100644 --- a/imu_filter_madgwick/test/madgwick_test.cpp +++ b/imu_filter_madgwick/test/madgwick_test.cpp @@ -25,6 +25,12 @@ void filterStationary(float Ax, float Ay, float Az, float Mx, float My, } filter.getOrientation(q0, q1, q2, q3); + + // test resetting the filter + filter.reset(); + double rq0, rq1, rq2, rq3; + filter.getOrientation(rq0, rq1, rq2, rq3); + ASSERT_QUAT_EQUAL(rq0, rq1, rq2, rq3, 1.0, 0.0, 0.0, 0.0); } template @@ -48,6 +54,12 @@ void filterStationary(float Ax, float Ay, float Az, double& q0, double& q1, } filter.getOrientation(q0, q1, q2, q3); + + // test resetting the filter + filter.reset(); + double rq0, rq1, rq2, rq3; + filter.getOrientation(rq0, rq1, rq2, rq3); + ASSERT_QUAT_EQUAL(rq0, rq1, rq2, rq3, 1.0, 0.0, 0.0, 0.0); } #define TEST_STATIONARY_ENU(in_am, exp_result) \