归一化方向角:
double MoveBase::normalizeAngle(double val, double min, double max)
{
float norm = 0.0;
if(val >=min)
norm = min + fmod((val - min), (max - min));
else
norm = max - fmod((min - val), (max - min));
return norm;
}