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: explicit MovingWindowFilter<T>(
114  MovingWindowFilterPrivate<T> &_d);
115 
117  protected: MovingWindowFilterPrivate<T> *dataPtr;
118  };
119 
121  template<typename T>
122  // false positive. Same code is fine if we remove the namespaces
123  // cppcheck-suppress uninitMemberVar
125  : dataPtr(new MovingWindowFilterPrivate<T>())
126  {
127  }
128 
130  template<typename T>
132  {
133  this->dataPtr->valHistory.clear();
134  delete this->dataPtr;
135  this->dataPtr = nullptr;
136  }
137 
139  template<typename T>
141  {
142  // update sum and sample size with incoming _val
143 
144  // keep running sum
145  this->dataPtr->sum += _val;
146 
147  // shift pointer, wrap around if end has been reached.
148  ++this->dataPtr->valIter;
149  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
150  {
151  // reset iterator to beginning of queue
152  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
153  }
154 
155  // increment sample size
156  ++this->dataPtr->samples;
157 
158  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
159  {
160  // subtract old value if buffer already filled
161  this->dataPtr->sum -= (*this->dataPtr->valIter);
162  // put new value into queue
163  (*this->dataPtr->valIter) = _val;
164  // reduce sample size
165  --this->dataPtr->samples;
166  }
167  else
168  {
169  // put new value into queue
170  (*this->dataPtr->valIter) = _val;
171  }
172  }
173 
175  template<typename T>
177  {
178  this->dataPtr->valWindowSize = _n;
179  this->dataPtr->valHistory.clear();
180  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
181  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
182  this->dataPtr->sum = T();
183  this->dataPtr->samples = 0;
184  }
185 
187  template<typename T>
189  {
190  return this->dataPtr->valWindowSize;
191  }
192 
194  template<typename T>
196  {
197  return this->dataPtr->samples == this->dataPtr->valWindowSize;
198  }
199 
201  template<typename T>
203  {
204  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
205  }
207  }
208 }
209 #endif
virtual ~MovingWindowFilter()
Destructor.
Definition: MovingWindowFilter.hh:131
Forward declarations for the common classes.
Definition: Animation.hh:26
unsigned int GetWindowSize() const
Get the window size.
Definition: MovingWindowFilter.hh:188
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition: MovingWindowFilter.hh:117
void SetWindowSize(unsigned int _n)
Set window size.
Definition: MovingWindowFilter.hh:176
Base class for MovingWindowFilter.
Definition: MovingWindowFilter.hh:83
T Get()
Get filtered result.
Definition: MovingWindowFilter.hh:202
void Update(T _val)
Update value of filter.
Definition: MovingWindowFilter.hh:140
MovingWindowFilter()
Constructor.
Definition: MovingWindowFilter.hh:124
bool GetWindowFilled() const
Get whether the window has been filled.
Definition: MovingWindowFilter.hh:195