added an optional low-pass filter in imu enablable

include/subscriber_imu_enablable.h

@@ -46,6 +46,12 @@ class SubscriberImuEnablable : public SubscriberImu
         FrameBasePtr last_frame_;
         ProcessorMotionPtr processor_motion_;
 
+        // LOW PASS FILTER
+        bool lowpass_filter_;
+        double lowpass_alpha_;
+        sensor_msgs::Imu::Ptr filtered_msg_;
+        Eigen::Vector6d filtered_vector_;
+        ros::Publisher pub_filtered_imu_;
 
     public:
         // Constructor

src/subscriber_imu_enablable.cpp

@@ -12,10 +12,19 @@ SubscriberImuEnablable::SubscriberImuEnablable(const std::string& _unique_name,
                                                const SensorBasePtr _sensor_ptr) :
         SubscriberImu(_unique_name, _server, _sensor_ptr),
         enabled_(false),
-        last_frame_(nullptr)
+        last_frame_(nullptr),
+        filtered_msg_(boost::make_shared<sensor_msgs::Imu>()),
+        filtered_vector_(Eigen::Vector6d::Zero())
 {
     topic_enable_           = _server.getParam<std::string>(prefix_ + "/topic_enable");
     duration_init_static_   = _server.getParam<double>(prefix_ + "/static_init_duration");
+    lowpass_filter_         = _server.getParam<bool>(prefix_ + "/lowpass_filter");
+    if (lowpass_filter_)
+    {
+        double fc = _server.getParam<double>(prefix_ + "/lowpass_cutoff_freq");
+        double dt = _server.getParam<double>(prefix_ + "/dt");
+        lowpass_alpha_ = (2 * M_PI * dt * fc) / (2 * M_PI * dt * fc + 1);
+    }
 
     for (auto proc : sensor_ptr_->getProcessorList())
     {
@@ -32,13 +41,51 @@ void SubscriberImuEnablable::initialize(ros::NodeHandle& nh, const std::string&
     SubscriberImu::initialize(nh,topic);
     sub_        = nh.subscribe(topic, 1, &SubscriberImuEnablable::callback, this);
     enable_sub_ = nh.subscribe(topic_enable_, 1, &SubscriberImuEnablable::enableCallback, this);
+
+    if (lowpass_filter_)
+        pub_filtered_imu_ = nh.advertise<sensor_msgs::Imu>(topic + "_imu_filtered", 1);
 }
 
 void SubscriberImuEnablable::callback(const sensor_msgs::Imu::ConstPtr& msg)
 {
+    // LOW PASS FILTER
+    if (lowpass_filter_)
+    {
+        Eigen::Vector6d input_vector;
+
+        input_vector << msg->angular_velocity.x,
+                        msg->angular_velocity.y,
+                        msg->angular_velocity.z,
+                        msg->linear_acceleration.x,
+                        msg->linear_acceleration.y,
+                        msg->linear_acceleration.z;
+
+        // filter
+        if (filtered_vector_.isZero(1e-9)) // first time
+            filtered_vector_ = input_vector;
+        else
+            filtered_vector_ = lowpass_alpha_ * input_vector + (1 - lowpass_alpha_) * filtered_vector_;
+
+        // store output
+        *filtered_msg_ = *msg;
+        filtered_msg_->angular_velocity.x       = filtered_vector_(0);
+        filtered_msg_->angular_velocity.y       = filtered_vector_(1);
+        filtered_msg_->angular_velocity.z       = filtered_vector_(2);
+        filtered_msg_->linear_acceleration.x    = filtered_vector_(3);
+        filtered_msg_->linear_acceleration.y    = filtered_vector_(4);
+        filtered_msg_->linear_acceleration.z    = filtered_vector_(5);
+
+        // publish
+        pub_filtered_imu_.publish(filtered_msg_);
+    }
+
+    // PROCESS DATA
     if (enabled_)
     {
-        SubscriberImu::callback(msg);
+        if (lowpass_filter_)
+            SubscriberImu::callback(filtered_msg_);
+        else
+            SubscriberImu::callback(msg);
 
         /* STATIC INITIALIZATION
          * impose zero velocity and same position & orientation to the frames within the first 'duration_init_static_' seconds