Gazebo Common

API Reference

6.0.0
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
23namespace gz
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