41 #ifndef IIROB_FILTERS_MOVING_MEAN_FILTER_H 42 #define IIROB_FILTERS_MOVING_MEAN_FILTER_H 45 #include <geometry_msgs/WrenchStamped.h> 46 #include <iirob_filters/MovingMeanParameters.h> 60 virtual bool update(
const T & data_in, T& data_out);
64 iirob_filters::MovingMeanParameters
params_;
91 ROS_ERROR(
"MovingMeanFilter did not find param divider");
105 values.push_back(data_in);
108 sum.wrench.force.x = 0.0;
109 sum.wrench.force.y = 0.0;
110 sum.wrench.force.z = 0.0;
111 sum.wrench.torque.x = 0.0;
112 sum.wrench.torque.y = 0.0;
113 sum.wrench.torque.x = 0.0;
114 for(
int i =0 ; i<
values.size(); ++i)
116 sum.wrench.force.x +=
values[i].wrench.force.x;
117 sum.wrench.force.y +=
values[i].wrench.force.y;
118 sum.wrench.force.z +=
values[i].wrench.force.z;
119 sum.wrench.torque.x +=
values[i].wrench.torque.x;
120 sum.wrench.torque.y +=
values[i].wrench.torque.y;
121 sum.wrench.torque.z +=
values[i].wrench.torque.z;
124 data_out.wrench.force.x = sum.wrench.force.x /
values.size();
125 data_out.wrench.force.y = sum.wrench.force.y /
values.size();
126 data_out.wrench.force.z = sum.wrench.force.z /
values.size();
127 data_out.wrench.torque.x = sum.wrench.torque.x /
values.size();
128 data_out.wrench.torque.y = sum.wrench.torque.y /
values.size();
129 data_out.wrench.torque.z = sum.wrench.torque.z /
values.size();
virtual bool update(const T &data_in, T &data_out)
iirob_filters::MovingMeanParameters params_