00001 00032 #ifndef CREATE_UTIL_H 00033 #define CREATE_UTIL_H 00034 00035 #include <sys/time.h> 00036 00037 #define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl) 00038 #define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl) 00039 00040 namespace create { 00041 namespace util { 00042 00043 static const uint8_t STREAM_HEADER = 19; 00044 static const float V_3_TICKS_PER_REV = 508.8; 00045 static const uint32_t V_3_MAX_ENCODER_TICKS = 65535; 00046 static const float MAX_RADIUS = 2.0; 00047 static const float STRAIGHT_RADIUS = 32.768; 00048 static const float IN_PLACE_RADIUS = 0.001; 00049 static const float PI = 3.14159; 00050 static const float TWO_PI = 6.28318; 00051 static const float EPS = 0.0001; 00052 00053 inline float normalizeAngle(const float& angle) { 00054 float a = angle; 00055 while (a < -PI) a += TWO_PI; 00056 while (a > PI) a -= TWO_PI; 00057 return a; 00058 }; 00059 00060 typedef unsigned long long timestamp_t; 00061 00064 static timestamp_t getTimestamp() { 00065 struct timeval now; 00066 gettimeofday(&now, NULL); 00067 return now.tv_usec + (timestamp_t) now.tv_sec * 1000000; 00068 } 00069 00070 inline bool willFloatOverflow(const float a, const float b) { 00071 return ( (a < 0.0) == (b < 0.0) && std::abs(b) > std::numeric_limits<float>::max() - std::abs(a) ); 00072 } 00073 } // namespace util 00074 } // namespace create 00075 00076 #endif // CREATE_UTIL_H