Merge pull request #202 from enwaytech:sr/reverse-backport-from-noetic

Author: mintar

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:
@@ -109,6 +111,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();
@@ -138,6 +141,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);
 
@@ -172,6 +180,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;
@@ -203,6 +213,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;
@@ -323,4 +335,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

@@ -26,7 +26,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 
+#include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2_ros/transform_listener.h"
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <message_filters/subscriber.hpp>
 #include <message_filters/sync_policies/approximate_time.hpp>
@@ -44,6 +47,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     typedef sensor_msgs::msg::Imu ImuMsg;
     typedef sensor_msgs::msg::MagneticField MagMsg;
     typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg;
+    typedef geometry_msgs::msg::PoseStamped PoseMsg;
 
     typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg>
         SyncPolicy;
@@ -55,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,
@@ -68,7 +75,10 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_filtered_debug_publisher_;
     rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_raw_debug_publisher_;
     rclcpp::Publisher<ImuMsg>::SharedPtr imu_publisher_;
+    rclcpp::Publisher<PoseMsg>::SharedPtr orientation_filtered_publisher_;
     tf2_ros::TransformBroadcaster tf_broadcaster_;
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
 
     rclcpp::TimerBase::SharedPtr check_topics_timer_;
 
@@ -90,11 +100,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
@@ -103,6 +115,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     // **** member functions
     void publishFilteredMsg(ImuMsg::ConstSharedPtr imu_msg_raw);
     void publishTransform(ImuMsg::ConstSharedPtr imu_msg_raw);
+    void publishOrientationFiltered(const ImuMsg& imu_msg);
 
     void publishRawMsg(const rclcpp::Time& t, float roll, float pitch,
                        float yaw);
@@ -112,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

@@ -37,6 +37,8 @@ using namespace std::placeholders;
 ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
     : BaseNode("imu_filter_madgwick", options),
       tf_broadcaster_(*this),
+      tf_buffer_(this->get_clock()),
+      tf_listener_(tf_buffer_),
       initialized_(false)
 {
     RCLCPP_INFO(get_logger(), "Starting ImuFilter");
@@ -61,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);
@@ -128,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};
@@ -151,7 +161,7 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
     add_parameter("mag_bias_z", rclcpp::ParameterValue(0.0), float_range,
                   "Magnetometer bias (hard iron correction), z component.");
     double orientation_stddev;
-    float_range = {0.0, 1.0, 0};
+    float_range = {0.0, 100.0, 0};
     add_parameter("orientation_stddev", rclcpp::ParameterValue(0.0),
                   float_range,
                   "Standard deviation of the orientation estimate.");
@@ -191,6 +201,10 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
         rpy_raw_debug_publisher_ =
             create_publisher<geometry_msgs::msg::Vector3Stamped>("imu/rpy/raw",
                                                                  5);
+
+        orientation_filtered_publisher_ =
+            create_publisher<geometry_msgs::msg::PoseStamped>(
+                "imu/orientation_filtered", 5);
     }
 
     // **** register subscribers
@@ -220,6 +234,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;
@@ -285,6 +301,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;
@@ -480,9 +498,40 @@ void ImuFilterMadgwickRos::publishFilteredMsg(
 
         rpy.header = imu_msg_raw->header;
         rpy_filtered_debug_publisher_->publish(rpy);
+
+        publishOrientationFiltered(imu_msg);
     }
 }
 
+void ImuFilterMadgwickRos::publishOrientationFiltered(const ImuMsg &imu_msg)
+{
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.stamp = imu_msg.header.stamp;
+    pose.header.frame_id = fixed_frame_;
+    pose.pose.orientation = imu_msg.orientation;
+
+    // get the current transform from the fixed frame to the imu frame
+    geometry_msgs::msg::TransformStamped transform;
+    try
+    {
+        transform = tf_buffer_.lookupTransform(
+            fixed_frame_, imu_msg.header.frame_id, imu_msg.header.stamp,
+            rclcpp::Duration::from_seconds(0.1));
+    } catch (tf2::TransformException &ex)
+    {
+        RCLCPP_WARN(
+            this->get_logger(), "Could not get transform from %s to %s: %s",
+            fixed_frame_.c_str(), imu_msg.header.frame_id.c_str(), ex.what());
+        return;
+    }
+
+    pose.pose.position.x = transform.transform.translation.x;
+    pose.pose.position.y = transform.transform.translation.y;
+    pose.pose.position.z = transform.transform.translation.z;
+
+    orientation_filtered_publisher_->publish(pose);
+}
+
 void ImuFilterMadgwickRos::publishRawMsg(const rclcpp::Time &t, float roll,
                                          float pitch, float yaw)
 {
@@ -545,6 +594,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)