Merge branch 'Sensor' of https://github.com/AutoVerse-ai/Verse-library …

…into Sensor
AutoVerse-ai · Feb 29, 2024 · 9f333d9 · 9f333d9
2 parents dd2f633 + a15bd7e
commit 9f333d9
Showing 1 changed file with 6 additions and 4 deletions.
diff --git a/verse/map/lane_segment.py b/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