localizer/include/common.h

/*
 * @Descripttion: common.h Project
 * @version: Project v0.1
 * @Author: lenotary
 * @Date: 2020/11/23 上午9:28
 * @LastEditor: lenotary
 * @LastEditTime: 2020/11/23 上午9:28
 */

#ifndef SRC_COMMON_H
#define SRC_COMMON_H
#include <cmath>

template<typename T>
T NormalizeAngle(T angle){
    if (angle >  M_PI){
        angle -= M_PI * 2;
    }else if(angle < -M_PI){
        angle += M_PI * 2;
    }
    return angle;
}
template<typename T>
T SetMin(T x, T min){
    x = x < min? min : x;
    return x;
}
template<typename T>
T SetMax(T x, T max){
    x = x > max? max : x;
    return x;
}
template<typename T>
double GetRosTime(T msg){
    return msg.header.stamp.toSec();
}
#endif //SRC_COMMON_H