MovingWindowFilter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef _GAZEBO_MOVING_WINDOW_FILTER_HH_
19 #define _GAZEBO_MOVING_WINDOW_FILTER_HH_
20 
21 #include <iostream>
22 #include <vector>
23 #include <map>
24 
25 #include <boost/function.hpp>
26 #include <boost/bind.hpp>
27 #include <boost/shared_ptr.hpp>
28 #include <boost/thread/mutex.hpp>
29 
30 #include <gazebo/gazebo_config.h>
31 #include <gazebo/common/Time.hh>
33 
34 namespace gazebo
35 {
36  namespace common
37  {
40 
44  template< typename T>
45  class MovingWindowFilterPrivate
46  {
47  // \brief Constructor
48  public: MovingWindowFilterPrivate<T>();
49 
51  public: unsigned int valWindowSize;
52 
54  public: std::vector<T> valHistory;
55 
57  public: typename std::vector<T>::iterator valIter;
58 
60  public: T sum;
62  public: unsigned int samples;
63  };
65 
68  template<typename T>
69  // false positive. Same code is fine if we remove the namespaces
70  // cppcheck-suppress uninitMemberVar
71  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate() :
72  valWindowSize(4),
73  sum(T()),
74  samples(0)
75  {
76  this->valHistory.resize(this->valWindowSize);
77  this->valIter = this->valHistory.begin();
78  }
79 
82  template< typename T>
84  {
86  public: MovingWindowFilter<T>();
87 
89  public: virtual ~MovingWindowFilter();
90 
93  public: void Update(T _val);
94 
97  public: void SetWindowSize(unsigned int _n);
98 
101  public: unsigned int GetWindowSize() const;
102 
105  public: bool GetWindowFilled() const;
106 
109  public: T Get();
110 
113  protected: MovingWindowFilter<T>(MovingWindowFilterPrivate<T> &_d);
114 
116  protected: MovingWindowFilterPrivate<T> *dataPtr;
117  };
118 
120  template<typename T>
121  // false positive. Same code is fine if we remove the namespaces
122  // cppcheck-suppress uninitMemberVar
124  : dataPtr(new MovingWindowFilterPrivate<T>())
125  {
126  }
127 
129  template<typename T>
131  {
132  this->dataPtr->valHistory.clear();
133  delete this->dataPtr;
134  this->dataPtr = nullptr;
135  }
136 
138  template<typename T>
140  {
141  // update sum and sample size with incoming _val
142 
143  // keep running sum
144  this->dataPtr->sum += _val;
145 
146  // shift pointer, wrap around if end has been reached.
147  ++this->dataPtr->valIter;
148  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
149  {
150  // reset iterator to beginning of queue
151  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
152  }
153 
154  // increment sample size
155  ++this->dataPtr->samples;
156 
157  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
158  {
159  // subtract old value if buffer already filled
160  this->dataPtr->sum -= (*this->dataPtr->valIter);
161  // put new value into queue
162  (*this->dataPtr->valIter) = _val;
163  // reduce sample size
164  --this->dataPtr->samples;
165  }
166  else
167  {
168  // put new value into queue
169  (*this->dataPtr->valIter) = _val;
170  }
171  }
172 
174  template<typename T>
176  {
177  this->dataPtr->valWindowSize = _n;
178  this->dataPtr->valHistory.clear();
179  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
180  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
181  this->dataPtr->sum = T();
182  this->dataPtr->samples = 0;
183  }
184 
186  template<typename T>
188  {
189  return this->dataPtr->valWindowSize;
190  }
191 
193  template<typename T>
195  {
196  return this->dataPtr->samples == this->dataPtr->valWindowSize;
197  }
198 
200  template<typename T>
202  {
203  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
204  }
206  }
207 }
208 #endif
bool GetWindowFilled() const
Get whether the window has been filled.
Definition: MovingWindowFilter.hh:194
virtual ~MovingWindowFilter()
Destructor.
Definition: MovingWindowFilter.hh:130
Forward declarations for the common classes.
Definition: Animation.hh:33
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition: MovingWindowFilter.hh:116
void SetWindowSize(unsigned int _n)
Set window size.
Definition: MovingWindowFilter.hh:175
unsigned int GetWindowSize() const
Get the window size.
Definition: MovingWindowFilter.hh:187
Base class for MovingWindowFilter.
Definition: MovingWindowFilter.hh:83
T Get()
Get filtered result.
Definition: MovingWindowFilter.hh:201
void Update(T _val)
Update value of filter.
Definition: MovingWindowFilter.hh:139
MovingWindowFilter()
Constructor.
Definition: MovingWindowFilter.hh:123