fixing wrap to pi issue in circular lane segment

sense.py

@@ -194,8 +194,6 @@ def TC_simulate(
     trace[1:, 0] = [round(i * time_step, 10) for i in range(num_points)]
     trace[0, 1:] = init
     for i in range(num_points):
-        if i==300:
-            print("stop")
         steering, a = car_action_handler(mode, init, lane_map)
         r = ode(car_dynamics)
         r.set_initial_value(init).set_f_params([steering, a])

verse/map/lane_segment.py

@@ -283,12 +283,14 @@ def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
         wrap = (wrap_to_pi(phi - self.start_phase))
         #if(wrap <= 0):
         phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
-        if(wrap > 0):
-            #print("hi")
-            print(phi) 
         r = np.linalg.norm(delta)
         # longitudinal = (phi - self.start_phase) * self.radius
-        longitudinal = self.direction * (phi - self.start_phase) * self.radius
+        phase_diff = wrap_to_pi(phi-self.start_phase)
+        if self.direction<0 and phase_diff > 0:
+            phase_diff -= np.pi*2 
+        elif self.direction>0 and phase_diff <0:
+            phase_diff += np.pi*2
+        longitudinal = self.direction * phase_diff * self.radius
         lateral = self.direction * (self.radius - r)
         return longitudinal, lateral