using filters from the filter package in/with ROS nodes
HI i am trying to implement filters in ROS that have to filter incoming analog data from a pin on a UR10 robot. At the moment i do not understand what is necessary to implement such a filter because the package does not seem to have launch files. I cannot find clear documentation at the moment as well. The packages seems to work only with classes that have been defined for four type of filters, but then i still do not know how to set parameters to those filters. Do i simply use the class in for example C and then build the filter functionality myself? At the moment i am working on a median filter.
i have already looked at the filter tutorials, but those seem to set up the entire functionality them selves.
Asked by stefvanlierop on 2019-04-15 05:35:57 UTC
Answers
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/284761/setting-parameters-for-mean-filter/
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;
}
Asked by NickCitizen on 2019-05-21 16:44:08 UTC
Comments
it's all C++, no C.
Asked by gvdhoorn on 2019-04-15 07:02:48 UTC