using filters from the filter package in/with ROS nodes

edit

My experience when I tried to use the MeanFilter by itself, was that there was something not working with the passing of the number_of_observations parameter, as I tried every namespace combination with no luck. I ended having to create a new class that extended the MeanFilter just to override the configure() method so I could pass the parameter then. Not very elegant, but as I said, I could not get the filter to work when instantiating it directly. You may also see this answer, although it didn't work for me: https://answers.ros.org/question/2847...

See example below:

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <filters/mean.h>

class MyMeanFilter: public filters::MeanFilter <double>
{
public:
  bool configure(int num_obs){
    number_of_observations_ = num_obs;
    data_storage_.reset(new filters::RealtimeCircularBuffer<double>(number_of_observations_, temp_));
    return true;
  }
};

class SimpleImuFilterNode
{
protected:
  ros::NodeHandle nh_;
  ros::Publisher output_pub_;
  ros::Subscriber imu_sub_;
  int num_obs_;
  MyMeanFilter mean_filter_;

public:
  SimpleImuFilterNode() 
  : mean_filter_()
  {
    ros::NodeHandle pnh_("~");
    output_pub_ = nh_.advertise<sensor_msgs::Imu>("imu_filtered_out", 10);
    imu_sub_    = nh_.subscribe("imu_in", 1, &SimpleImuFilterNode::callback, this);
    pnh_.param<int>("number_of_observations", num_obs_, 5);
    mean_filter_.configure(num_obs_);
  }

  void callback(const sensor_msgs::Imu::ConstPtr& msg_in)
  {
    sensor_msgs::Imu out_msg_;
    out_msg_.header = msg_in->header;
    mean_filter_.update (msg_in->linear_acceleration.x, out_msg_.linear_acceleration.x);
    output_pub_.publish(out_msg_);
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_filter_node");
  SimpleImuFilterNode simple_imu_filter_node;
  ros::spin();
  return 0;
}