angle issue

social_mpc/path.py

@@ -76,7 +76,7 @@ class PathPlanner():
 
         if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
             return []
-        d_angle = np.arctan2(vxl[0], -vyl[0]) - pos[2]
+        d_angle = utils.constraint_angle(np.arctan2(vxl[0], -vyl[0]) - pos[2])
         if dl[0] < self.max_d_orientation:  # if within range, set waypoint = target
             wpt_pos = [self.target[0], self.target[1], self.target[2]]
         elif dl[0] < self.max_d: