Added ability to reset IMU filters when ROS time jumps back (CCNYRobo…

…ticsLab#165)

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_;

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

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);

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<double>("orientation_stddev", 0.0);
     orientation_variance_ = orientation_stddev * orientation_stddev;
 
+    time_jump_threshold =
+        this->declare_parameter<double>("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

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

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();
 };

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);
+}

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<std::mutex> 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<std::mutex> 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<std::mutex> 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)