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>
 
// This fixes compiler warnings, see #3147 and #3160
#ifndef BOOST_BIND_GLOBAL_PLACEHOLDERS
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#endif
#include <boost/bind.hpp>
#include <boost/function.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>
    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>
    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