Gazebo Common

API Reference

7.0.0~pre1
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#include <gz/common/Export.hh>
23
24namespace gz
25{
26 namespace common
27 {
31 template< typename T>
32 class MovingWindowFilterPrivate
33 {
34 // \brief Constructor
35 public: MovingWindowFilterPrivate();
36
38 public: unsigned int valWindowSize = 4;
39
41 public: std::vector<T> valHistory;
42
44 public: typename std::vector<T>::iterator valIter;
45
47 public: T sum;
48
50 public: unsigned int samples = 0;
51 };
53
55 template<typename T>
56 MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
57 {
59 this->valHistory.resize(this->valWindowSize);
60 this->valIter = this->valHistory.begin();
61 this->sum = T();
62 }
63
65 template< typename T>
67 {
69 public: GZ_DEPRECATED(7) MovingWindowFilter();
70
72 public: GZ_DEPRECATED(7) virtual ~MovingWindowFilter();
73
76 public: void Update(T _val);
77
80 public: void SetWindowSize(unsigned int _n);
81
84 public: unsigned int WindowSize() const;
85
88 public: bool WindowFilled() const;
89
92 public: T Value();
93
96 protected: GZ_DEPRECATED(7) explicit MovingWindowFilter(
97 MovingWindowFilterPrivate<T> &_d);
98
100 protected: std::unique_ptr<MovingWindowFilterPrivate<T>> dataPtr;
101 };
102
104 template<typename T>
106 : dataPtr(new MovingWindowFilterPrivate<T>())
107 {
108 }
109
111 template<typename T>
113 {
114 this->dataPtr->valHistory.clear();
115 }
116
118 template<typename T>
120 {
121 // update sum and sample size with incoming _val
122
123 // keep running sum
124 this->dataPtr->sum += _val;
125
126 // shift pointer, wrap around if end has been reached.
127 ++this->dataPtr->valIter;
128 if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
129 {
130 // reset iterator to beginning of queue
131 this->dataPtr->valIter = this->dataPtr->valHistory.begin();
132 }
133
134 // increment sample size
135 ++this->dataPtr->samples;
136
137 if (this->dataPtr->samples > this->dataPtr->valWindowSize)
138 {
139 // subtract old value if buffer already filled
140 this->dataPtr->sum -= (*this->dataPtr->valIter);
141 // put new value into queue
142 (*this->dataPtr->valIter) = _val;
143 // reduce sample size
144 --this->dataPtr->samples;
145 }
146 else
147 {
148 // put new value into queue
149 (*this->dataPtr->valIter) = _val;
150 }
151 }
152
154 template<typename T>
156 {
157 this->dataPtr->valWindowSize = _n;
158 this->dataPtr->valHistory.clear();
159 this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
160 this->dataPtr->valIter = this->dataPtr->valHistory.begin();
161 this->dataPtr->sum = T();
162 this->dataPtr->samples = 0;
163 }
164
166 template<typename T>
168 {
169 return this->dataPtr->valWindowSize;
170 }
171
173 template<typename T>
175 {
176 return this->dataPtr->samples == this->dataPtr->valWindowSize;
177 }
178
180 template<typename T>
182 {
183 return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
184 }
185 }
186}
187#endif