Avoid unnecessary calculations

[wenguang1111]

Hello, i use your repo to do some data type analysis and printed value of most variables in code. I realized that the yaw of Pose could be more than 170 radians. This will not make an accuracy problem but create many unnecessary steps in Dubins::generatePath():line 78

for (int i = 0; i < (ang_end - ang_start) / step; i+=1) {
r_x.push_back(center[0] + cos(ang) * radius);
r_y.push_back(center[1] + sin(ang) * radius);
r_yaw.push_back(yaw);
yaw += step;
ang += step;
}

The calculated yaw will be used hier as variable "ang_end" and this makes the loop to be huge.
I hope I describe the problem clearly to make you understand what I am meaning.

For simulation, i used HybridAStar/hybrid_astar.py. you can try to print the value of "abs(hastar_hp->step_size / rad)" from HybridAStar.cpp and check if the value is bigger than 170 for example, to identify the problem.