Gazebo Common

API Reference

4.7.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 IGNITION_COMMON_MOVINGWINDOWFILTER_HH_
18 #define IGNITION_COMMON_MOVINGWINDOWFILTER_HH_
19 
20 #include <memory>
21 #include <vector>
22 
23 #include <gz/common/config.hh>
24 
25 namespace ignition
26 {
27  namespace common
28  {
32  template< typename T>
33  class MovingWindowFilterPrivate
34  {
35  // \brief Constructor
36  public: MovingWindowFilterPrivate();
37 
39  public: unsigned int valWindowSize = 4;
40 
42  public: std::vector<T> valHistory;
43 
45  public: typename std::vector<T>::iterator valIter;
46 
48  public: T sum;
49 
51  public: unsigned int samples = 0;
52  };
54 
56  template<typename T>
57  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
58  {
60  this->valHistory.resize(this->valWindowSize);
61  this->valIter = this->valHistory.begin();
62  this->sum = T();
63  }
64 
66  template< typename T>
68  {
70  public: MovingWindowFilter();
71 
73  public: virtual ~MovingWindowFilter();
74 
77  public: void Update(T _val);
78 
81  public: void SetWindowSize(unsigned int _n);
82 
85  public: unsigned int WindowSize() const;
86 
89  public: bool WindowFilled() const;
90 
93  public: T Value();
94 
97  protected: explicit MovingWindowFilter<T>(
98  MovingWindowFilterPrivate<T> &_d);
99 
102  };
103 
105  template<typename T>
107  : dataPtr(new MovingWindowFilterPrivate<T>())
108  {
109  }
110 
112  template<typename T>
114  {
115  this->dataPtr->valHistory.clear();
116  }
117 
119  template<typename T>
121  {
122  // update sum and sample size with incoming _val
123 
124  // keep running sum
125  this->dataPtr->sum += _val;
126 
127  // shift pointer, wrap around if end has been reached.
128  ++this->dataPtr->valIter;
129  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
130  {
131  // reset iterator to beginning of queue
132  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
133  }
134 
135  // increment sample size
136  ++this->dataPtr->samples;
137 
138  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
139  {
140  // subtract old value if buffer already filled
141  this->dataPtr->sum -= (*this->dataPtr->valIter);
142  // put new value into queue
143  (*this->dataPtr->valIter) = _val;
144  // reduce sample size
145  --this->dataPtr->samples;
146  }
147  else
148  {
149  // put new value into queue
150  (*this->dataPtr->valIter) = _val;
151  }
152  }
153 
155  template<typename T>
157  {
158  this->dataPtr->valWindowSize = _n;
159  this->dataPtr->valHistory.clear();
160  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
161  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
162  this->dataPtr->sum = T();
163  this->dataPtr->samples = 0;
164  }
165 
167  template<typename T>
169  {
170  return this->dataPtr->valWindowSize;
171  }
172 
174  template<typename T>
176  {
177  return this->dataPtr->samples == this->dataPtr->valWindowSize;
178  }
179 
181  template<typename T>
183  {
184  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
185  }
186  }
187 }
188 #endif
T resize(T... args)
MovingWindowFilter()
Constructor.
Definition: gz/common/MovingWindowFilter.hh:106
Forward declarations for the common classes.
std::unique_ptr< MovingWindowFilterPrivate< T > > dataPtr
Data pointer.
Definition: gz/common/MovingWindowFilter.hh:101
STL class.
unsigned int WindowSize() const
Get the window size.
Definition: gz/common/MovingWindowFilter.hh:168
void Update(T _val)
Update value of filter.
Definition: gz/common/MovingWindowFilter.hh:120
T Value()
Get filtered result.
Definition: gz/common/MovingWindowFilter.hh:182
bool WindowFilled() const
Get whether the window has been filled.
Definition: gz/common/MovingWindowFilter.hh:175
Base class for MovingWindowFilter.
Definition: gz/common/MovingWindowFilter.hh:67
virtual ~MovingWindowFilter()
Destructor.
Definition: gz/common/MovingWindowFilter.hh:113
STL class.
void SetWindowSize(unsigned int _n)
Set window size.
Definition: gz/common/MovingWindowFilter.hh:156