Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef FILTERS_TRANSFER_FUNCTION_H_
00033 #define FILTERS_TRANSFER_FUNCTION_H_
00034
00035 #include <stdint.h>
00036 #include <math.h>
00037 #include <vector>
00038 #include <string>
00039
00040 #include <boost/scoped_ptr.hpp>
00041
00042 #include "filters/filter_base.h"
00043 #include "filters/realtime_circular_buffer.h"
00044
00045 namespace filters
00046 {
00047
00048
00072
00073 template <typename T>
00074 class SingleChannelTransferFunctionFilter: public filters::FilterBase <T>
00075 {
00076 public:
00080 SingleChannelTransferFunctionFilter() ;
00081
00084 ~SingleChannelTransferFunctionFilter();
00085
00090 virtual bool configure();
00091
00096 virtual bool update(const T & data_in, T& data_out) ;
00097
00098
00099
00100 protected:
00101
00102 boost::scoped_ptr<RealtimeCircularBuffer<T > > input_buffer_;
00103 boost::scoped_ptr<RealtimeCircularBuffer<T > > output_buffer_;
00104
00105 T temp_;
00106
00107 std::vector<double> a_;
00108 std::vector<double> b_;
00109
00110 };
00111
00112 template <typename T>
00113 SingleChannelTransferFunctionFilter<T>::SingleChannelTransferFunctionFilter()
00114 {
00115 };
00116
00117 template <typename T>
00118 SingleChannelTransferFunctionFilter<T>::~SingleChannelTransferFunctionFilter()
00119 {
00120 };
00121
00122 template <typename T>
00123 bool SingleChannelTransferFunctionFilter<T>::configure()
00124 {
00125
00126
00127 if (!FilterBase<T>::getParam("a", a_))
00128 {
00129 ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
00130 return false;
00131 }
00132
00133
00134 if (!FilterBase<T>::getParam("b", b_))
00135 {
00136 ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
00137 return false;
00138 }
00139
00140
00141 input_buffer_.reset(new RealtimeCircularBuffer<T >(b_.size()-1, temp_));
00142 output_buffer_.reset(new RealtimeCircularBuffer<T >(a_.size()-1, temp_));
00143
00144
00145 if ( a_[0] == 0)
00146 {
00147 ROS_ERROR("a[0] can not equal 0.");
00148 return false;
00149 }
00150
00151
00152 if(a_[0] != 1)
00153 {
00154 for(uint32_t i = 0; i < b_.size(); i++)
00155 {
00156 b_[i] = (b_[i] / a_[0]);
00157 }
00158 for(uint32_t i = 1; i < a_.size(); i++)
00159 {
00160 a_[i] = (a_[i] / a_[0]);
00161 }
00162 a_[0] = (a_[0] / a_[0]);
00163 }
00164
00165 return true;
00166 };
00167
00168
00169 template <typename T>
00170 bool SingleChannelTransferFunctionFilter<T>::update(const T & data_in, T & data_out)
00171 {
00172 if (!FilterBase<T>::configured_)
00173 return false;
00174
00175
00176 temp_ = data_in;
00177
00178 data_out=b_[0] * temp_;
00179
00180 for (uint32_t row = 1; row <= input_buffer_->size(); row++)
00181 {
00182 data_out += b_[row] * (*input_buffer_)[row-1];
00183 }
00184 for (uint32_t row = 1; row <= output_buffer_->size(); row++)
00185 {
00186 data_out -= a_[row] * (*output_buffer_)[row-1];
00187 }
00188
00189 input_buffer_->push_front(temp_);
00190 output_buffer_->push_front(data_out);
00191
00192 return true;
00193 };
00194
00195
00196
00197
00222
00223
00224 template <typename T>
00225 class MultiChannelTransferFunctionFilter: public filters::MultiChannelFilterBase <T>
00226 {
00227 public:
00231 MultiChannelTransferFunctionFilter() ;
00232
00235 ~MultiChannelTransferFunctionFilter();
00236
00241 virtual bool configure();
00242
00247 virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
00248
00249
00250
00251 protected:
00252
00253 boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > input_buffer_;
00254 boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > output_buffer_;
00255
00256 std::vector<T> temp_;
00257
00258 std::vector<double> a_;
00259 std::vector<double> b_;
00260
00261 };
00262
00263 template <typename T>
00264 MultiChannelTransferFunctionFilter<T>::MultiChannelTransferFunctionFilter()
00265 {
00266 };
00267
00268 template <typename T>
00269 MultiChannelTransferFunctionFilter<T>::~MultiChannelTransferFunctionFilter()
00270 {
00271 };
00272
00273 template <typename T>
00274 bool MultiChannelTransferFunctionFilter<T>::configure()
00275 {
00276
00277 if (!FilterBase<T>::getParam("a", a_))
00278 {
00279 ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
00280 return false;
00281 }
00282
00283
00284 if (!FilterBase<T>::getParam("b", b_))
00285 {
00286 ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
00287 return false;
00288 }
00289
00290
00291 temp_.resize(this->number_of_channels_);
00292 input_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(b_.size()-1, temp_));
00293 output_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(a_.size()-1, temp_));
00294
00295
00296 if ( a_[0] == 0)
00297 {
00298 ROS_ERROR("a[0] can not equal 0.");
00299 return false;
00300 }
00301
00302
00303 if(a_[0] != 1)
00304 {
00305 for(uint32_t i = 0; i < b_.size(); i++)
00306 {
00307 b_[i] = (b_[i] / a_[0]);
00308 }
00309 for(uint32_t i = 1; i < a_.size(); i++)
00310 {
00311 a_[i] = (a_[i] / a_[0]);
00312 }
00313 a_[0] = (a_[0] / a_[0]);
00314 }
00315
00316 return true;
00317 };
00318
00319
00320 template <typename T>
00321 bool MultiChannelTransferFunctionFilter<T>::update(const std::vector<T> & data_in, std::vector<T> & data_out)
00322 {
00323
00324
00325 if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_ )
00326 {
00327 ROS_ERROR("Number of channels is %d, but data_in.size() = %d and data_out.size() = %d. They must match", this->number_of_channels_, (int)data_in.size(), (int)data_out.size());
00328 return false;
00329 }
00330
00331 temp_ = data_in;
00332
00333 for (uint32_t i = 0; i < temp_.size(); i++)
00334 {
00335 data_out[i]=b_[0] * temp_[i];
00336
00337 for (uint32_t row = 1; row <= input_buffer_->size(); row++)
00338 {
00339 (data_out)[i] += b_[row] * (*input_buffer_)[row-1][i];
00340 }
00341 for (uint32_t row = 1; row <= output_buffer_->size(); row++)
00342 {
00343 (data_out)[i] -= a_[row] * (*output_buffer_)[row-1][i];
00344 }
00345 }
00346 input_buffer_->push_front(temp_);
00347 output_buffer_->push_front(data_out);
00348 return true;
00349 };
00350
00351 }
00352
00353 #endif //#ifndef FILTERS_TRAjNSFER_FUNCTION_H_