##

MovingWindowFilter.hh
Go to the documentation of this file.
/*
* Copyright (C) 2014 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef _GAZEBO_MOVING_WINDOW_FILTER_HH_
#define _GAZEBO_MOVING_WINDOW_FILTER_HH_
#include <iostream>
#include <vector>
#include <map>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <gazebo/gazebo_config.h>
#include <gazebo/common/Time.hh>
#include <gazebo/common/CommonTypes.hh>
namespace gazebo
{
namespace common
{
template< typename T>
class MovingWindowFilterPrivate
{
// \brief Constructor
public: MovingWindowFilterPrivate<T>();
public: unsigned int valWindowSize;
public: std::vector<T> valHistory;
public: typename std::vector<T>::iterator valIter;
public: T sum;
public: unsigned int samples;
};
template<typename T>
// false positive. Same code is fine if we remove the namespaces
// cppcheck-suppress uninitMemberVar
MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate() :
valWindowSize(4),
sum(T()),
samples(0)
{
this->valHistory.resize(this->valWindowSize);
this->valIter = this->valHistory.begin();
}
template< typename T>
class MovingWindowFilter
{
public: MovingWindowFilter<T>();
public: virtual ~MovingWindowFilter();
public: void Update(T _val);
public: void SetWindowSize(unsigned int _n);
public: unsigned int GetWindowSize() const;
public: bool GetWindowFilled() const;
public: T Get();
protected: explicit MovingWindowFilter<T>(
MovingWindowFilterPrivate<T> &_d);
protected: MovingWindowFilterPrivate<T> *dataPtr;
};
template<typename T>
// false positive. Same code is fine if we remove the namespaces
// cppcheck-suppress uninitMemberVar
MovingWindowFilter<T>::MovingWindowFilter()
: dataPtr(new MovingWindowFilterPrivate<T>())
{
}
template<typename T>
MovingWindowFilter<T>::~MovingWindowFilter()
{
this->dataPtr->valHistory.clear();
delete this->dataPtr;
this->dataPtr = nullptr;
}
template<typename T>
void MovingWindowFilter<T>::Update(T _val)
{
// update sum and sample size with incoming _val
// keep running sum
this->dataPtr->sum += _val;
// shift pointer, wrap around if end has been reached.
++this->dataPtr->valIter;
if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
{
// reset iterator to beginning of queue
this->dataPtr->valIter = this->dataPtr->valHistory.begin();
}
// increment sample size
++this->dataPtr->samples;
if (this->dataPtr->samples > this->dataPtr->valWindowSize)
{
// subtract old value if buffer already filled
this->dataPtr->sum -= (*this->dataPtr->valIter);
// put new value into queue
(*this->dataPtr->valIter) = _val;
// reduce sample size
--this->dataPtr->samples;
}
else
{
// put new value into queue
(*this->dataPtr->valIter) = _val;
}
}
template<typename T>
void MovingWindowFilter<T>::SetWindowSize(unsigned int _n)
{
this->dataPtr->valWindowSize = _n;
this->dataPtr->valHistory.clear();
this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
this->dataPtr->valIter = this->dataPtr->valHistory.begin();
this->dataPtr->sum = T();
this->dataPtr->samples = 0;
}
template<typename T>
unsigned int MovingWindowFilter<T>::GetWindowSize() const
{
return this->dataPtr->valWindowSize;
}
template<typename T>
bool MovingWindowFilter<T>::GetWindowFilled() const
{
return this->dataPtr->samples == this->dataPtr->valWindowSize;
}
template<typename T>
T MovingWindowFilter<T>::Get()
{
return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
}
}
}
#endif
Time.hh
gazebo
Forward declarations for the common classes.
Definition: Animation.hh:26
gazebo::common::MovingWindowFilter::~MovingWindowFilter
virtual ~MovingWindowFilter()
Destructor.
Definition: MovingWindowFilter.hh:131
gazebo::common::MovingWindowFilter::GetWindowSize
unsigned int GetWindowSize() const
Get the window size.
Definition: MovingWindowFilter.hh:188
common
common
Definition: FuelModelDatabase.hh:37
gazebo::common::MovingWindowFilter::SetWindowSize
void SetWindowSize(unsigned int _n)
Set window size.
Definition: MovingWindowFilter.hh:176
CommonTypes.hh
gazebo::common::MovingWindowFilter::dataPtr
MovingWindowFilterPrivate< T > * dataPtr
Data pointer.
Definition: MovingWindowFilter.hh:117
gazebo::common::MovingWindowFilter::Get
T Get()
Get filtered result.
Definition: MovingWindowFilter.hh:202
gazebo::common::MovingWindowFilter
Base class for MovingWindowFilter.
Definition: MovingWindowFilter.hh:83
gazebo::common::MovingWindowFilter::MovingWindowFilter
MovingWindowFilter()
Constructor.
Definition: MovingWindowFilter.hh:124
gazebo::common::MovingWindowFilter::Update
void Update(T _val)
Update value of filter.
Definition: MovingWindowFilter.hh:140
gazebo::common::MovingWindowFilter::GetWindowFilled
bool GetWindowFilled() const
Get whether the window has been filled.
Definition: MovingWindowFilter.hh:195