All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 #include <gazebo/math/Helpers.hh>
34 
35 namespace gazebo
36 {
37  namespace common
38  {
41 
45  template< typename T>
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  };
66 
68  template<typename T>
70  {
72  this->valWindowSize = 4;
73  this->valHistory.resize(this->valWindowSize);
74  this->valIter = this->valHistory.begin();
75  this->sum = T();
76  this->samples = 0;
77  }
78 
81  template< typename T>
83  {
85  public: MovingWindowFilter();
86 
88  public: virtual ~MovingWindowFilter();
89 
92  public: void Update(T _val);
93 
96  public: void SetWindowSize(unsigned int _n);
97 
100  public: unsigned int GetWindowSize() const;
101 
104  public: bool GetWindowFilled() const;
105 
108  public: T Get();
109 
113 
116  };
117 
119  template<typename T>
121  : dataPtr(new MovingWindowFilterPrivate<T>())
122  {
123  }
124 
126  template<typename T>
128  {
129  this->dataPtr->valHistory.clear();
130  delete this->dataPtr;
131  this->dataPtr = NULL;
132  }
133 
135  template<typename T>
137  {
138  // update sum and sample size with incoming _val
139 
140  // keep running sum
141  this->dataPtr->sum += _val;
142 
143  // shift pointer, wrap around if end has been reached.
144  ++this->dataPtr->valIter;
145  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
146  {
147  // reset iterator to beginning of queue
148  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
149  }
150 
151  // increment sample size
152  ++this->dataPtr->samples;
153 
154  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
155  {
156  // subtract old value if buffer already filled
157  this->dataPtr->sum -= (*this->dataPtr->valIter);
158  // put new value into queue
159  (*this->dataPtr->valIter) = _val;
160  // reduce sample size
161  --this->dataPtr->samples;
162  }
163  else
164  {
165  // put new value into queue
166  (*this->dataPtr->valIter) = _val;
167  }
168  }
169 
171  template<typename T>
173  {
174  this->dataPtr->valWindowSize = _n;
175  this->dataPtr->valHistory.clear();
176  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
177  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
178  this->dataPtr->sum = T();
179  this->dataPtr->samples = 0;
180  }
181 
183  template<typename T>
185  {
186  return this->dataPtr->valWindowSize;
187  }
188 
190  template<typename T>
192  {
193  return this->dataPtr->samples == this->dataPtr->valWindowSize;
194  }
195 
197  template<typename T>
199  {
200  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
201  }
203  }
204 }
205 #endif