MovingWindowFilter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014-2016 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 #include <gazebo/math/Helpers.hh>
34 
35 namespace gazebo
36 {
37  namespace common
38  {
41 
45  template< typename T>
46  class MovingWindowFilterPrivate
47  {
48  // \brief Constructor
49  public: MovingWindowFilterPrivate();
50 
52  public: unsigned int valWindowSize;
53 
55  public: std::vector<T> valHistory;
56 
58  public: typename std::vector<T>::iterator valIter;
59 
61  public: T sum;
62 
64  public: unsigned int samples;
65  };
67 
69  template<typename T>
70  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
71  {
73  this->valWindowSize = 4;
74  this->valHistory.resize(this->valWindowSize);
75  this->valIter = this->valHistory.begin();
76  this->sum = T();
77  this->samples = 0;
78  }
79 
82  template< typename T>
84  {
86  public: MovingWindowFilter();
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>
122  : dataPtr(new MovingWindowFilterPrivate<T>())
123  {
124  }
125 
127  template<typename T>
129  {
130  this->dataPtr->valHistory.clear();
131  delete this->dataPtr;
132  this->dataPtr = NULL;
133  }
134 
136  template<typename T>
138  {
139  // update sum and sample size with incoming _val
140 
141  // keep running sum
142  this->dataPtr->sum += _val;
143 
144  // shift pointer, wrap around if end has been reached.
145  ++this->dataPtr->valIter;
146  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
147  {
148  // reset iterator to beginning of queue
149  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
150  }
151 
152  // increment sample size
153  ++this->dataPtr->samples;
154 
155  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
156  {
157  // subtract old value if buffer already filled
158  this->dataPtr->sum -= (*this->dataPtr->valIter);
159  // put new value into queue
160  (*this->dataPtr->valIter) = _val;
161  // reduce sample size
162  --this->dataPtr->samples;
163  }
164  else
165  {
166  // put new value into queue
167  (*this->dataPtr->valIter) = _val;
168  }
169  }
170 
172  template<typename T>
174  {
175  this->dataPtr->valWindowSize = _n;
176  this->dataPtr->valHistory.clear();
177  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
178  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
179  this->dataPtr->sum = T();
180  this->dataPtr->samples = 0;
181  }
182 
184  template<typename T>
186  {
187  return this->dataPtr->valWindowSize;
188  }
189 
191  template<typename T>
193  {
194  return this->dataPtr->samples == this->dataPtr->valWindowSize;
195  }
196 
198  template<typename T>
200  {
201  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
202  }
204  }
205 }
206 #endif
virtual ~MovingWindowFilter()
Destructor.
Definition: MovingWindowFilter.hh:128
Forward declarations for the common classes.
Definition: Animation.hh:33
unsigned int GetWindowSize() const
Get the window size.
Definition: MovingWindowFilter.hh:185
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition: MovingWindowFilter.hh:116
void SetWindowSize(unsigned int _n)
Set window size.
Definition: MovingWindowFilter.hh:173
Base class for MovingWindowFilter.
Definition: MovingWindowFilter.hh:83
T Get()
Get filtered result.
Definition: MovingWindowFilter.hh:199
#define NULL
Definition: CommonTypes.hh:31
void Update(T _val)
Update value of filter.
Definition: MovingWindowFilter.hh:137
MovingWindowFilter()
Constructor.
Definition: MovingWindowFilter.hh:121
bool GetWindowFilled() const
Get whether the window has been filled.
Definition: MovingWindowFilter.hh:192