Implement frenet to cartesian to frenet transformations

f1tenth · Mar 18, 2024 · 0114dc4 · 0114dc4
1 parent a9d10d9
commit 0114dc4
Showing 1 changed file with 13 additions and 3 deletions.
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
@@ -276,12 +276,22 @@ def cartesian_to_frenet(self, x, y, phi):
         phi: yaw angle
 
         returns:
-            s: distance along the raceline
+            s: distance along the centerline
             ey: lateral deviation
             ephi: heading deviation
         """
-        s, ey = self.raceline.spline.calc_arclength(x, y)
-        phi = phi - self.raceline.spline.calc_yaw(s)
+        s, ey = self.centerline.spline.calc_arclength(x, y)
+
+        # Use the normal to calculate the signed lateral deviation
+        normal = self.centerline.spline._calc_normal(s)
+        x_eval = self.centerline.spline.sx.calc_position(s)
+        y_eval = self.centerline.spline.sy.calc_position(s)
+        dx = x - x_eval
+        dy = y - y_eval
+        distance_sign = np.sign(np.dot([dx, dy], normal))
+        ey = ey * distance_sign
+
+        phi = phi - self.centerline.spline.calc_yaw(s)
 
         return s, ey, phi