Update the pi_2_pi function to a use of a modulo instead of two while.

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -18,13 +18,7 @@ class State:
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def update(state, v, delta, dt, L):