util.h

Go to the documentation of this file.

#ifndef CREATE_UTIL_H
#define CREATE_UTIL_H

#include <sys/time.h>

#define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl)
#define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl)

namespace create {
  namespace util {

    static const uint8_t STREAM_HEADER = 19;
    static const float V_3_TICKS_PER_REV = 508.8;
    static const uint32_t V_3_MAX_ENCODER_TICKS = 65535;
    static const float MAX_RADIUS = 2.0;
    static const float STRAIGHT_RADIUS = 32.768;
    static const float IN_PLACE_RADIUS = 0.001;
    static const float PI = 3.14159;
    static const float TWO_PI = 6.28318;
    static const float EPS = 0.0001;

    inline float normalizeAngle(const float& angle) {
      float a = angle;
      while (a < -PI) a += TWO_PI;
      while (a > PI) a -= TWO_PI;
      return a;
    };

    typedef unsigned long long timestamp_t;

    static timestamp_t getTimestamp() {
      struct timeval now;
      gettimeofday(&now, NULL);
      return now.tv_usec + (timestamp_t) now.tv_sec * 1000000;
    }

    inline bool willFloatOverflow(const float a, const float b) {
      return ( (a < 0.0) == (b < 0.0) && std::abs(b) > std::numeric_limits<float>::max() - std::abs(a) );
    }
  }  // namespace util
} // namespace create

#endif // CREATE_UTIL_H