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 // This fixes compiler warnings, see #3147 and #3160
26 #ifndef BOOST_BIND_GLOBAL_PLACEHOLDERS
27 #define BOOST_BIND_GLOBAL_PLACEHOLDERS
28 #endif
29 #include <boost/bind.hpp>
30 #include <boost/function.hpp>
31 #include <boost/shared_ptr.hpp>
32 #include <boost/thread/mutex.hpp>
33 
34 #include <gazebo/gazebo_config.h>
35 #include <gazebo/common/Time.hh>
37 
38 namespace gazebo
39 {
40  namespace common
41  {
44 
48  template< typename T>
49  class MovingWindowFilterPrivate
50  {
51  // \brief Constructor
52  public: MovingWindowFilterPrivate<T>();
53 
55  public: unsigned int valWindowSize;
56 
58  public: std::vector<T> valHistory;
59 
61  public: typename std::vector<T>::iterator valIter;
62 
64  public: T sum;
66  public: unsigned int samples;
67  };
69 
72  template<typename T>
73  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate() :
74  valWindowSize(4),
75  sum(T()),
76  samples(0)
77  {
78  this->valHistory.resize(this->valWindowSize);
79  this->valIter = this->valHistory.begin();
80  }
81 
84  template< typename T>
86  {
88  public: MovingWindowFilter<T>();
89 
91  public: virtual ~MovingWindowFilter();
92 
95  public: void Update(T _val);
96 
99  public: void SetWindowSize(unsigned int _n);
100 
103  public: unsigned int GetWindowSize() const;
104 
107  public: bool GetWindowFilled() const;
108 
111  public: T Get();
112 
115  protected: explicit MovingWindowFilter<T>(
116  MovingWindowFilterPrivate<T> &_d);
117 
119  protected: MovingWindowFilterPrivate<T> *dataPtr;
120  };
121 
123  template<typename T>
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
common
Definition: FuelModelDatabase.hh:42
Base class for MovingWindowFilter.
Definition: MovingWindowFilter.hh:86
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition: MovingWindowFilter.hh:119
void Update(T _val)
Update value of filter.
Definition: MovingWindowFilter.hh:140
unsigned int GetWindowSize() const
Get the window size.
Definition: MovingWindowFilter.hh:188
bool GetWindowFilled() const
Get whether the window has been filled.
Definition: MovingWindowFilter.hh:195
MovingWindowFilter()
Constructor.
Definition: MovingWindowFilter.hh:124
void SetWindowSize(unsigned int _n)
Set window size.
Definition: MovingWindowFilter.hh:176
T Get()
Get filtered result.
Definition: MovingWindowFilter.hh:202
virtual ~MovingWindowFilter()
Destructor.
Definition: MovingWindowFilter.hh:131
Forward declarations for the common classes.
Definition: Animation.hh:27