41 #ifndef IIROB_FILTERS_LOW_PASS_FILTER_H 42 #define IIROB_FILTERS_LOW_PASS_FILTER_H 45 #include <geometry_msgs/WrenchStamped.h> 47 #include <iirob_filters/LowPassFilterParameters.h> 48 #include <iirob_filters/LowPassFilterConfig.h> 49 #include <dynamic_reconfigure/server.h> 63 virtual bool update(
const T& data_in, T& data_out);
77 iirob_filters::LowPassFilterParameters
params_;
107 ROS_ERROR(
"LowPassFilter did not find param SamplingFrequency");
109 ROS_ERROR(
"LowPassFilter did not find param DampingFrequency");
111 ROS_ERROR(
"LowPassFilter did not find param DampingIntensity");
113 ROS_ERROR(
"Divider value not correct - cannot be 0. Check .param or .yaml files");
115 ROS_INFO(
"Low Pass Filter Params: Sampling Frequency:%f, Damping Frequency:%f, Damping Intensity:%f; Divider: %d " ,
124 for (
int ii=0; ii<6; ii++)
138 msg_old[0] = data_in.wrench.force.x;
139 msg_old[1] = data_in.wrench.force.y;
140 msg_old[2] = data_in.wrench.force.z;
141 msg_old[3] = data_in.wrench.torque.x;
142 msg_old[4] = data_in.wrench.torque.y;
143 msg_old[5] = data_in.wrench.torque.z;
158 filtered_old_value = data_out;
164 template <
typename T>
177 template <
typename T>
194 virtual bool update(
const std::vector<T> & data_in, std::vector<T>& data_out);
208 iirob_filters::LowPassFilterParameters
params_;
215 template <
typename T>
220 template <
typename T>
225 template <
typename T>
234 ROS_ERROR(
"MultiChannelLowPassFilter did not find param SamplingFrequency");
236 ROS_ERROR(
"MultiChannelLowPassFilter did not find param DampingFrequency");
238 ROS_ERROR(
"MultiChannelLowPassFilter did not find param DampingIntensity");
240 ROS_ERROR(
"Divider value not correct - cannot be 0. Check .param or .yaml files");
242 ROS_INFO(
"Low Pass Filter Params: Sampling Frequency:%f, Damping Frequency:%f, Damping Intensity:%f; Divider: %d " ,
258 template <
typename T>
267 for (
int i = 0; i<data_in.size();i++)
270 filtered_old_value[i] = data_out[i];
Eigen::Matrix< double, 6, 1 > msg_filtered
std::vector< T > old_value
Eigen::Matrix< double, 6, 1 > msg_old
~MultiChannelLowPassFilter()
Destructor to clean up.
double sampling_frequency_
iirob_filters::LowPassFilterParameters params_
std::map< std::string, std::string > map_param_
virtual bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
Update the filter and return the data seperately.
double damping_frequency_
void reconfigureConfigurationRequest(iirob_filters::LowPassFilterConfig &config, uint32_t level)
unsigned int number_of_channels_
iirob_filters::LowPassFilterParameters params_
double filtered_old_value
double damping_frequency_
dynamic_reconfigure::Server< iirob_filters::LowPassFilterConfig > reconfigCalibrationSrv_
double sampling_frequency_
MultiChannelLowPassFilter()
Construct the filter with the expected width and height.
Eigen::Matrix< double, 6, 1 > msg_filtered_old
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
std::vector< T > filtered_value
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
double damping_intensity_
double damping_intensity_
virtual bool update(const T &data_in, T &data_out)
std::vector< T > filtered_old_value