MovingWindowFilter.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014-2016 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>
#include <gazebo/math/Helpers.hh>

namespace gazebo
{
  namespace common
  {

    template< typename T>
    class MovingWindowFilterPrivate
    {
      // \brief Constructor
      public: MovingWindowFilterPrivate();

      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()
    {
      this->valWindowSize = 4;
      this->valHistory.resize(this->valWindowSize);
      this->valIter = this->valHistory.begin();
      this->sum = T();
      this->samples = 0;
    }

    template< typename T>
    class MovingWindowFilter
    {
      public: MovingWindowFilter();

      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: 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 = NULL;
    }

    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