Gazebo Common

API Reference

3.17.0
gz/common/MovingWindowFilter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 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 #ifndef GZ_COMMON_MOVINGWINDOWFILTER_HH_
18 #define GZ_COMMON_MOVINGWINDOWFILTER_HH_
19 
20 #include <memory>
21 #include <vector>
22 
23 namespace ignition
24 {
25  namespace common
26  {
30  template< typename T>
31  class MovingWindowFilterPrivate
32  {
33  // \brief Constructor
34  public: MovingWindowFilterPrivate();
35 
37  public: unsigned int valWindowSize = 4;
38 
40  public: std::vector<T> valHistory;
41 
43  public: typename std::vector<T>::iterator valIter;
44 
46  public: T sum;
47 
49  public: unsigned int samples = 0;
50  };
52 
54  template<typename T>
55  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
56  {
58  this->valHistory.resize(this->valWindowSize);
59  this->valIter = this->valHistory.begin();
60  this->sum = T();
61  }
62 
64  template< typename T>
66  {
68  public: MovingWindowFilter();
69 
71  public: virtual ~MovingWindowFilter();
72 
75  public: void Update(T _val);
76 
79  public: void SetWindowSize(unsigned int _n);
80 
83  public: unsigned int WindowSize() const;
84 
87  public: bool WindowFilled() const;
88 
91  public: T Value();
92 
95  protected: explicit MovingWindowFilter<T>(
96  MovingWindowFilterPrivate<T> &_d);
97 
100  };
101 
103  template<typename T>
105  : dataPtr(new MovingWindowFilterPrivate<T>())
106  {
107  }
108 
110  template<typename T>
112  {
113  this->dataPtr->valHistory.clear();
114  }
115 
117  template<typename T>
119  {
120  // update sum and sample size with incoming _val
121 
122  // keep running sum
123  this->dataPtr->sum += _val;
124 
125  // shift pointer, wrap around if end has been reached.
126  ++this->dataPtr->valIter;
127  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
128  {
129  // reset iterator to beginning of queue
130  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
131  }
132 
133  // increment sample size
134  ++this->dataPtr->samples;
135 
136  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
137  {
138  // subtract old value if buffer already filled
139  this->dataPtr->sum -= (*this->dataPtr->valIter);
140  // put new value into queue
141  (*this->dataPtr->valIter) = _val;
142  // reduce sample size
143  --this->dataPtr->samples;
144  }
145  else
146  {
147  // put new value into queue
148  (*this->dataPtr->valIter) = _val;
149  }
150  }
151 
153  template<typename T>
155  {
156  this->dataPtr->valWindowSize = _n;
157  this->dataPtr->valHistory.clear();
158  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
159  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
160  this->dataPtr->sum = T();
161  this->dataPtr->samples = 0;
162  }
163 
165  template<typename T>
167  {
168  return this->dataPtr->valWindowSize;
169  }
170 
172  template<typename T>
174  {
175  return this->dataPtr->samples == this->dataPtr->valWindowSize;
176  }
177 
179  template<typename T>
181  {
182  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
183  }
184  }
185 }
186 #endif
T resize(T... args)
MovingWindowFilter()
Constructor.
Definition: gz/common/MovingWindowFilter.hh:104
Forward declarations for the common classes.
std::unique_ptr< MovingWindowFilterPrivate< T > > dataPtr
Data pointer.
Definition: gz/common/MovingWindowFilter.hh:99
STL class.
unsigned int WindowSize() const
Get the window size.
Definition: gz/common/MovingWindowFilter.hh:166
void Update(T _val)
Update value of filter.
Definition: gz/common/MovingWindowFilter.hh:118
T Value()
Get filtered result.
Definition: gz/common/MovingWindowFilter.hh:180
bool WindowFilled() const
Get whether the window has been filled.
Definition: gz/common/MovingWindowFilter.hh:173
Base class for MovingWindowFilter.
Definition: gz/common/MovingWindowFilter.hh:65
virtual ~MovingWindowFilter()
Destructor.
Definition: gz/common/MovingWindowFilter.hh:111
STL class.
void SetWindowSize(unsigned int _n)
Set window size.
Definition: gz/common/MovingWindowFilter.hh:154