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#include <boost/function.hpp>
26#include <boost/bind.hpp>
27#include <boost/shared_ptr.hpp>
28#include <boost/thread/mutex.hpp>
29
30#include <gazebo/gazebo_config.h>
31#include <gazebo/common/Time.hh>
33
34namespace gazebo
35{
36 namespace common
37 {
40
44 template< typename T>
45 class MovingWindowFilterPrivate
46 {
47 // \brief Constructor
48 public: MovingWindowFilterPrivate<T>();
49
51 public: unsigned int valWindowSize;
52
54 public: std::vector<T> valHistory;
55
57 public: typename std::vector<T>::iterator valIter;
58
60 public: T sum;
62 public: unsigned int samples;
63 };
65
68 template<typename T>
69 // false positive. Same code is fine if we remove the namespaces
70 // cppcheck-suppress uninitMemberVar
71 MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate() :
72 valWindowSize(4),
73 sum(T()),
74 samples(0)
75 {
76 this->valHistory.resize(this->valWindowSize);
77 this->valIter = this->valHistory.begin();
78 }
79
82 template< typename T>
84 {
86 public: MovingWindowFilter<T>();
87
89 public: virtual ~MovingWindowFilter();
90
93 public: void Update(T _val);
94
97 public: void SetWindowSize(unsigned int _n);
98
101 public: unsigned int GetWindowSize() const;
102
105 public: bool GetWindowFilled() const;
106
109 public: T Get();
110
113 protected: explicit MovingWindowFilter<T>(
114 MovingWindowFilterPrivate<T> &_d);
115
117 protected: MovingWindowFilterPrivate<T> *dataPtr;
118 };
119
121 template<typename T>
122 // false positive. Same code is fine if we remove the namespaces
123 // cppcheck-suppress uninitMemberVar
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:37
Base class for MovingWindowFilter.
Definition MovingWindowFilter.hh:84
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition MovingWindowFilter.hh:117
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