sr_math_utils.hpp

Go to the documentation of this file.

#ifndef _SR_MATH_UTILS_HPP_
#define _SR_MATH_UTILS_HPP_

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/thread/detail/singleton.hpp>


#include <ros/ros.h>

namespace sr_math_utils
{
  static const double pi = 3.14159265;

  static inline double to_rad(double degrees)
  {
    return degrees * 0.017453292519943295;
  }

  static inline double to_degrees(double rad)
  {
    return rad * 57.295779513082323;
  }

  static inline int ipow(int base, int exp)
  {
    int result = 1;
    while (exp)
    {
      if (exp & 1)
        result *= base;
      exp >>= 1;
      base *= base;
    }

    return result;
  }

  static inline bool is_bit_mask_index_true(long int bit_mask, int index)
  {
    if ( bit_mask & (((long int)1)<<index) )
      return true;
    else
      return false;
  }

  static inline bool is_bit_mask_index_false(long int bit_mask, int index)
  {
    return !(is_bit_mask_index_true(bit_mask, index));
  }

  static inline uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
  {
    uint16_t last_value = full_value &    0xFFFF;       // Split the full value into the lower part
    full_value   &= (uint64_t)0xFFFFFFFFFFFF0000LL;     // and the overflow part

    if( new_value < last_value)                         // if we overflowed
      full_value += (uint64_t)0x0000000000010000LL;     // then count the overflow

    full_value   |= (uint64_t)new_value;                // replace the bottom 16 bits with their new value

    return full_value;
  }


  static inline double linear_interpolate_(double x,
                                           double x0, double y0,
                                           double x1, double y1)
  {
    //y1 - y0
    double y = y1 - y0;
    // (y1 - y0) / (x1 - x0)
    y /= (x1 - x0);
    //  (x-x0)*((y1-y0)/(x1-x0))
    y *= (x - x0);
    //  y0 + (x-x0)*((y1-y0)/(x1-x0))
    y += y0;

    return y;
  }

  static inline int sign(double x)
  {
    return x < 0.0 ? -1 : 1;
  }

  //  FILTERS  //
  namespace filters
  {

    class LowPassFilter
    {
    public:
      LowPassFilter(double tau= 0.05)
        : is_first(true), dt(0.0),
          timestamp_1(0.0), q_prev(0.0)
      {
        this->tau = tau;
        value_derivative.first = 0.0;
        value_derivative.second = 0.0;
      };

      std::pair<double, double> compute(double q, double timestamp)
      {
        if( is_first )
        {
          q_prev = q;
          //initializing dt to 1ms
          dt = 0.001;

          is_first = false;
        }
        else
          dt = timestamp - timestamp_1;

        double alpha = exp(-dt / tau);

        //filtering the input
        value_derivative.first = alpha * value_derivative.first + (1 - alpha)*q;
        //filtering the derivative
        value_derivative.second = alpha * value_derivative.second + (1 - alpha) / dt * (q - q_prev);

        q_prev = q;
        timestamp_1 = timestamp;

        return value_derivative;
      };

    private:
      bool is_first;
      double tau, dt, timestamp_1, q_prev;

      std::pair<double, double> value_derivative;
    };

    class AlphaBetaFilter
    {
    public:
      AlphaBetaFilter(double alpha = 0.85, double beta = 0.05)
        : a(alpha), b(beta),
          xk_1(0.0), vk_1(0.0), xk(0.0), vk(0.0), rk(0.0),
          dt(0.0), timestamp_1(0.0)
      {};

      ~AlphaBetaFilter()
      {};

      std::pair<double, double> compute(double xm, double timestamp)
      {
        dt = timestamp - timestamp_1;

        xk = xk_1 + ( vk_1 * dt );
        vk = vk_1;

        rk = xm - xk;

        xk += a * rk;
        vk += ( b * rk ) / dt;

        value_derivative.first = xk;
        value_derivative.second = vk;

        xk_1 = xk;
        vk_1 = vk;
        timestamp_1 = timestamp;

        return value_derivative;
      };

    protected:
      double a, b;
      double xk_1, vk_1, xk, vk, rk;
      double dt, timestamp_1;

      std::pair<double, double> value_derivative;
    };
  };


  //  RANDOM   //

  class Random_
  {
  public:
    Random_()
    {
      dist = boost::uniform_real<>(0.0, 1.0);
    };

    ~Random_()
    {};

    template <typename T>
    T generate(T min = static_cast<T>(0), T max = static_cast<T>(1))
    {
      return static_cast<T>( min + dist(gen) * (max - min)  );
    }

  protected:
    boost::mt19937 gen;
    boost::uniform_real<> dist;
  };

  typedef boost::detail::thread::singleton < class Random_ > Random;
}

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/

#endif